#include "vidoiltank.h"
#define CLEAR(_var_) memset (&_var_, 0x0 , sizeof(_var_));

int xioctl(int fd, int request, void *arg);
void printcaps(uint32_t caps);
int vid_initmmap(Video* video);

/*
 * helper functions
 */

int xioctl(int fd, int request, void *arg) {
	int r;
	int errnoioctl;
	struct timeval to1;
	struct timeval to2;
	float to;

	gettimeofday(&to1, NULL);
	do {
		r = ioctl(fd, request, arg);
		errnoioctl = errno;
		gettimeofday(&to2, NULL);
		to = (float)(to2.tv_sec - to1.tv_sec) + ((to2.tv_usec - to1.tv_usec) / 1000000.0);
	} while (r == -1 && errnoioctl == EINTR && to < 2.0);

	return r;
};


void printcaps(uint32_t caps) {
	printf ("%s:%d %s Caps: %x\n", __FILE__, __LINE__, __FUNCTION__, caps);

	if (caps & V4L2_CAP_VIDEO_CAPTURE) printf ("    V4L2_CAP_VIDEO_CAPTURE\n");
	if (caps & V4L2_CAP_EXT_PIX_FORMAT) printf ("    V4L2_CAP_EXT_PIX_FORMAT\n");
#ifdef V4L2_CAP_META_CAPTURE
	if (caps & V4L2_CAP_META_CAPTURE) printf ("    V4L2_CAP_META_CAPTURE\n");
#endif
	if (caps & V4L2_CAP_STREAMING) printf ("    V4L2_CAP_STREAMING\n");
	if (caps & V4L2_CAP_DEVICE_CAPS) printf ("    V4L2_CAP_DEVICE_CAPS\n");
	if (caps & V4L2_CAP_TUNER) printf ("    V4L2_CAP_TUNER\n");
	if (caps & V4L2_CAP_MODULATOR) printf ("    V4L2_CAP_MODULATOR\n");
	if (caps & V4L2_CAP_READWRITE) printf ("    V4L2_CAP_READWRITE\n");
	if (caps & V4L2_CAP_ASYNCIO) printf ("    V4L2_CAP_ASYNCIO\n");
}


/*
 *
 */

