#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>

#ifdef BUILD_WINDOWS
#include "windows.h"
#else
#include <arpa/inet.h>
#endif

#include "convert.h"
#include "video.h"
#include "configuration.h"
#include "debayer.h"

int debayer_mode = 1;		// testing 0 or 1

uint32_t convert_pixelformats [] = {
		V4L2_PIX_FMT_MJPEG,
		V4L2_PIX_FMT_YUYV,
		V4L2_PIX_FMT_RGB32,
		V4L2_PIX_FMT_BGR32,
		V4L2_PIX_FMT_RGB24,
		V4L2_PIX_FMT_BGR24,
		V4L2_PIX_FMT_UYVY,
		V4L2_PIX_FMT_SGRBG16,
		V4L2_PIX_FMT_SRGGB10P,
		V4L2_PIX_FMT_SRGGB10,
		V4L2_PIX_FMT_SGBRG10,
		V4L2_PIX_FMT_SRGGB8,
		V4L2_PIX_FMT_SGRBG8,
		0
};


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

/*
 * will be called on first frame
 */

//
// 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) {
	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 0;

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

	return 1;
};

int ConvertStop(ConvertData *cdata, uint32_t pixelformat) {
	dumpframe_close();

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

	return 1;
};


/*
 * 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;

	debug ("srcsize:%d pixfmt:%s size: %dx%d", srcsize, convert_from_pixelformat(pixelformat).c_str(), srcw, srch);
	if (config.vdev_dumppath.length() > 0) dumpframe(ptrsrc, srcsize, pixelformat, srcw, srch);

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

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

	dest->SetSize(srcw, srch);
	ptrdst = dest->GetPixBuf();

	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 */

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

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

			/* if the source image is too small ignore the other places.. */
			if (xd < srcw)
				ptrdst += 3 * (srcw - 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 */

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

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

			/* if the source image is too small ignore the other places.. */
			if (xd < srcw)
				ptrdst += 3 * (srcw - 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 < srcw) {
					/* set the pixel */
					*(ptrdst++) = r;
					*(ptrdst++) = g;
					*(ptrdst++) = b;
					xd++;
				}
			}

			/* if the source image is too small ignore the other places.. */
			if (xd < srcw)
				ptrdst += 3 * (srcw - 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 < srcw) {
					/* set the pixel */
					*(ptrdst++) = r;
					*(ptrdst++) = g;
					*(ptrdst++) = b;
					xd++;
				}
			}

			/* if the source image is too small ignore the other places.. */
			if (xd < srcw)
				ptrdst += 3 * (srcw - xd);
			yd++;
		}
		break;
	
	case (V4L2_PIX_FMT_SGRBG16):
		debayer_grbg16 ((uint16_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
		break;

	case (V4L2_PIX_FMT_SRGGB10P):
		debayer_rggb10packet ((uint8_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
		break;

	case (V4L2_PIX_FMT_SGBRG10):
        debayer_gbrg10 ((uint8_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
        break;

	case (V4L2_PIX_FMT_SRGGB10):
        debayer_rggb10 ((uint8_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
        break;

	case (V4L2_PIX_FMT_SRGGB8):
		debayer_rggb8 ((uint8_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
		break;

	case (V4L2_PIX_FMT_SGRBG8):
		debayer_grbg8 ((uint8_t *)ptrsrc, srcw, srch, ptrdst, srcw, srch);
		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 < srcw) {
					/* set the pixel */
					*(ptrdst++) = r;
					*(ptrdst++) = g;
					*(ptrdst++) = b;
					xd++;
				}
			}

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

	case (V4L2_PIX_FMT_YUYV):
		for (ys = 0, yd = 0; ys < (signed int)srch; ys++) {
			if (yd < srch) {
				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 < srcw) {
						/* set the pixel */
						*(ptrdst++) = r;
						*(ptrdst++) = g;
						*(ptrdst++) = b;
						xd++;
					}
				}

				/* if the source image is too small ignore the other places.. */
				if (xd < srcw)
					ptrdst += 3 * (srcw - 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 0;
		}

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



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


/*
 * copy part of an raw image
 * destination must be pointer in case we need to align the size of the destination image.
 * this function will also realloc needed memory if needed. (only if: givin size < needed size)
 *
 * crop image to event set of dimensions
 */
int PixCopy(unsigned char *srcdata, uint32_t srcpixfmt, int srcw, int srch,
			unsigned char **dstdataptr, int *dstsize, int *dstw, int *dsth,
			int regionx, int regiony, int regionw, int regionh) {

	if (srcpixfmt == 0 || srcpixfmt == V4L2_PIX_FMT_MJPEG) return 0;
	if (srcdata == NULL || dstdataptr == NULL) return 0;

	// crop size to an even number
	(*dsth) = regionw & ~1;
	(*dstw) = regionh & ~1;

	int bytesperpixel = 3;
	int dsize = 0;

	switch (srcpixfmt) {
	case (V4L2_PIX_FMT_SGRBG8):
		bytesperpixel = 1;
		break;
	case (V4L2_PIX_FMT_SGRBG16):
		bytesperpixel = 2;
		break;
	case (V4L2_PIX_FMT_BGR32):
	case (V4L2_PIX_FMT_RGB32):
		bytesperpixel = 4;
		break;
	case (V4L2_PIX_FMT_BGR24):
	case (V4L2_PIX_FMT_RGB24):
		bytesperpixel = 3;
		break;
	default:
		return 0;
	}

	//
	// calculate image size and allocate memory if needed
	dsize = (*dsth) * (*dstw) * bytesperpixel;
	if ((*dstsize) < dsize || (*dstdataptr) == NULL) {
		*dstdataptr = (unsigned char*) realloc (*dstdataptr, dsize);
		if (*dstdataptr == NULL) {
			errorexit((char*)"%s:%d could not realloc memory. dsize:%d   error:%s\n", __FILE__, __LINE__, dsize, strerror(errno));
		}
		*dstsize = dsize;
		printf ("%s:%d reallocate memory for destination raw image\n", __FILE__, __LINE__);
	}

	unsigned char *dptr, *sptr;
	int y, dy, x;

	//	debug_drawraw(srcdata, srcpixfmt, srcw, srch);
	for (y = regiony & (~1), dy = 0; dy < *dsth && y < srch; y++, dy++) {
		x = regionx & (~1);
		dptr = (*dstdataptr) + bytesperpixel * (dy * *dstw);
		sptr = (srcdata) + bytesperpixel * ( y * srcw + x);
		memcpy (dptr, sptr, *dstw * bytesperpixel);
	}

	return 1;
}