/*******************************************************************

 *******************************************************************/

#include <stdio.h>
#include <stdlib.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);

void yuv_420_to_422(FRAME *p, int interlace);
void yuv_422_to_444(FRAME *p);

void frame_to_bgr(FRAME *in, unsigned char *out, int chroma_format, int progressive, int type);
void save_frame_bmp(FRAME *in, int frame_count, int chroma_format, int progressive, int conv_type);

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

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

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

	r->y = (unsigned char *)malloc(width * height);
	r->u = (unsigned char *)malloc(width * height);
	r->v = (unsigned char *)malloc(width * height);

	return r;
}

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

void yuv_420_to_422(FRAME *p, int progressive)
{
	int x,y;
	int w,h;

	unsigned char *su;
	unsigned char *du;
	unsigned char *sv;
	unsigned char *dv;

	w = p->width;
	h = p->height;
	
	if(progressive){
		su = p->u + (w * (h/2-1));
		sv = p->v + (w * (h/2-1));
		du = p->u + (w * (h-2));
		dv = p->v + (w * (h-2));
		
		for(y=0;y<(h>>1);y++){
			for(x=0;x<(w>>1);x++){
				du[x] = su[x];
				du[w+x] = su[x];

				dv[x] = sv[x];
				dv[w+x] = sv[x];
			}
			su -= w;
			sv -= w;
			du -= 2*w;
			dv -= 2*w;
		}
	}else{
		su = p->u + (w * (h/2-2));
		sv = p->v + (w * (h/2-2));
		du = p->u + (w * (h-4));
		dv = p->v + (w * (h-4));

		for(y=0;y<(h>>2);y++){
			for(x=0;x<(w>>1);x++){
				du[x] = su[x];
				du[w+x] = su[w+x];
				du[w*2+x] = su[x];
				du[w*3+x] = su[w+x];

				dv[x] = sv[x];
				dv[w+x] = sv[w+x];
				dv[w*2+x] = sv[x];
				dv[w*3+x] = sv[w+x];
			}
			su -= 2*w;
			sv -= 2*w;
			du -= 4*w;
			dv -= 4*w;
		}
	}
}

void yuv_422_to_444(FRAME *p)
{
	int x,y;
	int w,h;

	unsigned char *su;
	unsigned char *sv;
	unsigned char *du;
	unsigned char *dv;

	w = p->width;
	h = p->height;

	su = p->u + (w/2-1);
	sv = p->v + (w/2-1);
	du = p->u + (w-2);
	dv = p->v + (w-2);
	
	for(y=0;y<h;y++){
		for(x=0;x<(w>>1);x++){
			du[-x*2] = su[-x];
			du[1-x*2] = su[-x];

			dv[-x*2] = sv[-x];
			dv[1-x*2] = sv[-x];
		}
		su += w;
		sv += w;
		dv += w;
		du += w;
	}
}

static const int yuv_to_rgb_table[2][3][3] = {
	{	/*   Y       U       V        */     
		{65537,    -61,  91860}, /* R */
		{65989, -22680, -47127}, /* G */
		{63197, 116944,   1735}, /* B */
	}, /* straight conversion */
	
	{	/*   Y       U       V        */     
		{76310,    -69, 104572}, /* R */
		{76837, -25819, -53649}, /* G */
		{73585, 133128,   1735}, /* B */
	}, /* re-map conversion */
};

void frame_to_bgr(FRAME *in, unsigned char *out, int chroma_format, int progressive, int type)
{
	int i, j;
	unsigned char *y, *u, *v;
	unsigned char *r, *g, *b;

	if(chroma_format == 1){
		yuv_420_to_422(in, progressive);
		yuv_422_to_444(in);
	}
	if(chroma_format == 2){
		yuv_422_to_444(in);
	}
	
	y = in->y;
	u = in->u;
	v = in->v;

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

	for(i=0;i<in->height;i++){
		for(j=0;j<in->width;j++){
			b[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( yuv_to_rgb_table[type][2][0] * y[j] + yuv_to_rgb_table[type][2][1] * ((int)(u[j]) - 128) + yuv_to_rgb_table[type][2][2] * ((int)(v[j]) - 128) ) >> 16)];
			g[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( yuv_to_rgb_table[type][1][0] * y[j] + yuv_to_rgb_table[type][1][1] * ((int)(u[j]) - 128) + yuv_to_rgb_table[type][1][2] * ((int)(v[j]) - 128) ) >> 16)];
			r[j*3] = uchar_clip_table[UCHAR_CLIP_TABLE_OFFSET+(( yuv_to_rgb_table[type][0][0] * y[j] + yuv_to_rgb_table[type][0][1] * ((int)(u[j]) - 128) + yuv_to_rgb_table[type][0][2] * ((int)(v[j]) - 128) ) >> 16)];
		}
		y += in->width;
		u += in->width;
		v += in->width;
		b += in->width * 3;
		g += in->width * 3;
		r += in->width * 3;
	}
}

void save_frame_bmp(FRAME *in, int frame_count, int chroma_format, int progressive, int conv_type)
{
	FILE *out;
	char filename[FILENAME_MAX];
	unsigned char *buffer;

	int image_size;
	int file_size;
	
	unsigned char bmp24[54] = {
		'B','M', 0,0,0,0, 0,0,0,0, 0x36,0,0,0,
		0x28,0,0,0, 0,0,0,0, 0,0,0,0, 1,0, 0x18,0,
		0,0,0,0, 0,0,0,0, 0x12,0x0B,0,0, 0x12,0x0B,0,0,
		0,0,0,0, 0,0,0,0,
	};

	image_size = in->width * in->height * 3;
	file_size = image_size + 54;

	bmp24[2] = file_size;
	bmp24[3] = file_size >> 8;
	bmp24[4] = file_size >> 16;
	bmp24[5] = file_size >> 24;

	bmp24[18] = in->width;
	bmp24[19] = in->width >> 8;
	bmp24[20] = in->width >> 16;
	bmp24[21] = in->width >> 24;

	bmp24[22] = in->height;
	bmp24[23] = in->height >> 8;
	bmp24[24] = in->height >> 16;
	bmp24[25] = in->height >> 24;

	buffer = (unsigned char *)malloc(image_size);

	frame_to_bgr(in, buffer, chroma_format, progressive, conv_type);

	sprintf(filename, "%08d.bmp", frame_count);
	out = fopen(filename, "wb");
	fwrite(bmp24, 1, 54, out);
	fwrite(buffer, 1, image_size, out);
	fclose(out);
	free(buffer);
}