Video *vid_open(char *name) {
	Video* video = malloc (sizeof(Video));
	strncpy (video->devname, name, LEN_FILENAME-1);

	//
	// open device and get device name and capabilities      | O_NONBLOCK
	if((video->fd = open(video->devname, O_RDWR)) == -1) {
		free (video);
		return NULL;
	}

	if(ioctl(video->fd, VIDIOC_QUERYCAP, &video->vcap) == -1)
		strncpy ((char*)&video->vcap.card, "unknown", sizeof(video->vcap.card));

//	printf ("%s:%d %s Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, video->vcap.capabilities);
//	printcaps(video->vcap.capabilities);
//	printf ("%s:%d %s Device Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, video->vcap.device_caps);
//	printcaps(video->vcap.device_caps);

	if (!(video->vcap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		printf ("%s:%d %s device has no video capture capabilities\n", __FILE__, __LINE__, __FUNCTION__);
		close(video->fd);
		free (video);
		return NULL;
	}

	//
	// check for cropping.. if we have it setup default
	CLEAR (video->cropcap);
	video->cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if (0 == xioctl (video->fd, VIDIOC_CROPCAP, &video->cropcap)) {
		video->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		video->crop.c = video->cropcap.defrect; // reset to default

		if (-1 == xioctl (video->fd, VIDIOC_S_CROP, &video->crop)) {
			// errors here can be ignored
			printf ("%s:%d %s VIDEOC_S_CROP Errorcode: %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno));
		}
	}

	vid_getctrls(video);

	return video;
}



void vid_close(Video *video) {
	convertstop(&video->jpgcinfo, video->fmt.fmt.pix.pixelformat);
	close (video->fd);
}


int vid_start(Video* video, int iomode, int width, int height) {
	int min;
	char txt[16] = "";

	//
	// prepare resolution and pixelformat
	CLEAR (video->fmt);
	video->fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (height != -1 && width != -1) {	// resolution
		video->fmt.fmt.pix.width       = width;
		video->fmt.fmt.pix.height      = height;
	}
	else {
		video->fmt.fmt.pix.width       = 1920;
		video->fmt.fmt.pix.height      = 1080;
	}

	video->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	video->fmt.fmt.pix.field       = V4L2_FIELD_NONE;
	if (-1 == xioctl (video->fd, VIDIOC_S_FMT, &video->fmt)) {
		fprintf (stderr, "%s:%d VIDIOC_S_FMT : %s\n", __FILE__, __LINE__, strerror (errno));
		return -1;
	}

	// Note VIDIOC_S_FMT may change width and height.
	// Buggy driver paranoia.     - as written in the v4l2 api documentation
	min = video->fmt.fmt.pix.width * 2;
	if (video->fmt.fmt.pix.bytesperline < min)
		video->fmt.fmt.pix.bytesperline = min;
	min = video->fmt.fmt.pix.bytesperline * video->fmt.fmt.pix.height;
	if (video->fmt.fmt.pix.sizeimage < min)
		video->fmt.fmt.pix.sizeimage = min;
	video->width = video->fmt.fmt.pix.width;
	video->height = video->fmt.fmt.pix.height;
	snprintf (txt, 32, "%c%c%c%c", ((char*)&video->fmt.fmt.pix.pixelformat)[0],
			((char*)&video->fmt.fmt.pix.pixelformat)[1],
			((char*)&video->fmt.fmt.pix.pixelformat)[2],
			((char*)&video->fmt.fmt.pix.pixelformat)[3]);
	strncpy (video->format, txt, 16);
//	printf ("%s:%d Video Format: %s\n", __FILE__, __LINE__, video->format);

	// init buffers
	video->iomode = iomode;
	switch (video->iomode) {
	case IOM_MMAP:
		if (vid_initmmap(video) == -1)
			return -1;
		break;
	case IOM_READ:
	default:
		break;
	}

	// convertstart // for mjpg only
	convertstart (&video->jpgcinfo, video->fmt.fmt.pix.pixelformat);

	return 0;
};


int vid_uninitmmap(Video *video) {
//	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
   	for (int inbuffer_idx = 0; inbuffer_idx < VDEV_INBUFFERS; inbuffer_idx++) {
   		if (video->inbuffer[inbuffer_idx].data) {
			if (-1 == munmap(video->inbuffer[inbuffer_idx].data, video->inbuffer[inbuffer_idx].size)){
				fprintf(stderr, "Fatal Error @ %s:%d munmap Error:%s\n", __FILE__, __LINE__, strerror(errno));
				exit(1);
			}
			video->inbuffer[inbuffer_idx].data = NULL;
			video->inbuffer[inbuffer_idx].size = 0;
   		}
   	}
   	return 0;
}


int vid_initmmap(Video *video) {
    struct v4l2_requestbuffers bufreq;
    struct v4l2_buffer bufinfo;
	struct v4l2_buffer buf;
	int i, type;

    CLEAR(bufreq);
    CLEAR(bufinfo);

    bufreq.count = VDEV_INBUFFERS;
    bufreq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    bufreq.memory = V4L2_MEMORY_MMAP;

    if (-1 == xioctl(video->fd, VIDIOC_REQBUFS, &bufreq)) {
		if (EINVAL == errno) {
			printf("device does not support memory mapping\n");
			return -1;
		} else {
			printf ("%s:%d %s Error %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno));
			return -1;
		}
    }

    if (bufreq.count < 1) {
        printf ( "Insufficient buffer memory\n");
        return -1;
    }

    for (i = 0; i < VDEV_INBUFFERS; i++) {
		CLEAR(bufinfo);
		bufinfo.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		bufinfo.memory = V4L2_MEMORY_MMAP;
		bufinfo.index = i;

		if(ioctl(video->fd, VIDIOC_QUERYBUF, &bufinfo) < 0){
			perror("VIDIOC_QUERYBUF");
			exit(1);
		}

		video->inbuffer[i].size = bufinfo.length;
		video->inbuffer[i].data = (unsigned char*)mmap(NULL, bufinfo.length, PROT_READ | PROT_WRITE,
				MAP_SHARED, video->fd, bufinfo.m.offset);
		if (video->inbuffer[i].data == MAP_FAILED) {
			printf ( "%s:%d error on mmap %s\n", __FILE__, __LINE__, strerror(errno));
			exit (1);
		}
    }

	for (video->inbuffer_idx = 0; video->inbuffer_idx < VDEV_INBUFFERS; video->inbuffer_idx++) {
		CLEAR(buf);
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = video->inbuffer_idx;

		if (-1 == xioctl(video->fd, VIDIOC_QBUF, &buf)) {
		   printf ( "%s:%d error on VIDIOC_QBUF %s\n", __FILE__, __LINE__, strerror(errno));
		   return -1;
		}
	}

	video->inbuffer_idx = 0;
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == xioctl(video->fd, VIDIOC_STREAMON, &type)) {
		printf ( "%s:%d VIDIOC_STREAMON %s\n", __FILE__, __LINE__, strerror(errno));
		return -1;
	}

	return 0;
};


ImageRaw *vid_grabimage(Video* video, ImageRaw *img) {
	struct v4l2_buffer buf;
	int len;
	ImageRaw *in = img;

	if (in == NULL) in = image_alloc (video->width, video->height);
	else if (img->w != video->width || img->h != video->height) {
		image_resize(in, video->width, video->height);
	}

	switch (video->iomode) {
	case IOM_READ:
		if ((len = read (video->fd, video->inbuffer[0].data, video->fmt.fmt.pix.sizeimage)) == -1) {
			return NULL;
		}
		else {
			convert(&video->jpgcinfo, in, video->inbuffer[0].data, len, video->fmt.fmt.pix.pixelformat, video->fmt.fmt.pix.width, video->fmt.fmt.pix.height);
		}
		break;

	case IOM_MMAP:
		CLEAR(buf);

		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;

		if (-1 == xioctl(video->fd, VIDIOC_DQBUF, &buf)) {
			return NULL;
		}

		if (buf.index >= 0 && buf.index < VDEV_INBUFFERS) {
			convert(&video->jpgcinfo, in, video->inbuffer[buf.index].data, buf.bytesused, video->fmt.fmt.pix.pixelformat, video->fmt.fmt.pix.width, video->fmt.fmt.pix.height);		}

		if (-1 == xioctl(video->fd, VIDIOC_QBUF, &buf)) {
           printf ( "%s:%d error on VIDIOC_QBUF %s\n", __FILE__, __LINE__, strerror(errno));
           return NULL;
		}

		if (++(video->inbuffer_idx) >= VDEV_INBUFFERS) video->inbuffer_idx = 0;
		break;

	default:
		printf ("%s:%d %s wrong IOMODE?\n", __FILE__, __LINE__, __FUNCTION__);
		errno = EINVAL;
		return NULL;
	}

	return in;
};


int vid_stop(Video* video) {
    enum v4l2_buf_type type;
//	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	convertstop(&video->jpgcinfo, video->fmt.fmt.pix.pixelformat);

    switch (video->iomode) {
    case IOM_READ:
            /* Nothing to do. */
            break;
    case IOM_MMAP:
            type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            if (-1 == xioctl(video->fd, VIDIOC_STREAMOFF, &type)) {
    	    	fprintf(stderr, "%s:%d VIDIOC_STREAMOFF Error:%s\n", __FILE__, __LINE__, strerror(errno));
    			return -1;
            }
            break;
	default:
		printf ("%s:%d %s wrong IOMODE?\n", __FILE__, __LINE__, __FUNCTION__);
		return -1;
	}
	return 0;
};


void vid_getctrls (Video* video) {
	int i, j;
	struct v4l2_queryctrl queryctrl;
	memset (&queryctrl, 0, sizeof (queryctrl));
	for (j = 0, i = V4L2_CID_BASE; i < V4L2_CID_DETECT_CLASS_BASE+0x1000; i++) {
		queryctrl.id  = i;
        if (0 == ioctl (video->fd, VIDIOC_QUERYCTRL, &queryctrl)) {
        	if (j == 0) video->ctrl_bright = queryctrl.id;
        	else if (j == 1) video->ctrl_contrast = queryctrl.id;
        	j++;
        }
	}
};


void vid_setctrls (Video* video, int bright, int contrast) {
	struct v4l2_control ctrl;

	CLEAR(ctrl);
	ctrl.id = video->ctrl_bright;
	ctrl.value = bright;
	xioctl (video->fd, VIDIOC_S_CTRL, &ctrl);

	CLEAR(ctrl);
	ctrl.id = video->ctrl_contrast;
	ctrl.value = contrast;
	xioctl (video->fd, VIDIOC_S_CTRL, &ctrl);
};