#include"node.h"

#include<sys/ioctl.h>
#include<sys/time.h>
#include<sys/mman.h>
#include<stdlib.h>
#include<stdio.h>
#include<fcntl.h>
#include<string.h>
#include<time.h>
#include<linux/videodev2.h>
#include<libv4l2.h>
#include<errno.h>
#include"img.h"
#include<assert.h>
#include<smmintrin.h>

static int camId = -1;

static struct Buf {
	size_t length;
	uint8_t *ptr;
} bufs[2];

struct v4l2_format fmt;

static void xioctl(int fh, int request, void *arg) {
	int r;

	do {
		r = v4l2_ioctl(fh, request, arg);
	} while(r == -1 && ((errno == EINTR) || (errno == EAGAIN)));

	if(r == -1) {
		fprintf(stderr, "error %d, %s\\n", errno, strerror(errno));
		exit(EXIT_FAILURE);
	}
}

static int camera_perform(CHiPubNode *pubn) {
	pubn->sources[0].type = CUTIHI_VAL_SAMPLE;
	
	fd_set fds;
	FD_ZERO(&fds);
	FD_SET(camId, &fds);
	int r = select(camId + 1, &fds, NULL, NULL, &(struct timeval) {.tv_sec = 0, .tv_usec = 100});
	if(r == -1) {
		return 1;
	}
	
	struct v4l2_buffer buf;
	memset(&buf, 0, sizeof(buf));
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	xioctl(camId, VIDIOC_DQBUF, &buf);
	
	CHiImage *ret;
	if(pubn->sources[0].data.sample) {
		ret = pubn->sources[0].data.sample;
	} else {
		ret = CHi_Image_New(2, 4, 8 * ((fmt.fmt.pix.width + 15) & ~15), fmt.fmt.pix.width, fmt.fmt.pix.height, NULL);
		pubn->sources[0].data.sample = ret;
	}
	
	/* 24-to-32 TODO: make faster by processing 48-to-64 bytes in one iteration (like bgra32torgb24 does) */
	for(size_t y = 0; y < ret->height; y++) {
		for(size_t x = 0; x < ret->width; x += 2) {
			__m128i asdf = _mm_loadu_si128((__m128i*) &bufs[buf.index].ptr[(y * ret->width + x) * 3]);
			asdf = _mm_shuffle_epi8(asdf, _mm_set_epi8(-128, -128, 5, -128, 4, -128, 3, -128, -128, -128, 2, -128, 1, -128, 0, -128));
			asdf = _mm_or_si128(asdf, _mm_set_epi32(0xFFFF0000, 0, 0xFFFF0000, 0));
			_mm_stream_si128((__m128i*) ((uintptr_t) ret->data16 + y * ret->stride + x * 8), asdf);
		}
	}
	
	xioctl(camId, VIDIOC_QBUF, &buf);
	
	return 1;
}

CUTIVIS CHiPubNode *CHi_Camera() {
	if(camId == -1) {
		camId = v4l2_open("/dev/video0", O_RDWR | O_NONBLOCK, 0);
		
		memset(&fmt, 0, sizeof(fmt));
		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.fmt.pix.width = 640;
		fmt.fmt.pix.height = 480;
		fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24;
		fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
		xioctl(camId, VIDIOC_S_FMT, &fmt);
		assert(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24);
		
		struct v4l2_requestbuffers req;
		memset(&req, 0, sizeof(req));
		req.count = 2;
		req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		req.memory = V4L2_MEMORY_MMAP;
		xioctl(camId, VIDIOC_REQBUFS, &req);
		
		for(int i = 0; i < 2; i++) {
			struct v4l2_buffer buf;
			memset(&buf, 0, sizeof(buf));
			
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_MMAP;
			buf.index = i;
			xioctl(camId, VIDIOC_QUERYBUF, &buf);
			
			bufs[i].length = buf.length;
			bufs[i].ptr = v4l2_mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camId, buf.m.offset);
			
			assert(MAP_FAILED != bufs[i].ptr);
		}
		
		for(int i = 0; i < 2; i++) {
			struct v4l2_buffer buf;
			memset(&buf, 0, sizeof(buf));
			
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_MMAP;
			buf.index = i;
			xioctl(camId, VIDIOC_QBUF, &buf);
		}
		
		xioctl(camId, VIDIOC_STREAMON, &(enum v4l2_buf_type) {V4L2_BUF_TYPE_VIDEO_CAPTURE});
	}
	
	CHiPubNode *pubn = calloc(1, sizeof(*pubn));
	pubn->type = CUTIHI_T('CWeb','Cam ');
	pubn->clean = 0;
	pubn->Start = pubn->Stop = NULL;
	pubn->Perform = camera_perform;
	pubn->sinks = calloc(sizeof(*pubn->sinks), pubn->sinkCount = 0);
	pubn->sources = calloc(sizeof(*pubn->sources), pubn->sourceCount = 1);
	pubn->ng = NULL;
	return pubn;
}