#include "gfx_conv_c.h"

#include <strings.h>
#include <stdio.h>


void
gfx_conv_null(AVFrame *in, AVFrame *out, int width, int height)
{
	memcpy(out->data[0], in->data[0], height * in->linesize[0]);
}


void
gfx_conv_yuv410p_ycbcr422_c(AVFrame *in, AVFrame *out, int width, int height)
{
	int i;
//	bool toggle=false;
	unsigned long *po_eol;
	unsigned long *p;
	unsigned long y1;
	unsigned long y2;
	unsigned short u;
	unsigned short v;
	unsigned long a;
	unsigned long b;
	unsigned long c;
	unsigned long d;
//	printf("[%ld, %ld, %ld] -> [%ld, %ld, %ld]\n", in->linesize[0],
//		in->linesize[1], in->linesize[2], out->linesize[0], out->linesize[1],
//		out->linesize[2]);
//	memcpy(out->data[0], in->data[0], height * in->linesize[0]);
	unsigned long *po = (unsigned long *)out->data[0];
	unsigned long *pi = (unsigned long *)in->data[0];
	unsigned short *pi2 = (unsigned short *)in->data[1];
	unsigned short *pi3 = (unsigned short *)in->data[2];
	for (i = 0; i < height; i++) {
		for (p = po,
			po_eol = (unsigned long *)(((char *)po) + out->linesize[0]);
			p < po_eol;) {
//			*(((long *)po) + j) = (long)(*(pi + j) + (*(pi2 + j) << 8) 
//				+ (*(pi + j + 3) << 16) + (*(pi3 + j) << 24));
			y1 = *pi++;
			y2 = *pi++;
			u = *pi2;
			v = *pi3;
			a = (long)((y1 & 0x0FF) | ((u& 0x0FF) << 8) | ((y1 & 0x0FF00) << 8)
				| ((v & 0x0FF) << 24));
			b = (long)(((y1 & 0x0FF0000) >> 16) | ((u& 0x0FF) << 8)
				| ((y1 & 0x0FF000000) >> 8) | ((v & 0x0FF) << 24));
			c = (long)((y2 & 0x0FF) | (u & 0x0FF00) | ((y2 & 0x0FF00) << 8)
				| ((v & 0x0FF00) << 16));
			d = (long)(((y2 & 0x0FF0000) >> 16) | ((u& 0x0FF00))
				| ((y2 & 0x0FF000000) >> 8) | ((v & 0x0FF00) << 16));
//			if (toggle) {
				pi2++;
//			} else {
				pi3++;
//			}
//			toggle = !toggle;

			*(p++) = a;
			*(p++) = b;
			*(p++) = c;
			*(p++) = d;
		}
		po = (unsigned long *)((char *)po + out->linesize[0]);
		pi = (unsigned long *)(in->data[0] + i * in->linesize[0]);
		pi2 = (unsigned short *)(in->data[1] + ((i + 2) / 4)
			* in->linesize[1]);
		pi3 = (unsigned short *)(in->data[2] + ((i + 3) / 4)
			* in->linesize[2]);
	}
}


void
gfx_conv_yuv411p_ycbcr422_c(AVFrame *in, AVFrame *out, int width, int height)
{
	// this one is for cyuv
	// TODO: (== yuv410p atm)
	gfx_conv_yuv410p_ycbcr422_c(in, out, width, height);
}


