void write_pixel32(FILE *f, uint32_t pixel32, rgb_map_t map)
{
  unsigned char *pixels = (unsigned char *)&pixel32;
  if(!f) return;
  fputc(pixels[map.r], f);
  fputc(pixels[map.g], f);
  fputc(pixels[map.b], f);
}


uint32_t yuv2rgb(int y, int u, int v)
{
   uint32_t pixel32;
   unsigned char *pixel = (unsigned char *)&pixel32;
   int r, g, b;
   
   /*
     Another formula I found:  (seems to work)
     
     R = Y + 1.370705 (V-128)
     G = Y - 0.698001 (V-128) - 0.337633 (U-128)
     B = Y + 1.732446 (U-128)
   */
   
   r = (int) y + (1.370705 * (v-128));
   g = (int)  y - (0.698001 * (v-128)) - (0.337633 * (u-128));
   b = (int)  y + (1.732446 * (u-128));

   /* Even with proper conversion, some values still need clipping. */
   if (r > 255) r = 255;
   if (g > 255) g = 255;
   if (b > 255) b = 255;
   if (r < 0) r = 0;
   if (g < 0) g = 0;
   if (b < 0) b = 0;

   /* Values only go from 0-220..  Why? */
   pixel[0] = r * 220 / 256;
   pixel[1] = g * 220 / 256;
   pixel[2] = b * 220 / 256;
   pixel[3] = 0;

   /* Debug
   printf("yuv2rgb(%i, %i, %i) -> %i, %i, %i  (0x%x)\n",
	  y, u, v,
	  pixel[0], pixel[1], pixel[2],
	  pixel32);
   */
   
   return pixel32;
}


void yuv420p_to_ppm(FILE * fpin, FILE * fpout)
{
	uint32_t pixel32;
	unsigned char *buffer, *src;
	int depth = 24;
	int y=0, u=0, v=0;
	int i=0;
	rgb_map_t rgb_map = { 0, 1, 2 };
	rewind(fpout);
	struct timeval tvstart;
	struct timeval tvfinish;
  
	gettimeofday(&tvstart, NULL); 
	
	buffer = (unsigned char *) malloc((g_win.width * g_win.height * depth));

	// This should go into another function
	i = 0;
	size_t bytesread;
	
	read(g_fdin, buffer, g_win.width * g_win.height * depth);
        gettimeofday(&tvfinish, NULL); 
        fprintf(stderr, "capture in:%ld ms\n", (tvfinish.tv_usec - tvstart.tv_usec)/1000);
	
	
	src = buffer;
	
	fprintf(fpout, "P6\n%d %d 255\n", g_win.width, g_win.height);
	
	for (i = 0; i < g_win.width * g_win.height; i++) 
	{
		if(i == 0) 
		{
			src = buffer + (g_win.width*g_win.height);

		}
		y = buffer[i];
		u = src[(i/2)%(g_win.width/2)];
		v = src[(g_win.width*g_win.height)/4 + (i/2)%(g_win.width/2)];
		if(i && (i%(g_win.width*4)) == 0) 
		{
			src += g_win.width;

		}
		pixel32 = yuv2rgb(y, u, v);
		write_pixel32(fpout, pixel32, rgb_map);
	}

	free(buffer);
	rewind(fpout);
        gettimeofday(&tvfinish, NULL); 
        //fprintf(stderr, "conversion in:%ld ms\n", (tvfinish.tv_usec - tvstart.tv_usec)/1000);
	
	
}