#include "convert.h"

#ifdef USE_V4L2

#include "videodev-v4l2.h"

VideoDev_V4L2::VideoDev_V4L2() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	io = IOMODE_MMAP;
	fd = -1;
	inbuffer_idx = 0;
};

VideoDev_V4L2::~VideoDev_V4L2() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if (running > 0) 	CaptureStop();
	if (fd >= 0)		Close ();
}



/*
 * try to send ioctl command as long as EINTR is valid. But abort after 2 seconds.
 */
int VideoDev_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;
};



/*
 * return a list of /dev/video* devices found on the system, and read out its human friendly name
 * output will be a lit of: "V4L2 /dev/videoX [Name]"
 */
int VideoDev_V4L2::GetDeviceList(std::list<std::string> *list) {
	std::string device;
	int devnum;

	if (list == NULL) return 0;

	for (devnum = 0; devnum < 255; devnum++) {
		device = "/dev/video"+std::to_string(devnum);

		if (device.compare (conf_device) != 0) {
			int fd;
			struct v4l2_capability vcap;

			if((fd = open(device.c_str(), O_RDONLY)) == -1){
				continue;
			}

			if(ioctl(fd, VIDIOC_QUERYCAP, &vcap) == -1)
				strncpy ((char*)&vcap.card, "unknown", sizeof(vcap.card));
			close(fd);
			device += " [" + (std::string) ((char*)vcap.card) + "]";
		}
		else {
			device += " [" + (std::string) conf_devicename + "]";
		}

		list->push_back("V4L2 " + device);
	}

	return 1;
}


/*
 * print out the important capabilities, which are reported by the V4L2 device
 */
void VideoDev_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");
}


/*
 * convert the V4L2 Format data into a text humans can read.
 * the pixelformat is coded in 4 chars, maybe we need a general coding table
 * convert still need the v4l2 pixelformats.
 * as soon as we support GPhoto or something else we need our own pixelformat table.
 */
void VideoDev_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);
}



/*
 * Open Device
 * prepare the buffer, InitMMAP and read all controls
 */
int VideoDev_V4L2::Open() {
	int i;
	struct v4l2_capability vcap;
	VideoDevCtrl vctl;
	char txt[32];

	printf ("%s:%d %s Device: '%s'\n", __FILE__, __LINE__, __FUNCTION__, conf_device.c_str());
	if (fd != -1) return VDEV_STATUS_ERROR;

	//
	// open device and get device name and capabilities      | O_NONBLOCK
	if((fd = open(conf_device.c_str(), O_RDWR)) == -1){
		return VDEV_STATUS_ERROR;
	}

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

	printf ("%s:%d %s Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, vcap.capabilities);
	PrintCaps(vcap.capabilities);
	printf ("%s:%d %s Device Capabilities: %u\n", __FILE__, __LINE__, __FUNCTION__, vcap.device_caps);
	PrintCaps(vcap.device_caps);

	if (!(vcap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		printf ("%s:%d %s device has no video capture capabilities\n", __FILE__, __LINE__, __FUNCTION__);
		return VDEV_STATUS_ERROR;
		Close();
	}

	//
	// query controls
	struct v4l2_queryctrl queryctrl;
	uint32_t min;

	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);
        }
	}

	//
	// check for cropping.. if we have it setup default
	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)) {
			printf ("%s:%d %s VIDEOC_S_CROP Errorcode: %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno));
		}
	}

	//
	// prepare resolution and pixelformat
	CLEAR (fmt);
	fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (conf_height != -1 && conf_width != -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_format);
	fmt.fmt.pix.field       = V4L2_FIELD_NONE;
	if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) {
		fprintf (stderr, "%s:%d VIDIOC_S_FMT : %s\n", __FILE__, __LINE__, strerror (errno));
		close (fd);
		fd = -1;
		return VDEV_STATUS_ERROR;
	}

	// 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_format = txt;
	PrintFmt (&fmt);

	// init buffers
	switch (io) {
	case IOMODE_MMAP:
		if (InitMMap() == VDEV_STATUS_ERROR)
			Close();
		break;
	case IOMODE_READ:
	default:
		break;
	}

	return VDEV_STATUS_OK;
};


/*
 * prepare memory mapped buffers
 */
int VideoDev_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_device.c_str());
			return VDEV_STATUS_ERROR;
		} else {
			printf ("%s:%d %s Error %s\n", __FILE__, __LINE__, __FUNCTION__, strerror(errno));
			return VDEV_STATUS_ERROR;
		}
    }

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

    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 VDEV_STATUS_OK;
}


/*
 * Close Device
 * Free videobuffer
 */