void
gfx_conv_yuv420p_ycbcr422_c(AVFrame *in, AVFrame *out, int width, int height)
{
	unsigned long *po_eol;
	unsigned long *p;
	unsigned long y1;
	unsigned long y2;
	unsigned long u;
	unsigned long v;
	unsigned long a;
	unsigned long b;
	unsigned long c;
	unsigned long d;
//	printf("[%ld, %ld, %ld] -> [%ld, %ld, %ld]\n", in->linesize[0],
//		in->linesize[1], in->linesize[2], out->linesize[0], out->linesize[1],
//		out->linesize[2]);
//	memcpy(out->data[0], in->data[0], height * in->linesize[0]);
	unsigned long *po = (unsigned long *)out->data[0];
	unsigned long *pi = (unsigned long *)in->data[0];
	unsigned long *pi2 = (unsigned long *)in->data[1];
	unsigned long *pi3 = (unsigned long *)in->data[2];
	for (int i = 0; i < height; i++) {
		for (p = po, po_eol = (unsigned long *)(((char *)po)+out->linesize[0]);
			p < po_eol;) {
//			*(((long *)po) + j) = (long)(*(pi + j) + (*(pi2 + j) << 8)
//				+ (*(pi + j + 3) << 16) + (*(pi3 + j) << 24));
			y1 = *pi++;
			y2 = *pi++;
			u = *pi2++;
			v = *pi3++;
			a = (long)((y1 & 0x0FF) | ((u & 0x0FF) << 8) | ((y1 & 0x0FF00) << 8)
				| ((v & 0x0FF) << 24));
			b = (long)(((y1 & 0x0FF0000) >> 16) | ((u & 0x0FF00))
				| ((y1 & 0x0FF000000) >> 8) | ((v & 0x0FF00) << 16));
			c = (long)((y2 & 0x0FF) | ((u & 0x0FF0000) >> 8)
				| ((y2 & 0x0FF00) << 8) | ((v & 0x0FF0000) << 8));
			d = (long)(((y2 & 0x0FF0000) >> 16) | ((u & 0x0FF000000) >> 16)
				| ((y2 & 0x0FF000000) >> 8) | ((v & 0x0FF000000)));

			*(p++) = a;
			*(p++) = b;
			*(p++) = c;
			*(p++) = d;
		}
		po = (unsigned long *)((char *)po + out->linesize[0]);
		pi = (unsigned long *)(in->data[0] + i * in->linesize[0]);
		pi2 = (unsigned long *)(in->data[1] + ((i + 1) / 2) * in->linesize[1]);
		pi3 = (unsigned long *)(in->data[2] + (i / 2) * in->linesize[2]);
	}
}


#define CLIP(a) if (0xffffff00 & (uint32)a) { if (a < 0) a = 0; else a = 255; }

static inline uint32
YUV10TORGBA8888(uint16 y, uint16 u, uint16 v)
{
	int32 c = y - 64;
	int32 d = u - 512;
	int32 e = v - 512;

	int32 r = (298 * c + 409 * e + 512) >> 10;
	int32 g = (298 * c - 100 * d - 208 * e + 512) >> 10;
	int32 b = (298 * c + 516 * d + 512) >> 10;

	CLIP(r);
	CLIP(g);
	CLIP(b);

	return (uint32)((255 << 24) | (r << 16) | (g << 8) | b);
}


void
gfx_conv_yuv420p10le_rgb32_c(AVFrame *in, AVFrame *out, int width, int height)
{
	uint16 *yBase = (uint16 *)in->data[0];
	uint16 *uBase = (uint16 *)in->data[1];
	uint16 *vBase = (uint16 *)in->data[2];

	uint32 *rgbBase = (uint32 *)out->data[0];

	int uvIndex;

	for (int32 i = 0; i < height; i++) {
		uvIndex = 0;
		for (int32 j=0; j < width; j += 2) {
			rgbBase[j] = YUV10TORGBA8888(yBase[j], uBase[uvIndex],
				vBase[uvIndex]);
			rgbBase[j + 1] = YUV10TORGBA8888(yBase[j + 1], uBase[uvIndex],
				vBase[uvIndex]);
			uvIndex++;
		}

		// Advance pointers to next line
		yBase += in->linesize[0] / 2;

		if ((i & 1) == 0) {
			// These are the same for 2 lines
			uBase += in->linesize[1] / 2;
			vBase += in->linesize[2] / 2;
		}

		rgbBase += out->linesize[0] / 4;
	}
}


