#include "yuv.h"

#define TOX

// fixed point YUV to RGB conversion according to ITU-R BT.601
// see libwebp's yuv.h

#define clamp(v)	(v > 255 ? 255 : v < 0 ? 0 : v)

#ifdef WEBP
enum {
	YUV_FIX2	= 6,
	YUV_MASK2 	=((256<<YUV_FIX2)-1),
};

#define mulhi(a, b) ((a*b)>>8)

#define clip8(v) ((v & ~YUV_MASK2) == 0 ? (v >> YUV_FIX2) : (v < 0) ? 0 : 255)
#define yuvr(y, v) (clip8(mulhi(y, 19077) + mulhi(v, 26149) - 14234))
#define yuvg(y, u, v) (clip8(mulhi(y, 19077) - mulhi(u, 6419) - mulhi(v, 13320) + 8708))
#define yuvb(y, u) (clip8(mulhi(y, 19077) + mulhi(u, 33050) - 17685))

#define yuvtorgb(yuv, rgb) do{ \
	int y=yuv[0],u=yuv[1],v=yuv[2];\
	rgb[0]=yuvr(y,v); \
	rgb[1]=yuvg(y,u,v); \
	rgb[2]=yuvb(y,u); \
	}while(0)

#endif

#ifdef TOX

// uTox (?) yuv decoder
#define yuvtorgb(yuv, rgb) do{ \
	int r,g,b,ty,tu,tv; \
	ty = yuv[0], tu = yuv[1], tv = yuv[2]; \
	ty = ty < 16 ? 16 : ty; \
	r = (298 * (ty - 16) + 409 * (tv - 128) + 128) >> 8; \
	g = (298 * (ty - 16) - 100 * (tu - 128) - 208 * (tv - 128) + 128) >> 8; \
	b = (298 * (ty - 16) + 516 * (tu - 128) + 128) >> 8; \
	rgb[0] = clamp(r); \
	rgb[1] = clamp(g); \
	rgb[2] = clamp(b); \
	}while(0)
#endif

#ifdef AIJU
#define yuvtorgb(yuv, rgb) do{ \
	double Y, U, V, R, G, B; \
	Y = ((int)yuv[0] - 16) / 219.0; \
	U = ((int)yuv[1] - 128) / 224.0; \
	V = ((int)yuv[2] - 128) / 224.0; \
	R = Y + V; \
	B = Y + U; \
	G = Y - 0.509 * V - 0.194 * U; \
	if(R < 0) R = 0; if(R > 1) R = 1; \
	if(G < 0) G = 0; if(G > 1) G = 1; \
	if(B < 0) B = 0; if(B > 1) B = 1; \
	rgb[0] = R * 255; \
	rgb[1] = G * 255; \
	rgb[2] = B * 255; \
	}while(0)
#endif


int
YUV420ptoRGB888(unsigned char *yuv, int width, int height, unsigned char *rgb)
{
	int total, t4, px, py;
	int uvoff;
	int cyuv[4];

	total = width * height;
	t4 = total + (total/4);

	for(py = 0; py < height; py++){
		for(px = 0; px < width; px++){
			uvoff = (py/2)*(width/2)+(px/2);

			cyuv[0] = yuv[py * width + px];
			cyuv[1] = yuv[uvoff+total];
			cyuv[2] = yuv[uvoff+t4];

			yuvtorgb(cyuv, rgb);

			rgb += 3;
		}
	}

	return total * 3;
}