#include "miniwebcam.h"
#include "video.h"
#include "convert.h"
#include <stdio.h>
#include <stdlib.h>



VideoDevice::VideoDevice() {
};


VideoDevice::~VideoDevice() {

};


VideoDevice_V4L2::VideoDevice_V4L2() {
    conf_height = 0;
    conf_width = 0;
    conf_videodev = "";
	conf_iomode = 0;
    fd = -1;
};


VideoDevice_V4L2::~VideoDevice_V4L2() {
    if (fd != 0) Stop();
};



int VideoDevice_V4L2::SetDevice(std::string newdevice, int nwidth, int nheight) {
    int isrunning = 0;

    if (fd != -1) { 
        Stop();
        isrunning = 1;
    }

    conf_videodev = newdevice;
    conf_width = nwidth;
    conf_height = nheight;

    if (isrunning) {
        if (Start() == 0) return 0;
    }

    return 1;
};


int VideoDevice_V4L2::SetIOMode(int newiomode) {
    int isrunning = 0;

    if (fd != -1) { 
        Stop();
        isrunning = 1;
    }

    conf_iomode = newiomode;

    if (isrunning) {
        if (Start() == 0) return 0;
    }

    return 1;
};


int VideoDevice_V4L2::Open() {
	int i;
	struct v4l2_capability vcap;
	VideoDevCtrl vctl;
	char txt[32];

	debug ("Device: '%s'", conf_videodev.c_str());
	if (fd != -1) return 0;

	//
	// open device and get device name and capabilities      | O_NONBLOCK
	if((fd = open(conf_videodev.c_str(), O_RDWR)) == -1){
        debug ("could not open device error: %s", strerror(errno));
		return 0;
	}

	if(ioctl(fd, VIDIOC_QUERYCAP, &vcap) == -1)
		strncpy ((char*)&vcap.card, "unknown", sizeof(vcap.card));
	debug ("VideoDevice_V4L2::Open Devicefile:%s  Card:%s  fd:%d", conf_videodev.c_str(), vcap.card, fd);

	debug ("Capabilities: %u", vcap.capabilities);
	PrintCaps(vcap.capabilities);
	debug ("Device Capabilities: %u", vcap.device_caps);
	PrintCaps(vcap.device_caps);

	if (!(vcap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		debug ("Error: device has no video capture capabilities");
		return 0;
		Close();
	}

	//
	// query controls
	struct v4l2_queryctrl queryctrl;
	uint32_t min;
	debug ("Get Controls");

	vidctrls.clear();
	memset (&queryctrl, 0, sizeof (queryctrl));
	for (i = V4L2_CID_BASE; i < V4L2_CID_DETECT_CLASS_BASE+0x1000; i++) {
		queryctrl.id  = i;
        if (0 == ioctl (fd, VIDIOC_QUERYCTRL, &queryctrl)) {
			vctl.name = (char*)queryctrl.name;
			vctl.id = queryctrl.id;
			vctl.min = queryctrl.minimum;
			vctl.max = queryctrl.maximum;
			GetDevCtrl(queryctrl.id, &vctl.value);
			vidctrls.push_back(vctl);
        }
	}
	debug ("got %d controls.", vidctrls.size());

	//
	// check for cropping.. if we have it setup default
	debug ("setup cropping");
	CLEAR (cropcap);
	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

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

		if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
			debug ("VIDEOC_S_CROP Errorcode: %s", strerror(errno));
		}
	}

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

	// fixme: hardcoded video format?????
	fmt.fmt.pix.pixelformat = convert_to_pixelformat(conf_videofmt);
	fmt.fmt.pix.field       = V4L2_FIELD_NONE;
	if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) {
		debug ("VIDIOC_S_FMT : %s", strerror (errno));
		close (fd);
		fd = -1;
		return 0;
	}

	// Note VIDIOC_S_FMT may change width and height.
	// Buggy driver paranoia.     - as written in the v4l2 api documentation
	min = fmt.fmt.pix.width * 2;
	if (fmt.fmt.pix.bytesperline < min)
		fmt.fmt.pix.bytesperline = min;
	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
	if (fmt.fmt.pix.sizeimage < min)
		fmt.fmt.pix.sizeimage = min;
	conf_width = fmt.fmt.pix.width;
	conf_height = fmt.fmt.pix.height;
	snprintf (txt, 32, "%c%c%c%c", ((char*)&fmt.fmt.pix.pixelformat)[0],
			((char*)&fmt.fmt.pix.pixelformat)[1],
			((char*)&fmt.fmt.pix.pixelformat)[2],
			((char*)&fmt.fmt.pix.pixelformat)[3]);
	conf_videofmt = txt;
	PrintFmt (&fmt);

	// init buffers
	switch (conf_iomode) {
	case IOMODE_MMAP:
		if (InitMMap() == 0) {
			debug ("could not setup MMap (memory mapping)");
			Close();
		}
		break;
	case IOMODE_READ:
		for (i = 0; i < VDEV_INBUFFERS; i++) {
			inbuffer[i].size = fmt.fmt.pix.sizeimage;
			inbuffer[i].data = (unsigned char*) malloc (fmt.fmt.pix.sizeimage);
		}

	default:
		break;
	}

    return 1;
};


int VideoDevice_V4L2::InitMMap() {
    struct v4l2_requestbuffers bufreq;
    struct v4l2_buffer bufinfo;
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	int i;

    CLEAR(bufreq);
    CLEAR(bufinfo);

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

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

    if (bufreq.count < 1) {
        printf ( "Insufficient buffer memory on %s\n", conf_videodev.c_str());
            return 0;
    }

    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(fd, VIDIOC_QUERYBUF, &bufinfo) < 0){
			perror("VIDIOC_QUERYBUF");
			exit(1);
		}

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


