#include "colourspace.h"

namespace mimas {

#define Y_X_FACTOR  1.1644
#define V_R_FACTOR  1.5960
#define U_G_FACTOR -0.3918
#define V_G_FACTOR -0.8130
#define U_B_FACTOR  2.0172


#define V_R_FACTOR_P  1.402
#define U_G_FACTOR_P -0.344136
#define V_G_FACTOR_P -0.714136
#define U_B_FACTOR_P  1.772

unsigned char clip_colour_cell( int x ) {
  return x > 0xffffff ? 0xff : ( x <= 0xffff ? 0x0 : x >> 16 );
};

void rgb_to_rgba( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
#ifndef NDEBUG
  std::cerr << "RGB to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif
  int numpix = width * height;
  unsigned char *p = (unsigned char *)out;
  const unsigned char *q = (const unsigned char *)in;
  for ( int i=0; i<numpix; i++ ) {
    *p++ = *q++;
    *p++ = *q++;
    *p++ = *q++;
    *p++ = 0;
  };
}

void yv12_to_rgba( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
  assert( width % 2 == 0 );
  assert( height % 2 == 0 );
#ifndef NDEBUG
  std::cerr << "YV12 to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif
 
  const unsigned char
    *src_y = (const unsigned char *)in;
  const signed char
    *src_u = (const signed char *)in + width * height,
    *src_v = src_u + ( width / 2 ) * ( height / 2 );

  unsigned char
    *out1 = (unsigned char *)out,
    *out2 = (unsigned char *)out + ( width << 2 );
 
  // R' = [ 1.1644         0    1.5960 ]   ([ Y' ]   [  16 ])
  // G' = [ 1.1644   -0.3918   -0.8130 ] * ([ Cb ] - [ 128 ])
  // B' = [ 1.1644    2.0172         0 ]   ([ Cr ]   [ 128 ])
  
  const int
    yxScale = (int)( Y_X_FACTOR * 65536 ),
    vrScale = (int)( V_R_FACTOR * 65536 ),
    ugScale = (int)( U_G_FACTOR * 65536 ),
    vgScale = (int)( V_G_FACTOR * 65536 ),
    ubScale = (int)( U_B_FACTOR * 65536 );

  for ( int y=0; y<height; y+=2 ) {
 
    for ( int x=0; x<width; x+=2 ) {
 
      int
        ys0int = ( src_y[ 0         ] - 16 ) * yxScale,
        ys1int = ( src_y[ 1         ] - 16 ) * yxScale,
        ys2int = ( src_y[ width     ] - 16 ) * yxScale,
        ys3int = ( src_y[ width + 1 ] - 16 ) * yxScale;

      signed char
        cb = *src_u ^ 0x80,
        cr = *src_v ^ 0x80;
      
      int
        rint =                vrScale * cr,
        gint = ugScale * cb + vgScale * cr,
        bint = ubScale * cb;
      
      out1[ 0 ] = clip_colour_cell( ys0int + bint );
      out1[ 1 ] = clip_colour_cell( ys0int + gint );
      out1[ 2 ] = clip_colour_cell( ys0int + rint );
      out1[ 3 ] = 0;
      out1[ 4 ] = clip_colour_cell( ys1int + bint );
      out1[ 5 ] = clip_colour_cell( ys1int + gint );
      out1[ 6 ] = clip_colour_cell( ys1int + rint );
      out1[ 7 ] = 0;
      out2[ 0 ] = clip_colour_cell( ys2int + bint );
      out2[ 1 ] = clip_colour_cell( ys2int + gint );
      out2[ 2 ] = clip_colour_cell( ys2int + rint );
      out2[ 3 ] = 0;
      out2[ 4 ] = clip_colour_cell( ys3int + bint );
      out2[ 5 ] = clip_colour_cell( ys3int + gint );
      out2[ 6 ] = clip_colour_cell( ys3int + rint );
      out2[ 7 ] = 0;

      src_y += 2;
      src_u++;
      src_v++;

      out1 += 8;
      out2 += 8;
 
    }

    src_y += width;
    out1 += width << 2;
    out2 += width << 2;

  }
 
}

void rgba_to_yv12( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
  assert( width % 2 == 0 );
  assert( height % 2 == 0 );
#ifndef NDEBUG
  std::cerr << "YV12 to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif
 
  const unsigned char
    *in1 = (const unsigned char *)in,
    *in2 = (const unsigned char *)in + ( width << 2 );

  unsigned char
    *out_y = (unsigned char *)out;
  signed char
    *out_u = (signed char *)out + width * height,
    *out_v = out_u + ( width / 2 ) * ( height / 2 );

  // Y =    = [  0.257    0.504    0.098 ]   [ R ]   [  16 ]
  // Cb = U = [ -0.148   -0.291    0.439 ] * [ G ] + [ 128 ]
  // Cr = V = [  0.497   -0.368   -0.071 ]   [ B ]   [ 128 ]
  
  const int
    yrScale = (int)(  0.257 * 65536 ),
    ygScale = (int)(  0.504 * 65536 ),
    ybScale = (int)(  0.098 * 65536 ),
    brScale4 = (int)( -0.148 * 65536 / 4 ),
    bgScale4 = (int)( -0.291 * 65536 / 4 ),
    ubScale4 = (int)(  0.439 * 65536 / 4 ),
    vrScale4 = (int)(  0.497 * 65536 / 4 ),
    rgScale4 = (int)( -0.368 * 65536 / 4 ),
    rbScale4 = (int)( -0.071 * 65536 / 4 ),
    yOffset =  16 * 65536,
    rOffset = 128 * 65536,
    bOffset = 128 * 65536;

  for ( int y=0; y<height; y+=2 ) {
 
    for ( int x=0; x<width; x+=2 ) {
 
      out_y[ 0         ] = clip_colour_cell( in1[ 2 ] * yrScale +
                                             in1[ 1 ] * ygScale +
                                             in1[ 0 ] * ybScale + yOffset );
      out_y[ 1         ] = clip_colour_cell( in1[ 6 ] * yrScale +
                                             in1[ 5 ] * ygScale +
                                             in1[ 4 ] * ybScale + yOffset );
      out_y[ width     ] = clip_colour_cell( in2[ 2 ] * yrScale +
                                             in2[ 1 ] * ygScale +
                                             in2[ 0 ] * ybScale + yOffset ),
      out_y[ width + 1 ] = clip_colour_cell( in2[ 6 ] * yrScale +
                                             in2[ 5 ] * ygScale +
                                             in2[ 4 ] * ybScale + yOffset );
      int
        ravg4 = in1[ 2 ] + in1[ 6 ] + in2[ 2 ] + in2[ 6 ],
        gavg4 = in1[ 1 ] + in1[ 5 ] + in2[ 1 ] + in2[ 5 ],
        bavg4 = in1[ 0 ] + in1[ 4 ] + in2[ 0 ] + in2[ 4 ];

      *out_u = (signed char)clip_colour_cell( ravg4 * brScale4 +
                                              gavg4 * bgScale4 +
                                              bavg4 * ubScale4 +
                                              rOffset );
      *out_v = (signed char)clip_colour_cell( ravg4 * vrScale4 +
                                              gavg4 * rgScale4 +
                                              bavg4 * rbScale4 +
                                              bOffset );

      out_y += 2;
      out_u++;
      out_v++;

      in1 += 8;
      in2 += 8;
 
    }

    out_y += width;
    in1 += width << 2;
    in2 += width << 2;

  }
 
}

void uyvy_to_rgba( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
  assert( width % 2 == 0 );
  assert( height % 2 == 0 );
#ifndef NDEBUG
  std::cerr << "UYVY to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif

  // R' = [ 1.1644         0    1.5960 ]   ([ Y' ]   [  16 ])
  // G' = [ 1.1644   -0.3918   -0.8130 ] * ([ Cb ] - [ 128 ])
  // B' = [ 1.1644    2.0172         0 ]   ([ Cr ]   [ 128 ])

  const int
    yxScale = (int)( Y_X_FACTOR * 65536 ),
    vrScale = (int)( V_R_FACTOR * 65536 ),
    ugScale = (int)( U_G_FACTOR * 65536 ),
    vgScale = (int)( V_G_FACTOR * 65536 ),
    ubScale = (int)( U_B_FACTOR * 65536 );
  
  for ( int y=0; y<height; y++ ) {
          
    for ( int x=0; x<width; x+=2 ) {

      int
        ys0int = ( ((const unsigned char *)in)[ 1 ] - 16 ) * yxScale,
        ys1int = ( ((const unsigned char *)in)[ 3 ] - 16 ) * yxScale;

      signed char
        cb = ((const signed char *)in)[0] ^ 0x80,
        cr = ((const signed char *)in)[2] ^ 0x80;

      int
        rint =                vrScale * cr,
        gint = ugScale * cb + vgScale * cr,
        bint = ubScale * cb;

      out[ 0 ] = clip_colour_cell( ys0int + bint );
      out[ 1 ] = clip_colour_cell( ys0int + gint );
      out[ 2 ] = clip_colour_cell( ys0int + rint );
      out[ 3 ] = 0;
      out[ 4 ] = clip_colour_cell( ys1int + bint );
      out[ 5 ] = clip_colour_cell( ys1int + gint );
      out[ 6 ] = clip_colour_cell( ys1int + rint );
      out[ 7 ] = 0;

      in += 4;
      out += 8;
    };
  };
}

void yuy2_to_rgba( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
  assert( width % 4 == 0 );
#ifndef NDEBUG
  std::cerr << "YUY2 to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif

  // R' = [ 1.1644         0    1.5960 ]   ([ Y' ]   [  16 ])
  // G' = [ 1.1644   -0.3918   -0.8130 ] * ([ Cb ] - [ 128 ])
  // B' = [ 1.1644    2.0172         0 ]   ([ Cr ]   [ 128 ])
  
  const int
    yxScale = (int)( Y_X_FACTOR * 65536 ),
    vrScale = (int)( V_R_FACTOR * 65536 ),
    ugScale = (int)( U_G_FACTOR * 65536 ),
    vgScale = (int)( V_G_FACTOR * 65536 ),
    ubScale = (int)( U_B_FACTOR * 65536 );

  for ( int y=0; y<height; y++ ) {
 
    for ( int x=0; x<width; x+=2 ) {
 
      int
        ys0int = ( ((const unsigned char *)in)[ 0 ] - 16 ) * yxScale,
        ys1int = ( ((const unsigned char *)in)[ 2 ] - 16 ) * yxScale;

      signed char
        cb = ((const signed char *)in)[ 1 ] ^ 0x80,
        cr = ((const signed char *)in)[ 3 ] ^ 0x80;

      int
        rint =                vrScale * cr,
        gint = ugScale * cb + vgScale * cr,
        bint = ubScale * cb;
      
      out[ 0 ] = clip_colour_cell( ys0int + bint );
      out[ 1 ] = clip_colour_cell( ys0int + gint );
      out[ 2 ] = clip_colour_cell( ys0int + rint );
      out[ 3 ] = 0;
      out[ 4 ] = clip_colour_cell( ys1int + bint );
      out[ 5 ] = clip_colour_cell( ys1int + gint );
      out[ 6 ] = clip_colour_cell( ys1int + rint );
      out[ 7 ] = 0;

      in += 4;
      out += 8; 
    }

  }
 
}

void yuv420p_to_rgba( const char *in, int width, int height, char *out )
{
  // Direct access to image-memory!
  assert( out != NULL );
  assert( width % 2 == 0 );
  assert( height % 2 == 0 );
#ifndef NDEBUG
  std::cerr << "YV12 to RGBA for " << width << "x" << height << "-image."
            << std::endl;
#endif
 
  const unsigned char
    *src_y = (const unsigned char *)in;
  const signed char
    *src_u = (const signed char *)in + width * height,
    *src_v = src_u + ( width / 2 ) * ( height / 2 );

  unsigned char
    *out1 = (unsigned char *)out,
    *out2 = (unsigned char *)out + ( width << 2 );
 
  const int
    vrScale = (int)( V_R_FACTOR_P * 65536 ),
    ugScale = (int)( U_G_FACTOR_P * 65536 ),
    vgScale = (int)( V_G_FACTOR_P * 65536 ),
    ubScale = (int)( U_B_FACTOR_P * 65536 );

  for ( int y=0; y<height; y+=2 ) {
 
    for ( int x=0; x<width; x+=2 ) {
 
      int
        ys0int = src_y[ 0         ] << 16,
        ys1int = src_y[ 1         ] << 16,
        ys2int = src_y[ width     ] << 16,
        ys3int = src_y[ width + 1 ] << 16;

      signed char
        cb = *src_u ^ 0x80,
        cr = *src_v ^ 0x80;
      
      int
        rint =                vrScale * cr,
        gint = ugScale * cb + vgScale * cr,
        bint = ubScale * cb;
      
      out1[ 0 ] = clip_colour_cell( ys0int + bint );
      out1[ 1 ] = clip_colour_cell( ys0int + gint );
      out1[ 2 ] = clip_colour_cell( ys0int + rint );
      out1[ 3 ] = 0;
      out1[ 4 ] = clip_colour_cell( ys1int + bint );
      out1[ 5 ] = clip_colour_cell( ys1int + gint );
      out1[ 6 ] = clip_colour_cell( ys1int + rint );
      out1[ 7 ] = 0;
      out2[ 0 ] = clip_colour_cell( ys2int + bint );
      out2[ 1 ] = clip_colour_cell( ys2int + gint );
      out2[ 2 ] = clip_colour_cell( ys2int + rint );
      out2[ 3 ] = 0;
      out2[ 4 ] = clip_colour_cell( ys3int + bint );
      out2[ 5 ] = clip_colour_cell( ys3int + gint );
      out2[ 6 ] = clip_colour_cell( ys3int + rint );
      out2[ 7 ] = 0;

      src_y += 2;
      src_u++;
      src_v++;

      out1 += 8;
      out2 += 8;
 
    }

    src_y += width;
    out1 += width << 2;
    out2 += width << 2;

  }
 
}

};