#include <stdlib.h>
#include "convert.h"
#include "gui.h"
#include "video.h"

uint32_t convert_pixelformats [] = {
		V4L2_PIX_FMT_MJPEG,
		V4L2_PIX_FMT_YUYV,
		V4L2_PIX_FMT_RGB32,
		V4L2_PIX_FMT_RGB24,
		V4L2_PIX_FMT_BGR32,
		V4L2_PIX_FMT_UYVY,
		0
};



/*
 * helper part for converting different video types
 */

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


int ConvertStart(ConvertData *cdata, uint32_t pixelformat) {
	if (cdata == NULL) return VDEV_STATUS_ERROR;

	if (pixelformat == V4L2_PIX_FMT_MJPEG) {
		jpeg_create_decompress(&cdata->cinfo);
	}

	return VDEV_STATUS_OK;
};

int ConvertStop(ConvertData *cdata, uint32_t pixelformat) {
	if (cdata == NULL) return VDEV_STATUS_ERROR;

	if (pixelformat == V4L2_PIX_FMT_MJPEG) jpeg_destroy_decompress(&cdata->cinfo);

	return VDEV_STATUS_OK;
};

/*
 * converts the video from input type to RGB24 type - 24Bit
 */
int Convert (ConvertData *cdata, 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;

	struct jpg_error_mgr jerr;
	if (cdata == NULL) return VDEV_STATUS_ERROR;

	// 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_RGB24):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			for (xs = 0, xd = 0; xs < (signed int)srcw; xs++) {
				/* read the pixel */

				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_BGR24):
		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++);

				/* 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):
		cdata->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(&cdata->cinfo, ptrsrc, srcsize);
	    jpeg_read_header(&cdata->cinfo, TRUE);
	    jpeg_start_decompress(&cdata->cinfo);

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

	    jpeg_finish_decompress(&cdata->cinfo);

		break;
	default:
			printf ("%s:%d Error no default possible, need to finish\n", __FILE__, __LINE__);
			exit (-1);
			break;
	}

	return VDEV_STATUS_OK;
};



std::string convert_from_pixelformat (uint32_t fmt) {
	char txt[5];

	snprintf (txt, 5, "%c%c%c%c", ((char*)&fmt)[0], ((char*)&fmt)[1], ((char*)&fmt)[2], ((char*)&fmt)[3]);
	return txt;
};


uint32_t convert_to_pixelformat(std::string s) {
	uint32_t u = 0;

	u = ((unsigned char) s[0]) + ((unsigned char) s[1] << 8) +
		((unsigned char) s[2] << 16) + ((unsigned char) s[3] << 24);

	return u;
};