// http://en.wikipedia.org/wiki/YUV
static inline uint32
YUV444TORGBA8888(uint8 y, uint8 u, uint8 v)
{
	int32 c = y - 16;
	int32 d = u - 128;
	int32 e = v - 128;

	int32 r = (298 * c + 409 * e + 128) >> 8;
	int32 g = (298 * c - 100 * d - 208 * e + 128) >> 8;
	int32 b = (298 * c + 516 * d + 128) >> 8;

	CLIP(r);
	CLIP(g);
	CLIP(b);

	return (uint32)((255 << 24) | (r << 16) | (g << 8) | b);
}


void
gfx_conv_yuv410p_rgb32_c(AVFrame *in, AVFrame *out, int width, int height)
{
	uint8 *yBase = (uint8 *)in->data[0];
	uint8 *uBase = (uint8 *)in->data[1];
	uint8 *vBase = (uint8 *)in->data[2];

	uint32 *rgbBase = (uint32 *)out->data[0];

	int uvIndex;

	for (int32 i = 0; i < height; i++) {
		uvIndex = 0;
		for (int32 j=0; j < width; j+=4) {
			rgbBase[j] = YUV444TORGBA8888(yBase[j], uBase[uvIndex],
				vBase[uvIndex]);
			rgbBase[j + 1] = YUV444TORGBA8888(yBase[j + 1], uBase[uvIndex],
				vBase[uvIndex]);
			rgbBase[j + 2] = YUV444TORGBA8888(yBase[j + 2], uBase[uvIndex],
				vBase[uvIndex]);
			rgbBase[j + 3] = YUV444TORGBA8888(yBase[j + 3], uBase[uvIndex],
				vBase[uvIndex]);
			uvIndex++;
		}

		// Advance pointers to next line
		yBase += in->linesize[0];

		if ((i & 3) == 0) {
			// These are the same for 4 lines
			uBase += in->linesize[1];
			vBase += in->linesize[2];
		}

		rgbBase += out->linesize[0] / 4;
	}
}


void
gfx_conv_yuv411p_rgb32_c(AVFrame *in, AVFrame *out, int width, int height)
{
	gfx_conv_null(in, out, width, height);
}


void
gfx_conv_YCbCr422_RGB32_c(AVFrame *in, AVFrame *out, int width, int height)
{
	uint8 *yBase = (uint8 *)in->data[0];
	uint8 *uBase = (uint8 *)in->data[1];
	uint8 *vBase = (uint8 *)in->data[2];

	uint32 *rgbBase = (uint32 *)out->data[0];

	int uvIndex;

	for (int32 i = 0; i < height; i++) {

		uvIndex = 0;

		for (int32 j = 0; j < width; j += 2) {
			rgbBase[j] = YUV444TORGBA8888(yBase[j], uBase[uvIndex],
				vBase[uvIndex]);
			rgbBase[j + 1] = YUV444TORGBA8888(yBase[j + 1], uBase[uvIndex],
				vBase[uvIndex]);

			uvIndex++;
		}

		yBase += in->linesize[0];
		uBase += in->linesize[1];
		vBase += in->linesize[2];

		rgbBase += out->linesize[0] / 4;
	}

	if (height & 1) {
		// XXX special case for last line if height not multiple of 2 goes here
		memset((height - 1) * out->linesize[0] + (uint8 *)out->data[0], 0,
			width * 4);
	}

}


void
gfx_conv_GBRP_RGB32_c(AVFrame *in, AVFrame *out, int width, int height)
{
	uint8 *bBase = (uint8 *)in->data[0];
	uint8 *gBase = (uint8 *)in->data[1];
	uint8 *rBase = (uint8 *)in->data[2];

	uint32 *rgbBase = (uint32 *)out->data[0];

	for (int32 i = 0; i < height; i++) {

		for (int32 j = 0; j < width; j ++) {
			rgbBase[j] = gBase[j] | (bBase[j] << 8) | (rBase[j] << 16);
		}

		bBase += in->linesize[0];
		gBase += in->linesize[1];
		rBase += in->linesize[2];

		rgbBase += out->linesize[0] / 4;
	}
}