/*******************************************************************
                  YUV frame treating module
 *******************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define FRAME_C
#include "frame.h"

const unsigned char uchar_clip_table[1024] = {
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  0,  0,  0,  0,  0,  0,  0,
	  0,  1,  2,  3,  4,  5,  6,  7,
	  8,  9, 10, 11, 12, 13, 14, 15,
	 16, 17, 18, 19, 20, 21, 22, 23,
	 24, 25, 26, 27, 28, 29, 30, 31,
	 32, 33, 34, 35, 36, 37, 38, 39,
	 40, 41, 42, 43, 44, 45, 46, 47,
	 48, 49, 50, 51, 52, 53, 54, 55,
	 56, 57, 58, 59, 60, 61, 62, 63,
	 64, 65, 66, 67, 68, 69, 70, 71,
	 72, 73, 74, 75, 76, 77, 78, 79,
	 80, 81, 82, 83, 84, 85, 86, 87,
	 88, 89, 90, 91, 92, 93, 94, 95,
	 96, 97, 98, 99,100,101,102,103,
	104,105,106,107,108,109,110,111,
	112,113,114,115,116,117,118,119,
	120,121,122,123,124,125,126,127,
	128,129,130,131,132,133,134,135,
	136,137,138,139,140,141,142,143,
	144,145,146,147,148,149,150,151,
	152,153,154,155,156,157,158,159,
	160,161,162,163,164,165,166,167,
	168,169,170,171,172,173,174,175,
	176,177,178,179,180,181,182,183,
	184,185,186,187,188,189,190,191,
	192,193,194,195,196,197,198,199,
	200,201,202,203,204,205,206,207,
	208,209,210,211,212,213,214,215,
	216,217,218,219,220,221,222,223,
	224,225,226,227,228,229,230,231,
	232,233,234,235,236,237,238,239,
	240,241,242,243,244,245,246,247,
	248,249,250,251,252,253,254,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
	255,255,255,255,255,255,255,255,
};

FRAME *new_frame(int width, int height);
void delete_frame(FRAME *p);
FRAME *copy_frame(FRAME *in);

void upsample_chroma_444(FRAME *p);
void upsample_chroma_422(FRAME *p);
void upsample_chroma_420i(FRAME *p);
void upsample_chroma_420p(FRAME *p);

void upsample_chroma_422_mmx(FRAME *p);
void upsample_chroma_420i_mmx(FRAME *p);
void upsample_chroma_420p_mmx(FRAME *p);

void __stdcall yuv444_to_bgr(FRAME *top, FRAME *bottom, unsigned char *out, BGR_CONVERSION_PARAMETER *prm);

static void chroma420i_to_422(unsigned char *data, int width, int height);
static void chroma420p_to_422(unsigned char *data, int width, int height);
static void chroma422_to_444(unsigned char *data, int width, int height);

/* in yuv_to_bgr.asm */
extern void __stdcall chroma420i_to_422_mmx(unsigned char *data, int width, int height);
extern void __stdcall chroma420p_to_422_mmx(unsigned char *data, int width, int height);
extern void __stdcall chroma422_to_444_mmx(unsigned char *data, int width, int height);

FRAME *new_frame(int width, int height)
{
	FRAME *r;

	r = (FRAME *)malloc(sizeof(FRAME));

	r->width = width;
	r->height = height;

	r->p = (unsigned char *)malloc(width * height * 3 + 16);

	r->y = r->p + 16 - ( ((long)r->p) & 0xf);
	r->u = r->y + (width * height);
	r->v = r->u + (width * height);

	return r;
}

FRAME *copy_frame(FRAME *in)
{
	FRAME *r;

	if(in == NULL){
		return NULL;
	}

	r = new_frame(in->width, in->height);

	if(r == NULL){
		return NULL;
	}

	memcpy(r->y, in->y, in->width * in->height);
	memcpy(r->u, in->u, in->width * in->height);
	memcpy(r->v, in->v, in->width * in->height);
	
	return r;
}

void delete_frame(FRAME *p)
{
	free(p->p);
	
	free(p);
}

void __stdcall yuv444_to_bgr(FRAME *top, FRAME *bottom, unsigned char *out, BGR_CONVERSION_PARAMETER *prm)
{
	int i, j;
	unsigned char *yt, *ut, *vt;
	unsigned char *yb, *ub, *vb;
	unsigned char *r, *g, *b;

	yt = top->y;
	ut = top->u;
	vt = top->v;

	yb = bottom->y + prm->in_step;
	ub = bottom->u + prm->in_step;
	vb = bottom->v + prm->in_step;

	b = out;
	g = out+1;
	r = out+2;

	for(i=0;i<prm->height/2;i++){
		for(j=0;j<prm->width;j++){
			b[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->bu * (((int)ut[j]) - 128)) >> 16)];
			g[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->gu * (((int)ut[j]) - 128) + prm->gv * (((int)vt[j]) - 128)) >> 16)];
			r[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->rv * (((int)vt[j]) - 128)) >> 16)];
		}
		yt += prm->in_step*2;
		ut += prm->in_step*2;
		vt += prm->in_step*2;
		b += prm->out_step;
		g += prm->out_step;
		r += prm->out_step;
		
		for(j=0;j<prm->width;j++){
			b[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yb[j] - prm->yo) + prm->bu * (((int)ub[j]) - 128)) >> 16)];
			g[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yb[j] - prm->yo) + prm->gu * (((int)ub[j]) - 128) + prm->gv * (((int)vb[j]) - 128)) >> 16)];
			r[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yb[j] - prm->yo) + prm->rv * (((int)vb[j]) - 128)) >> 16)];
		}
		yb += prm->in_step*2;
		ub += prm->in_step*2;
		vb += prm->in_step*2;
		b += prm->out_step;
		g += prm->out_step;
		r += prm->out_step;
	}

	if(prm->height%2){
		for(j=0;j<prm->width;j++){
			b[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->bu * (((int)ut[j]) - 128)) >> 16)];
			g[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->gu * (((int)ut[j]) - 128) + prm->gv * (((int)vt[j]) - 128)) >> 16)];
			r[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( prm->yg * ((int)yt[j] - prm->yo) + prm->rv * (((int)vt[j]) - 128)) >> 16)];
		}
	}
}

