#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 "gui.h"
#include "video.h"
#include "configuration.h"
#include "debayer.h"

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_SGRBG8,
		0
};


/*
 * dump data into debug file
 *
 * file description:
 * <HEDER>
 * 		uint32_t width;
 * 		uint32_t height;
 * 		uint32_t pixelformat;
 * <FRAMEHEADER>
 * 		uint32_t size;
 * 		uint32_t timestamp_ms;
 * <FRAME>
 * 		uint8_t data[size];
 * <FRAMEHEADER>
 * <FRAME>
 * ....
 */
int convert_debug_fd = -1;
struct timeval convert_debug_tv;

/*
 * will be called on first frame
 */
int convert_debug_open(uint32_t pixelformat, int srcw, int srch) {
	time_t t = time(NULL);
	struct tm *tmptr;
	char fname[LEN_FILENAME];
	char fullfname[LEN_FULLFILENAME];

	if (config.debugpath == NULL) return -1;

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

	//
	// check to create file, if not possible try creating the directory and create the file again
	tmptr = localtime(&t);
	strftime(fname, LEN_FILENAME, "%Y%m%d-%H%M%S", tmptr);
	snprintf (fullfname, LEN_FULLFILENAME, "%s/%s.videodump", config.debugpath, fname);

	if ((convert_debug_fd = creat (fullfname, 0666)) == -1) {
		printf ("%s:%d could not create file '%s'. Error:%s\n", __FILE__, __LINE__, fullfname, strerror(errno));
		printf ("%s:%d try to create folder\n", __FILE__, __LINE__);

		// create folder
		// FIXME: how to do thin on windows
#ifdef BUILD_WINDOWS
		if ((mkdir (config.debugpath)) == -1) {
#else
		if ((mkdir (config.debugpath, 0777)) == -1) {
#endif
			printf ("%s:%d could not create debug folder.\n", __FILE__, __LINE__);
			return -1;
		}
		if ((convert_debug_fd = creat (fullfname, 0666)) == -1) {
			printf ("%s:%d could again not create file '%s'. Error:%s\n", __FILE__, __LINE__, fullfname, strerror(errno));
			return -1;
		}
	}


	//
	// file is open for writing, write header information
	uint32_t wwidth = htonl(srcw);
	uint32_t wheight = htonl(srch);
	uint32_t wpixfmt = htonl(pixelformat);
	write (convert_debug_fd, &wwidth, 4);
	write (convert_debug_fd, &wheight, 4);
	write (convert_debug_fd, &wpixfmt, 4);

	//
	// save start time, this is needed to calculate the time between the frames
	gettimeofday (&convert_debug_tv, NULL);

	return 0;
}


/*
 * if fd is not open open file and set up header
 */
void convert_debug_dumpframe(unsigned char *ptrsrc, int srcsize, uint32_t pixelformat, int srcw, int srch) {
	struct timeval tv;
	uint32_t wts, ts;
	uint32_t wsize;

	if (ptrsrc == NULL) return;
	if (config.debugpath == NULL) return;
	if (convert_debug_fd == -1)
		if (convert_debug_open(pixelformat, srcw, srch) == -1) return;

	//
	// construct and write header
	wsize = htonl(srcsize);
	gettimeofday (&tv, NULL);
	ts = 1000 * (tv.tv_sec - convert_debug_tv.tv_sec) +
			    (tv.tv_usec - convert_debug_tv.tv_usec) / 1000;
	wts = htonl(ts);

	write (convert_debug_fd, &wsize, 4);
	write (convert_debug_fd, &wts, 4);

	//
	// write frame
	write (convert_debug_fd, ptrsrc, srcsize);
};


/*
 * close file and reset all data
 */
void convert_debug_close() {
	if (convert_debug_fd != -1) {
		close (convert_debug_fd);
		convert_debug_fd = -1;
	}
}





/*
 * 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) {
	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) {
	convert_debug_close();

	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;

	if (config.debugpath != NULL) convert_debug_dumpframe(ptrsrc, srcsize, pixelformat, srcw, srch);

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

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

				/* 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_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 < 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_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++) = 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_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++) = 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_SGRBG16):
	        if (config.debayer_mode == 0)
		  debayer_grbg16_simple ((uint16_t *)ptrsrc, srcw, srch, ptrdst, dest->w, dest->h);
	        else
		  debayer_grbg16_bilinear ((uint16_t *)ptrsrc, srcw, srch, ptrdst, dest->w, dest->h);
		break;

	case (V4L2_PIX_FMT_SGRBG8):
	        if (config.debayer_mode == 0)
		  debayer_grbg8_simple ((uint8_t *)ptrsrc, srcw, srch, ptrdst, dest->w, dest->h);
	        else
		  debayer_grbg8_bilinear ((uint8_t *)ptrsrc, srcw, srch, ptrdst, dest->w, dest->h);
		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++) = 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_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;
};


/*
 * 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);
		*dstsize = dsize;
		printf ("%s:%d reallocate memory for destination raw image\n", __FILE__, __LINE__);
	}

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

	//	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);
		for (dx = (*dstw*bytesperpixel); dx > 0; dx--, dptr++, sptr++)
			*dptr = *sptr;
	}

	if (config.show_debugwin) debug_drawraw(*dstdataptr, srcpixfmt, *dstw, *dsth);
	return 1;
}