int VideoDev_V4L2::Close() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

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

	conf_device = "";
	conf_devicename = "";

	return VDEV_STATUS_OK;
};



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

    switch (io) {
    case IOMODE_READ:
    	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 VDEV_STATUS_ERROR;
	}


 	return VDEV_STATUS_OK;
};

/*****************************************************************************************************
 * VideoGrabbing
 */


/*
 * send the start capture signal to the cam
 */
int VideoDev_V4L2::CaptureStart() {
	enum v4l2_buf_type type;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	switch (io) {
	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)) {
			   printf ( "%s:%d error on VIDIOC_QBUF %s\n", __FILE__, __LINE__, strerror(errno));
			   return VDEV_STATUS_ERROR;
			}
		}

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

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

	pixelformat = fmt.fmt.pix.pixelformat;

	return VDEV_STATUS_OK;
};


int VideoDev_V4L2::CaptureStop() {
    enum v4l2_buf_type type;
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

    switch (io) {
    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 VDEV_STATUS_ERROR;
            }
            break;
	default:
		printf ("%s:%d %s wrong IOMODE?\n", __FILE__, __LINE__, __FUNCTION__);
		return VDEV_STATUS_ERROR;
	}
	return VDEV_STATUS_OK;
};


/*
 * If something goes wrong return an error code.
 * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read.
 * To reduce the time of malloc/free reuse the destination buffer.
 */
int VideoDev_V4L2::Grab(VideoFrameRaw *vf) {
	struct v4l2_buffer buf;
	int len;

	if (vf == NULL) return VDEV_STATUS_ERROR;

	switch (io) {
	case IOMODE_READ:
		if ((len = read (fd, inbuffer[0].data, fmt.fmt.pix.sizeimage)) == -1) {
			switch (errno) {
				case EAGAIN:
					return VDEV_STATUS_AGAIN;
				case EIO:
				default:
					printf ("v4l2_grab IOM_READ: %s  dest:%p size:%d\n", strerror (errno), vf, fmt.fmt.pix.sizeimage);
					return VDEV_STATUS_ERROR;
			}
		}
		else {
			LockMutex();

			UnLockMutex();
		}
		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 VDEV_STATUS_AGAIN;

				case EIO:
					printf ( "%s:%d error on VIDIOC_DQBUF EIO %s\n", __FILE__, __LINE__, strerror(errno));
					return VDEV_STATUS_ERROR;

				default:
		            printf ( "%s:%d error on VIDIOC_DQBUF %s\n", __FILE__, __LINE__, strerror(errno));
		            return VDEV_STATUS_ERROR;
			}
		}

		if (buf.index >= 0 && buf.index < VDEV_INBUFFERS) {
			LockMutex();
			vf->CopyFrom(fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height, buf.bytesused, inbuffer[buf.index].data);
			UnLockMutex();
		}

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

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

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

	return VDEV_STATUS_OK;
}


int VideoDev_V4L2::GetDeviceFormats(string device, std::list<string> *formats) {
	int i, fd_;
	struct v4l2_format format = {0};
	std::string result = "";

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	LockMutex();

	if (fd == -1){
		//
		// open device and get device name and capabilities      | O_NONBLOCK
		if((fd_ = open(device.c_str(), O_RDWR)) == -1) {
			printf ("%s could not open device %s: %s\n", __FUNCTION__, device.c_str(), strerror(errno));
			return VDEV_STATUS_ERROR;
		}
	}
	else fd_ = fd;

	for(i = 0; convert_pixelformats[i] != 0; i++){
	    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    format.fmt.pix.width = -1;
	    format.fmt.pix.height = -1;
	    format.fmt.pix.pixelformat = convert_pixelformats[i];
	    if(xioctl(fd_, VIDIOC_S_FMT, &format) != -1) {
	    	if (format.fmt.pix.pixelformat == convert_pixelformats[i]) {
	    		formats->push_back(convert_from_pixelformat(format.fmt.pix.pixelformat));
	    	}
	    }
    	else {
    		printf ("error: %s\n", strerror(errno));
    	}
	}

	if (fd == -1) {
		close (fd_);
	}

	UnLockMutex();
	return VDEV_STATUS_OK;
}




/*****************************************************************************************************
 * Controls
 */

/*
 * set video control identified by id
 */
int VideoDev_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 VDEV_STATUS_ERROR;
	}

	return VDEV_STATUS_OK;
};


/*
 * get video control identified by id
 */
int VideoDev_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 VDEV_STATUS_ERROR;
	}
	*value = ctrl.value;

	return VDEV_STATUS_OK;
};


#endif