#include "vidoiltank.h"

//
// 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 convert (struct jpeg_decompress_struct *cdata, ImageRaw *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 -1;

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

	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):
		cdata->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 -1;
		}

	    jpeg_mem_src(cdata, ptrsrc, srcsize);
	    jpeg_read_header(cdata, TRUE);
	    jpeg_start_decompress(cdata);

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

	    jpeg_finish_decompress(cdata);

		break;
	default:
			break;
	}

	return 0;
};


int convertstart(struct jpeg_decompress_struct *cdata, uint32_t pixelformat) {
	if (cdata == NULL) return -1;

	if (pixelformat == V4L2_PIX_FMT_MJPEG) {
		jpeg_create_decompress(cdata);
	}

	return 0;
};


int convertstop(struct jpeg_decompress_struct *cdata, uint32_t pixelformat) {
	if (cdata == NULL) return -1;
	if (pixelformat == V4L2_PIX_FMT_MJPEG) jpeg_destroy_decompress(cdata);
	return 0;
};