int VideoDevice_V4L2::Close() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	if (fd >= 0) {
		UnInit();
		close (fd);
		fd = -1;
	}

	return 1;
};


int VideoDevice_V4L2::UnInit() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

    switch (conf_iomode) {
    case IOMODE_READ:
		for (inbuffer_idx = 0; inbuffer_idx < VDEV_INBUFFERS; inbuffer_idx++) {
			if (inbuffer[inbuffer_idx].data) {
				free (inbuffer[inbuffer_idx].data);
				inbuffer[inbuffer_idx].data = NULL;
				inbuffer[inbuffer_idx].size = 0;
			}
		}

    	break;

    case IOMODE_MMAP:
    	for (inbuffer_idx = 0; inbuffer_idx < VDEV_INBUFFERS; inbuffer_idx++)
    		if (inbuffer[inbuffer_idx].data) {
				if (-1 == munmap(inbuffer[inbuffer_idx].data, inbuffer[inbuffer_idx].size)){
					fprintf(stderr, "Fatal Error @ %s:%d munmap Error:%s\n", __FILE__, __LINE__, strerror(errno));
					exit(1);
				}
				inbuffer[inbuffer_idx].data = NULL;
				inbuffer[inbuffer_idx].size = 0;
    		}
    	break;

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


 	return 1;
};


int VideoDevice_V4L2::Start() {
	enum v4l2_buf_type type;

	debug ("");

    if (Open() == 0) {
        debug ("VideoDevice_V4L2::Start Open Device Failed.");
        return 0;
    }

	switch (conf_iomode) {
	case IOMODE_READ:
		/* Nothing to do. */
		break;

	case IOMODE_MMAP:
		struct v4l2_buffer buf;

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

			if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
			   debug ("error on VIDIOC_QBUF %s", strerror(errno));
			   return 0;
			}
		}

		inbuffer_idx = 0;
		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		if (-1 == xioctl(fd, VIDIOC_STREAMON, &type)) {
			debug ("VIDIOC_STREAMON %s", strerror(errno));
			return 0;
		}
		break;

	default:
		debug ("wrong IOMODE?");
		return 0;
	}

    ConvertStart(&cdata, fmt.fmt.pix.pixelformat);

    return 1;
};


int VideoDevice_V4L2::Stop() {
   enum v4l2_buf_type type;
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
    ConvertStop(&cdata, fmt.fmt.pix.pixelformat);

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


int VideoDevice_V4L2::GetFrame(VideoFrame *destframe) {
	struct v4l2_buffer buf;
	int len;

	if (destframe == NULL) return 0;

	switch (conf_iomode) {
	case IOMODE_READ:
		if ((len = read (fd, inbuffer[0].data, fmt.fmt.pix.sizeimage)) == -1) {
			switch (errno) {
				case EAGAIN:
					return -1;
				case EIO:
				default:
					debug ("v4l2_grab IOM_READ: %s  dest:%p size:%d", strerror (errno), destframe, fmt.fmt.pix.sizeimage);
					return 0;
			}
		}
		else {
            Convert(&cdata, destframe, inbuffer[0].data, len, 
                    fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height);
		}
		break;

	case IOMODE_MMAP:
		CLEAR(buf);

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

		if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
			switch (errno) {
				case EAGAIN:
					return -1;

				case EIO:
					debug ("error on VIDIOC_DQBUF EIO %s", strerror(errno));
					return 0;

				default:
		            debug ("error on VIDIOC_DQBUF %s", strerror(errno));
		            return 0;
			}
		}

		if (buf.index >= 0 && buf.index < VDEV_INBUFFERS) {
            Convert(&cdata, destframe, inbuffer[buf.index].data, buf.bytesused, 
                    fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height);
		}

		if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
           debug ("error on VIDIOC_QBUF %s", strerror(errno));
           return 0;		}

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

	default:
		debug ("wrong IOMODE?");
		return 0;
	}

	return 1;
};


int VideoDevice_V4L2::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;
};


int VideoDevice_V4L2::SetDevCtrl(unsigned int id, int value) {
	struct v4l2_control ctrl;

	CLEAR(ctrl);
	ctrl.id = id;
	ctrl.value = value;
	if (-1 == xioctl (fd, VIDIOC_S_CTRL, &ctrl)) {
		return 0;
	}

	return 1;
};


int VideoDevice_V4L2::GetDevCtrl(unsigned int id, int *value) {
	struct v4l2_control ctrl;

	CLEAR(ctrl);
	ctrl.id = id;
	if (-1 == xioctl (fd, VIDIOC_G_CTRL, &ctrl)) {
		return 0;
	}
	*value = ctrl.value;

	return 1;
};


void VideoDevice_V4L2::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");
};


void VideoDevice_V4L2::PrintFmt(struct v4l2_format *f) {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	printf ("  type : %u\n", f->type);
	printf ("  fmt.pix.width  : %d\n", f->fmt.pix.width);
	printf ("  fmt.pix.height  : %d\n", f->fmt.pix.height);
	printf ("  fmt.fmt.pix.pixelformat : %c%c%c%c\n", ((char*)&f->fmt.pix.pixelformat)[0],
			((char*)&f->fmt.pix.pixelformat)[1],
			((char*)&f->fmt.pix.pixelformat)[2],
			((char*)&f->fmt.pix.pixelformat)[3]);
	printf ("  fmt.pix.field : %d\n", f->fmt.pix.field);
	printf ("  fmt.pix.bytesperline : %d\n", f->fmt.pix.bytesperline);
	printf ("  fmt.pix.sizeimage : %d\n", f->fmt.pix.sizeimage);
};