#include "libxmps.h"

char *rgb_to_yuv_y;
char *rgb_to_yuv_u;
char *rgb_to_yuv_v;

void libxmps_color_init()
{
  unsigned char r, g, b;

  rgb_to_yuv_y = (char *) malloc(64*64*64);
  rgb_to_yuv_u = (char *) malloc(64*64*64);
  rgb_to_yuv_v = (char *) malloc(64*64*64);

  //
  // YUV
  //

  for(r=0; r < 64; r++) {
    
    for(g=0; g < 64; g++) {
      
      for(b=0; b < 64; b++) {
	
	rgb_to_yuv_y[64*64*r + 64*g + b] = ( 0.2990*(double)r + 0.5870*(double)g + 0.1140*(double)b );
	rgb_to_yuv_u[64*64*r + 64*g + b] = ( 0.5000*b - 0.4187*g - 0.0813*r )  + 31;
	rgb_to_yuv_v[64*64*r + 64*g + b] = ( -0.1684*b - 0.3316*g + 0.5000*r ) + 31;

      }
    }
  }
}

void convert_RGB8_to_RGB16(unsigned char *src, 
			   xmps_rgb_color_t *palette, 
			   short *dest, 
			   unsigned int w, 
			   unsigned int h)
{
  register int i;
  register int j;

  if(palette != NULL)
    for(i=0; i < h; i++)
      {
	for(j=0; j<w; j++)
	  {
	    unsigned char q = *src++;
	    *dest++ =  ( (palette[q].b & 0xF8) >> 3)  | 
	               ( (palette[q].g & 0xFC) << 3 ) | 
	               ( (palette[q].r & 0xF8) << 8 );
	  }
      }  
}

void convert_RGB8_to_RGB24(unsigned char  *src, 
			   xmps_rgb_color_t *palette, 
			   unsigned char *dest, 
			   unsigned int w, 
			   unsigned int h)
{
  register int i;
  register int j;

  if(palette != NULL)
    for(i=0; i < h; i++)
      {
	for(j=0; j<w; j = j+=1)
	  {
	    unsigned char q = *src++;
	    *dest++ = palette[q].b;
	    *dest++ = palette[q].g ;
	    *dest++ = palette[q].r;
	  }
      }  
}

void convert_RGB8_to_RGB32(unsigned char *src, 
			   xmps_rgb_color_t *palette, 
			   unsigned int *dest, 
			   unsigned int w, 
			   unsigned int h)
{
  register int i;
  register int j;

  if(palette != NULL)
    for(i=0; i < h; i++)
      {
	for(j=0; j<w; j++)
	  {
	    unsigned char q = *src++;
	    *dest++ = (unsigned int)(( palette[q].r | 
				       palette[q].g << 8 | 
				       palette[q].b << 16 ) & 
				     0xFFFFFFFF);
	  }
      }  
}

void convert_RGB8_to_YUV12(unsigned char  *src, 
			   xmps_rgb_color_t *palette, 
			   unsigned char *dest, 
			   unsigned int w, 
			   unsigned int h)
{
  register int i;
  register int j;
  unsigned char r;
  unsigned char g;
  unsigned char b;
  unsigned long long index;

  if(src != NULL && dest != NULL)
    {
      for(i=0; i<h; i++)
	{
	  for(j=0; j<w; j++)
	    {
	      r = palette[*src].b;
	      g = palette[*src].g;
	      b = palette[*src++].r;

	      index = (64*64)*(r/4) + 64*(g/4) + (b/4);

	      *(((unsigned char **)dest)[0] + i*w + j) = rgb_to_yuv_y[index]*4;
	      
	      if(!(j%2) && !(i%2))
		{
		  *(((unsigned char **)dest)[1] + i/2*w/2 + j/2) = rgb_to_yuv_u[index]*4;
		  *(((unsigned char **)dest)[2] + i/2*w/2 + j/2) = rgb_to_yuv_v[index]*4;
		}
	    }
	}
    }
}

void convert_RGB24_to_RGB16(unsigned char *src,
                            short *dest,
                            unsigned int w,
                            unsigned int h)
{
  register int i;
  register int j;
  unsigned char r;
  unsigned char g;
  unsigned char b;

  if(src != NULL && dest != NULL)
    {
      for(i=0; i<h; i++)
        {
          for(j=0; j<w; j++)
            {
	      
              r = *src++;
              g = *src++;
              b = *src++;

              *dest++= ( (r & 0xF8) >> 3)  |
                       ( (g & 0xFC) << 3 ) |
                       ( (b & 0xF8) << 8 );
            }
        }
    }

}

void convert_RGB24_to_YUV12(unsigned char *src,
                            unsigned char *dest,
                            unsigned int w,
                            unsigned int h)
{
  register int  i;
  register int  j;
  unsigned long long index;

  if(src != NULL && dest != NULL)
    {
      for(i = 0; i < h; i++)
        {
          for(j = 0; j < w; j++)
            {
	      index = (64*64)*((*src)/4) + 64*((*(src+1))/4) + ((*(src+2))/4);
	      src += 3;

	      *(((unsigned char **)dest)[0] + i*w + j) = rgb_to_yuv_y[index]*4;
	      
	      if(!(j%2) && !(i%2)) {
		*(((unsigned char **)dest)[1] + i*w/4 + j/2) = rgb_to_yuv_u[index]*4;
		*(((unsigned char **)dest)[2] + i*w/4 + j/2) = rgb_to_yuv_v[index]*4;
	      }
            }
        }
    }  
}

/*
 * xmps_adapter_convert
 *
 * - the main conversion function
 *
 */

void xmps_video_conversion(xmps_video_format_t *src_format, 
			   xmps_video_format_t *dest_format,
			   xmps_video_info_t  *video_info,
			   void *src, 
			   void *dest)
{
  switch(src_format->type)
    {
    case XMPS_VIDEO_FORMAT_RGB24:
      switch(dest_format->type)
        {
        case XMPS_VIDEO_FORMAT_RGB16:
          convert_RGB24_to_RGB16((unsigned char *)src,
                                 (short *)dest,
                                 video_info->width,
                                 video_info->height);
          break;
        case XMPS_VIDEO_FORMAT_YUV12:

          convert_RGB24_to_YUV12((unsigned char *)src,
                                 (unsigned char *)dest,
                                 video_info->width,
                                 video_info->height);
          break;
        default:
          break;
        }
      break;
      
    case XMPS_VIDEO_FORMAT_RGB8:
      switch(dest_format->type)
	{
	case XMPS_VIDEO_FORMAT_RGB16:
	  convert_RGB8_to_RGB16((unsigned char *)src, 
				video_info->palette, 
				(short *)dest, 
				video_info->width, 
				video_info->height);
	  break;
	case XMPS_VIDEO_FORMAT_RGB24:
	  convert_RGB8_to_RGB24((unsigned char *)src, 
				video_info->palette, 
				(unsigned char *)dest, 
				video_info->width,
				video_info->height);
	  break;
	case XMPS_VIDEO_FORMAT_RGB32:
	  convert_RGB8_to_RGB32((unsigned char *)src, 
				video_info->palette, 
				(unsigned int *)dest, 
				video_info->width, 
				video_info->height);
	  break;
	case XMPS_VIDEO_FORMAT_YUV12:
	  convert_RGB8_to_YUV12((unsigned char *)src, 
				video_info->palette, 
				(unsigned char *)dest, 
				video_info->width,
				video_info->height);
	  break;
	default:
	  break;
	}
      break;




    default:
      break;
    }
}