static void yuv2rgb(int y, int u, int v, char *r, char *g, char *b) {
	int r1, g1, b1;
	int c = y - 16, d = u - 128, e = v - 128;

	r1 = (298 * c + 409 * e + 128) >> 8;
	g1 = (298 * c - 100 * d - 208 * e + 128) >> 8;
	b1 = (298 * c + 516 * d + 128) >> 8;

	if (r1 > 255)
		r1 = 255;
	if (g1 > 255)
		g1 = 255;
	if (b1 > 255)
		b1 = 255;
	if (r1 < 0)
		r1 = 0;
	if (g1 < 0)
		g1 = 0;
	if (b1 < 0)
		b1 = 0;

	*r = r1;
	*g = g1;
	*b = b1;
}

void yuyv_to_rgb(rgb_ptr buffer_yuyv, rgb_ptr buffer_rgb, int width, int height)
{
	rgb_ptr pixel_16;   // for YUYV
	rgb_ptr pixel_24;	// for RGB
	int y, u, v, y2;
	char r, g, b;
	if ( buffer_yuyv == NULL || buffer_rgb == NULL)
	return;

	pixel_16 = (rgb_ptr)buffer_yuyv; //width * height * 2
	pixel_24 = buffer_rgb;	//width * height * 3

	int i = 0, j = 0;
	while ((i + 2) < height * width * 2)
	{
		v = pixel_16[i];
		y = pixel_16[i+1];
		u = pixel_16[i+2];
		y2 = pixel_16[i+3];

		yuv2rgb(y, u, v, &r, &g, &b);// 1st pixel

		pixel_24[j] = r;
		pixel_24[j + 1] = g;
		pixel_24[j + 2] = b;
		j+=BPP_RGB24;

		yuv2rgb(y2, u, v, &r, &g, &b);// 2nd pixel

		pixel_24[j] = r;
		pixel_24[j + 1] = g;
		pixel_24[j + 2] = b;
		j+=BPP_RGB24;
		i += BPP_YUY2;
	}
}

static const int BPP_YUY2 = 2;
static const int BPP_YUY2_PIXEL = 4;
static const int BPP_RGB24 = 3;

char clip(int x){
	//return x > 255 ? x : x < 0 ? 0 : x;
	if (x>255)
		return 255;
	if (x<0)
		return 0;
	return x;
}

static void yuv2rgb(int y, int u, int v, char *r, char *g, char *b) {
	int c = y - 16, d = u - 128, e = v - 128;

	*r = clip(1.164383 * c + 1.596027 * e);
	*g = clip(1.164383 * c - (0.391762 * d) - (0.812968 * e));
	*b = clip(1.164383 * c + 2.017232 * d);

}

void yuyv_to_rgb(rgb_ptr buffer_yuyv, rgb_ptr buffer_rgb, int width, int height)
{
	rgb_ptr pixel_16;   // for YUYV
	rgb_ptr pixel_24;// for RGB
	int y0, u0, v0, y1;
	char r, g, b;
	if ( buffer_yuyv == NULL || buffer_rgb == NULL)
	return;

	pixel_16 = (rgb_ptr)buffer_yuyv;//width * height * 2
	pixel_24 = buffer_rgb;//width * height * 3

	int i = 0, j = 0;
	while ((i + 2) < height * width * 2)
	{
		y0 = pixel_16[i];
		u0 = pixel_16[i+1];
		y1 = pixel_16[i+2];
		v0 = pixel_16[i+3];

		yuv2rgb(y0, u0, v0, &r, &g, &b);	// 1st pixel

		pixel_24[j] = r;
		pixel_24[j + 1] = g;
		pixel_24[j + 2] = b;
		j+=BPP_RGB24;

		yuv2rgb(y1, u0, v0, &r, &g, &b);// 2nd pixel

		pixel_24[j] = r;
		pixel_24[j + 1] = g;
		pixel_24[j + 2] = b;
		j+=BPP_RGB24;
		i += BPP_YUY2_PIXEL;
	}
}