/***************************************************************************************
 *
 * videodev.cc is part of SimpleSkyCam.
 *
 *****************************************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <errno.h>
#include <getopt.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <jpeglib.h>

#include <list>
#include <string>
#include <setjmp.h>

#include "gui.h"
#include "video.h"

#define CLEAR(x) memset (&(x), 0, sizeof (x))


//
// jpeg: replacement for error_exit
//
METHODDEF(void) jpg_error_exit (j_common_ptr cinfo) {
  jpg_error_ptr myerr = (jpg_error_ptr) cinfo->err;
  (*cinfo->err->output_message) (cinfo);
  longjmp(myerr->setjmp_buffer, 1);
}



//
// clamp and convert2rgb is build on the sample of the v4l2 api documentation
//
inline unsigned char clamp (double x) {
	int r = (int)x;

	if (r < 0)			return 0;
	else if (r > 255)	return 255;
	else				return r;
};


inline void convert2rgb (unsigned char Y1, unsigned char Cb, unsigned char Cr,
						 unsigned char *ER, unsigned char *EB, unsigned char *EG) {
	register int y1, pb, pr;

	y1 = Y1 - 16;
	pb = Cb - 128;
	pr = Cr - 128;

	*ER = clamp (y1              + 1.402 * pr);
	*EB = clamp (y1 - 0.344 * pb - 0.714 * pr);
	*EG = clamp (y1 + 1.772 * pb);
};


//
// C / C++ Wrapper for the thread function
//
gpointer _VideoDevThread (gpointer data) {
	videodev.Thread ();
	return NULL;
};



VideoDev::VideoDev() {
	int i;

	for (i = 0; i < VDEV_INBUFFERS; i++) {
		inbuffer[i].size = 0;
		inbuffer[i].data = NULL;
	}
	vf.data = NULL;
	vf.h = 0;
	vf.w = 0;
	vf.size = 0;
	running = 0;
	callback = NULL;
	thread = NULL;
	fd = -1;
	io = IOMODE_MMAP;
	inbuffer_idx = 0;
	CLEAR(cropcap);
	CLEAR(crop);
	CLEAR(fmt);
	g_mutex_init (&mutex);
	Close(); // will reset almost everything
};


VideoDev::~VideoDev() {
	Stop();
}


//
// return list of devices in form of /dev/videoYY [Name]
//
int VideoDev::GetDeviceList(std::list<std::string> *list) {
	std::string device;
	int devnum;

	if (list == NULL) return 0;

	list->clear();
	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(device);
	}

	return 1;
}


//
// return a list of strings for controls
//
int VideoDev::GetCtrlList(std::list<std::string> *list) {
	std::list<VideoDevCtrl>::iterator iter;
	if (list == NULL) return VDEV_STATUS_ERROR;

	list->clear();
	LockMutex();

	for (iter = vidctrls.begin(); iter != vidctrls.end(); iter++) {
		list->push_back ((*iter).name);
	}

	UnLockMutex();

	return VDEV_STATUS_OK;
};


//
// return the values for an control, on error VDEV_STATUS_UNKNOWN
//
int VideoDev::GetCtrlMinMaxValue(std::string name, int *min, int *max, int *value) {
	std::list<VideoDevCtrl>::iterator iter;
	LockMutex();
	for (iter = vidctrls.begin(); iter != vidctrls.end(); iter++)
		if (iter->name.compare(name) == 0) {
			GetDevCtrl(iter->id, &(iter->value));
			if (value != NULL)	*value = iter->value;
			if (min != NULL)	*min = iter->min;
			if (max != NULL)	*max = iter->max;
			break;
		}
	UnLockMutex();
	if (iter == vidctrls.end()) return VDEV_STATUS_ERROR;
	return VDEV_STATUS_OK;
};


//
// set the value for an control, on error VDEV_STATUS_UNKNOWN
//
int VideoDev::SetCtrlValue(std::string name, int value) {
	std::list<VideoDevCtrl>::iterator iter;
	LockMutex();
	for (iter = vidctrls.begin(); iter != vidctrls.end(); iter++)
		if (iter->name.compare(name) == 0) {
			SetDevCtrl(iter->id, value);
			break;
		}
	UnLockMutex();
	if (iter == vidctrls.end()) return VDEV_STATUS_ERROR;
	return VDEV_STATUS_OK;
};



//
// start the video, start capturing, start thread
// after return of this function we can call the Ctrl thread
//
int VideoDev::Start(std::string dev, gboolean (*callback_func)(gpointer data)) {
	if (running != 0 || thread != NULL) return VDEV_STATUS_ERROR;

	running = 1;
	conf_device = dev;
	callback = callback_func;

	//
	// open and init device buffers device
	if (OpenInit() != VDEV_STATUS_OK) {
		Close();
		return VDEV_STATUS_ERROR;
	}

	//
	// start capturing
	if (CaptureStart() != VDEV_STATUS_OK) {
		Close();
		return VDEV_STATUS_ERROR;
	}

	thread = g_thread_new("VideoDev", _VideoDevThread, NULL);

	return VDEV_STATUS_OK;
};


int VideoDev::Stop() {
	if (running == 1) {
		running = 0;		// we can jump directly to 0
	}

	if (thread) {
		g_thread_join (thread);
		thread = NULL;
	}

	return VDEV_STATUS_OK;
};


//
// try to read a video every 0.05ms (25hz)
// running = 2 ... thread is running normaly
// running = 1 ... thread need to close wait for GTK to set running to 0
// running = 0 ... thread stopped and all handlers can be freed
//
#define CYCLETIME 0.050
void VideoDev::Thread() {
	struct timeval cycle_timestamp;
	int lastsec = 0;
	float cycle_time = 0.0;
	float cycle_wait = 0.0;
	int i;

	printf ("%s:%d %s Enter\n", __FILE__, __LINE__, __FUNCTION__);
	cycle_time = get_cycletime(&cycle_timestamp);	// just start counting

	//
	// read untill something bad happens..
	while (running) {
		i = Grab(&vf);

		switch (i) {
		case VDEV_STATUS_OK:
			if (callback) gdk_threads_add_idle(callback, &vf);
			break;

		case VDEV_STATUS_AGAIN:
			break;

			default:
				running = 0;
			}
		//
		// keep 25fps, write every second a message
		cycle_time = get_cycletime(&cycle_timestamp);
		cycle_wait = (CYCLETIME - cycle_time) + cycle_wait;
		if (lastsec != cycle_timestamp.tv_sec) {
			printf ("%s:%d %s   cycle_time:%f              Freq:%f Hz                    \r", __FILE__, __LINE__, __FUNCTION__, cycle_time, (1.0/cycle_time));
			lastsec = cycle_timestamp.tv_sec;
		}
		if (cycle_wait > 0.0 && cycle_wait < 1.0 ) usleep ((int)(cycle_wait * 1000000.0));
	}

	//
	// stop capturing
	if (callback) gdk_threads_add_idle(callback, NULL);

	CaptureStop();
	UnInit();
	Close();

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


void VideoDev::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");
	if (caps & V4L2_CAP_META_CAPTURE) printf ("    V4L2_CAP_META_CAPTURE\n");
	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 VideoDev::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*)&fmt.fmt.pix.pixelformat)[0],
			((char*)&fmt.fmt.pix.pixelformat)[1],
			((char*)&fmt.fmt.pix.pixelformat)[2],
			((char*)&fmt.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);
}



int VideoDev::Grab(VideoFrame *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[vf_rec].data, fmt.fmt.pix.sizeimage);
					return VDEV_STATUS_ERROR;
			}
		}
		else {
			LockMutex();
			Convert(vf, inbuffer[0].data, len, fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height);
			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));
				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();
			Convert(vf, inbuffer[buf.index].data, buf.bytesused, fmt.fmt.pix.pixelformat, fmt.fmt.pix.width, fmt.fmt.pix.height);
			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;
}

//
// Open Device
int VideoDev::OpenInit() {
	int i;
	struct v4l2_capability vcap;
	VideoDevCtrl vctl;

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

	CLEAR (fmt);
	fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width       = 1920;
	fmt.fmt.pix.height      = 1080;
//	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB32;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
	fmt.fmt.pix.field       = V4L2_FIELD_NONE;

	if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt)) {
		fprintf (stderr, "VIDIOC_S_FMT : %s", strerror (errno));
		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;
	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;
};


//
// set video control identified by id
int VideoDev::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::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;
};



//
// prepare memory mapped buffers
int VideoDev::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 mappingn", 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;
}


//
// send the start capture signal to the cam
int VideoDev::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;
	}

	if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
		jpeg_create_decompress(&cinfo);
	}

	return VDEV_STATUS_OK;
};


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

	if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
	    jpeg_destroy_decompress(&cinfo);
	}

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


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


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

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

	conf_device = "";
	conf_devicename = "";

	vf_rec = -1;
	vf_get = -1;

	return VDEV_STATUS_OK;
};


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


//
// converts the video from input type to RGB24 type - 24Bit
//
int VideoDev::Convert (VideoFrame *dest, unsigned char *ptrsrc, int srcsize, uint32_t pixelformat, int srcw, int srch) {
	int xs, ys;
	int xd, yd;
	unsigned char r,g,b;
	unsigned char cb, cr, y1;
	unsigned char *ptrdst = NULL;

	// check if there is a destination and that the destination is large to keep
	// the full image
	if (dest == NULL || ptrsrc == NULL)
		return VDEV_STATUS_ERROR;

	if (dest->data != NULL && dest->w != srcw && dest->h != srch) {
		free (dest->data);
		dest->data = NULL;
	}

	if (dest->data == NULL) {
		dest->w = srcw;
		dest->h = srch;
		dest->size = srcw * srch * 3;
		dest->data = (unsigned char*) malloc (dest->size);
	}
	ptrdst = dest->data;

	switch (pixelformat) {
	case (V4L2_PIX_FMT_RGB32):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) {
				/* read the pixel */

				ptrsrc++;
				r = *(ptrsrc++);
				g = *(ptrsrc++);
				b = *(ptrsrc++);

				/* only paint the image if the source is within the destination */
				if (xd < dest->w) {
					/* set the pixel */
					*(ptrdst++) = b;
					*(ptrdst++) = g;
					*(ptrdst++) = r;
					xd++;
				}
			}

			/* if the source image is too small ignore the other places.. */
			if (xd < dest->w)
				ptrdst += 3 * (dest->w - xd);
			yd++;
		}
		break;

	case (V4L2_PIX_FMT_BGR32):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) {
				/* read the pixel */

				b = *(ptrsrc++);
				g = *(ptrsrc++);
				r = *(ptrsrc++);
				ptrsrc++;

				/* only paint the image if the source is within the destination */
				if (xd < dest->w) {
					/* set the pixel */
					*(ptrdst++) = b;
					*(ptrdst++) = g;
					*(ptrdst++) = r;
					xd++;
				}
			}

			/* if the source image is too small ignore the other places.. */
			if (xd < dest->w)
				ptrdst += 3 * (dest->w - xd);
			yd++;
		}
		break;

	case (V4L2_PIX_FMT_UYVY):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) {
				/* read the pixel */
				if (xs & 1) {
					y1 = (unsigned char)*(ptrsrc + 1);
					cr = (unsigned char)*(ptrsrc);
					cb = (unsigned char)*(ptrsrc - 2);
				}
				else {
					y1 = (unsigned char)*(ptrsrc + 1);
					cr = (unsigned char)*(ptrsrc + 2);
					cb = (unsigned char)*(ptrsrc);
				}

				convert2rgb (y1, cr, cb, &r, &g, &b);
				ptrsrc += 2;

				/* only paint the image if the source is within the destination */
				if (xd < dest->w) {
					/* set the pixel */
					*(ptrdst++) = b;
					*(ptrdst++) = g;
					*(ptrdst++) = r;
					xd++;
				}
			}

			/* if the source image is too small ignore the other places.. */
			if (xd < dest->w)
				ptrdst += 3 * (dest->w - xd);
			yd++;
		}
		break;

	case (V4L2_PIX_FMT_YUYV):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			if (yd < dest->h) {
				for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) {
					/* read the pixel */
					if (xs & 1) {
						y1 = *(ptrsrc);
						cb = *(ptrsrc + 1);
						cr = *(ptrsrc - 1);
					}
					else {
						y1 = *(ptrsrc);
						cb = *(ptrsrc + 3);
						cr = *(ptrsrc + 1);
					}
					convert2rgb (y1, cr, cb, &r, &g, &b);
					ptrsrc += 2;

					/* only paint the image if the source is within the destination */
					if (xd < dest->w) {
						/* set the pixel */
						*(ptrdst++) = r;
						*(ptrdst++) = g;
						*(ptrdst++) = b;
						xd++;
					}
				}

				/* if the source image is too small ignore the other places.. */
				if (xd < dest->w)
					ptrdst += 3 * (dest->w - xd);
				yd++;
			}
		}
		break;

	case (V4L2_PIX_FMT_MJPEG):
		cinfo.err = jpeg_std_error(&jerr.pub);
		jerr.pub.error_exit = jpg_error_exit;
		if (setjmp(jerr.setjmp_buffer)) {
			// jpeg data and allocations will be destroyed via StopCapture.
			printf ("%s:%d %s JPEG Error\n", __FILE__, __LINE__, __FUNCTION__);
			return VDEV_STATUS_ERROR;
		}

	    jpeg_mem_src(&cinfo, ptrsrc, srcsize);
	    jpeg_read_header(&cinfo, TRUE);
	    jpeg_start_decompress(&cinfo);

	    while (cinfo.output_scanline < cinfo.output_height) {
	        unsigned char *temp_array[] = {ptrdst + (cinfo.output_scanline) * srcw * 3};
	        jpeg_read_scanlines(&cinfo, temp_array, 1);
	    }

	    jpeg_finish_decompress(&cinfo);

		break;
	default:
			break;
	}

	return VDEV_STATUS_OK;
};