static void chroma420i_to_422(unsigned char *data, int width, int height)
{
	int x,y;
	int hw;

	hw = width >> 1;
	
	/* copy last 2 lines */
	memcpy(data+(height-2)*width, data+((height>>1)-2)*width, hw);
	memcpy(data+(height-1)*width, data+((height>>1)-1)*width, hw);

	/* interpolation */
	for(y=(height>>2)-2;y>0;y--){
		for(x=0;x<hw;x++){
			data[(y*4+2)*width+x] = ( ( (unsigned int) data[(y*2  )*width+x] * 5 + data[(y*2+2)*width+x] * 3) >> 3);
			data[(y*4+3)*width+x] = ( ( (unsigned int) data[(y*2+1)*width+x] * 7 + data[(y*2+3)*width+x]    ) >> 3);
			data[(y*4+4)*width+x] = ( ( (unsigned int) data[(y*2  )*width+x]     + data[(y*2+2)*width+x] * 7) >> 3);
			data[(y*4+5)*width+x] = ( ( (unsigned int) data[(y*2+1)*width+x] * 3 + data[(y*2+3)*width+x] * 5) >> 3);
		}			
	}

	/* process top 6 lines */
	for(x=0;x<hw;x++){
		data[4*width+x] = ( ( (unsigned int) data[      x]     + data[2*width+x] * 7) >> 3);
		data[5*width+x] = ( ( (unsigned int) data[width+x] * 3 + data[3*width+x] * 5) >> 3);
		data[2*width+x] = ( ( (unsigned int) data[      x] * 5 + data[2*width+x] * 3) >> 3);
		data[3*width+x] = ( ( (unsigned int) data[width+x] * 7 + data[3*width+x]    ) >> 3);
	}
}

static void chroma420p_to_422(unsigned char *data, int width, int height)
{
	int x,y;
	int hw,hh;

	hw = width >> 1;
	hh = height >> 1;
	
	/* copy last line */
	memcpy(data+((height-1)*width), data+((hh-1)*width), hw);

	/* interpolation */
	for(y=hh-2;y>0;y--){
		for(x=0;x<hw;x++){
			data[(2*y+1)*width+x] = ( ((unsigned int) data[y*width+x] * 3 + data[(y+1)*width+x]) >> 2 );
			data[(2*y+2)*width+x] = ( ((unsigned int) data[y*width+x] + data[(y+1)*width+x] * 3) >> 2 );
		}
	}

	/* top 3 lines */
	for(x=0;x<hw;x++){
		data[2*width+x] = ( ((unsigned int) data[x] + data[width+x] * 3) >> 2 );
		data[width+x] = ( ((unsigned int) data[x] * 3 + data[width+x]) >> 2 );
	}

}

static void chroma422_to_444(unsigned char *data, int width, int height)
{
	int x,y;

	for(y=0;y<height;y++){
		/* copy pass */
		for(x=(width>>1)-1;x>=0;x--){
			data[y*width+x*2] = data[y*width+x];
		}

		/* supplement pass */
		for(x=0;x<(width>>1)-1;x++){
			data[y*width+x*2+1] = ( ( (unsigned int) data[y*width+x*2] + data[y*width+x*2+2] ) >> 1);
		}

		/* last colum */
		data[y*width+width-1] = data[y*width+width-2];
	}
}

void upsample_chroma_444(FRAME *p)
{
	return;
}

void upsample_chroma_422(FRAME *p)
{
	chroma422_to_444(p->u, p->width, p->height);
	chroma422_to_444(p->v, p->width, p->height);
}

void upsample_chroma_420i(FRAME *p)
{
	chroma420i_to_422(p->u, p->width, p->height);
	chroma422_to_444(p->u, p->width, p->height);
	chroma420i_to_422(p->v, p->width, p->height);
	chroma422_to_444(p->v, p->width, p->height);
}

void upsample_chroma_420p(FRAME *p)
{
	chroma420p_to_422(p->u, p->width, p->height);
	chroma422_to_444(p->u, p->width, p->height);
	chroma420p_to_422(p->v, p->width, p->height);
	chroma422_to_444(p->v, p->width, p->height);
}

void upsample_chroma_422_mmx(FRAME *p)
{
	chroma422_to_444_mmx(p->u, p->width, p->height);
	chroma422_to_444_mmx(p->v, p->width, p->height);
}

void upsample_chroma_420i_mmx(FRAME *p)
{
	chroma420i_to_422_mmx(p->u, p->width, p->height);
	chroma422_to_444_mmx(p->u, p->width, p->height);
	chroma420i_to_422_mmx(p->v, p->width, p->height);
	chroma422_to_444_mmx(p->v, p->width, p->height);
}

void upsample_chroma_420p_mmx(FRAME *p)
{
	chroma420p_to_422_mmx(p->u, p->width, p->height);
	chroma422_to_444_mmx(p->u, p->width, p->height);
	chroma420p_to_422_mmx(p->v, p->width, p->height);
	chroma422_to_444_mmx(p->v, p->width, p->height);
}