#include "image.h"

#include <cstdlib>
#include <cstdio>
#include <cmath>

namespace seemicro
{

extern int DEBUGLEVEL;

void rgb2hsv (int rgbIn, unsigned short*
piOutHue, unsigned short* piOutSaturation, unsigned short*
piOutValue)
{
   unsigned char red = (rgbIn >> 16) & 255;
   unsigned char green = (rgbIn >> 8) & 255;
   unsigned char blue = rgbIn & 255;

   short  iMax, iMin;
   // Calculate the value component
   if (red > green) {
      iMax = red;
      iMin = green;
   } else {
      iMin = red;
      iMax = green;
   }
   if (blue > iMax)
      iMax = blue;
   if (blue < iMin)
      iMin = blue;
   *piOutValue = iMax;
   // Calculate the saturation component
   if (iMax != 0) {
      *piOutSaturation = 255 *
          (iMax - iMin) / iMax;
   } else {
      *piOutSaturation = 0;
   }
   // Calculate the hue
   if (*piOutSaturation == 0) {
      *piOutHue = 0;// kHueUndefined
   } else {
      float   fHue;
      float   fDelta;
      fDelta = iMax - iMin;
      if (red == iMax) {
          fHue = (float)(green -
              blue) / fDelta;
      } else if (green == iMax) {
          fHue = 2.0 + (blue -
              red) / fDelta;
      } else {
          fHue = 4.0 + (red -
              green) / fDelta;
      }
      fHue *= 60.0;
      if (fHue < 0)
          fHue += 360;
      *piOutHue = (short)fHue;
  }
}


int hsv2rgb(int h, unsigned char s, unsigned char v)
{
  if(s==0) return (v<<8 | v)<<8 | v;
  //if(h>360)
  h = h % 360;
  float hd = h / 60.0;// h is now IN [0,6)
  unsigned char R,G,B,i;
  i = (unsigned char) hd;// largest integer <= h
  float dummy;
  float f = modff(hd,&dummy);// fractional part of h
  float S = s/255.0;

  unsigned char p = (unsigned char)(v * (1.0 - S));
  unsigned char q = (unsigned char)(v * (1.0 - (S * f)));
  unsigned char t = (unsigned char)(v * (1.0 - (S * (1.0 - f))));

  switch(i)
  {
    case 0: R = v; G = t; B = p;break;
    case 1: R = q; G = v; B = p;break;
    case 2: R = p; G = v; B = t;break;
    case 3: R = p; G = q; B = v;break;
    case 4: R = t; G = p; B = v;break;
    case 5: R = v; G = p; B = q;break;
    default: abort();
  }

//   fprintf(stderr, "hsv: %i %i %i rgb: %i %i %i\n",h,s,v,R,G,B);
  return (R<<8 | G)<<8 | B;
}

 /* Turn a YUV4:2:0 block into an RGB block
  *
  * Video4Linux seems to use the blue, green, red channel
  * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
  *
  * Color space conversion coefficients taken from the excellent
  * http://www.inforamp.net/~poynton/ColorFAQ.html
  * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
  * Y values are given for all 4 pixels, but the U (Pb)
  * and V (Pr) are assumed constant over the 2x2 block.
  *
  * To avoid floating point arithmetic, the color conversion
  * coefficients are scaled into 16.16 fixed-point integers.
  * They were determined as follows:
  *
  *      double brightness = 1.0;  (0->black; 1->full scale)
  *      double saturation = 1.0;  (0->greyscale; 1->full color)
  *      double fixScale = brightness * 256 * 256;
  *      int rvScale = (int)(1.402 * saturation * fixScale);
  *      int guScale = (int)(-0.344136 * saturation * fixScale);
  *      int gvScale = (int)(-0.714136 * saturation * fixScale);
  *      int buScale = (int)(1.772 * saturation * fixScale);
  *      int yScale = (int)(fixScale);
  */

 /* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
int LIMIT(int x)
{
    return ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)));
}

int YuvToRgb(int yuv)
{
     const int rvScale = 91881;
     const int guScale = -22553;
     const int gvScale = -46801;
     const int buScale = 116129;
     const int yScale  = 65536;

     int y=0xff&(yuv>>16);
     int u=0xff&(yuv>>8);
     int v=0xff&(yuv);
     int r, g, b;
     u-=128;
     v-=128;
     g = guScale * u + gvScale * v;
     r = rvScale * v;
     b = buScale * u;
     y *= yScale;
     return (((LIMIT(r+y)<<8) | LIMIT(g+y))<<8) | LIMIT(b+y);
}

void yuv420p_unpack_2_rgb24(const unsigned char *src, char *dst,
 int width, int height)
{
#ifdef DEBUG
  if(DEBUGLEVEL>=3) fprintf(stderr, "unpacking...\n");
#endif
  const unsigned char *Y = (const unsigned char *) src;// start of Y plane
  const unsigned char *U = Y+width*height;// U plane
  const unsigned char *V = U+width*height/4;// V plane

  // braucht width*height* (1 + 1/4 + 1/4)
  // fprintf(stderr, "%hhd %hhd %hhd", *Y, *U, *V);
  int i,j;// this is no C99 code

  for(i=0;i<height/2;++i) // alle 2 zeilen
  {
    for(j=0;j<width/2;++j) // erste zeile
    {
      int x = YuvToRgb( (((*Y << 8) | *U ) << 8) | *V );
      *dst++ = x & 255;
      *dst++ = (x>>8) & 255;
      *dst++ = (x>>16) & 255;
      Y++;
      x = YuvToRgb( (((*Y << 8) | *U ) << 8) | *V );
      *dst++ = x & 255;
      *dst++ = (x>>8) & 255;
      *dst++ = (x>>16) & 255;
      Y++;
      U++;
      V++;
    }
    U-=width/2;
    V-=width/2;

    for(j=0;j<width/2;++j) // zweite zeile
    {
      int x = YuvToRgb( (((*Y << 8) | *U ) << 8) | *V );
      *dst++ = x & 255;
      *dst++ = (x>>8) & 255;
      *dst++ = (x>>16) & 255;
      Y++;
      x = YuvToRgb( (((*Y << 8) | *U ) << 8) | *V );
      *dst++ = x & 255;
      *dst++ = (x>>8) & 255;
      *dst++ = (x>>16) & 255;
      Y++;
      U++;
      V++;
    }
  }
}

void yuv420p_unpack_shrink_halfsize(char *src, char *dest, int width, int height)
{
  const unsigned char *Y = (const unsigned char*) src;
  const unsigned char *U = Y+width*2*height*2;
  const unsigned char *V = U+width*2*height*2/4;
  int *dst = (int*) dest;
  int i,j;
  for(i=0;i<height;++i)
  {
    for(j=0;j<width;++j)
    {
      int y = ((*Y)+(*(Y+1))+(*(Y+width*2))+(*(Y+width*2+1)))>>2;
      *dst = (((y << 8) | (*U) ) << 8) | (*V);
      Y++;Y++;
      U++;
      V++;
      dst++;
    }
    Y+=width*2;
  }
}

void convertYuvToRgb(char *image, int width, int height)
{
  int *b = (int*) image;
  int i;
  for(i=0;i<width*height;++i)
  {
    *b=YuvToRgb(*b);
    ++b;
  }
}

/*
void scale(const char *src,int width, int height)
{
     int* x_limits = new int[width+1];
     int* y_limits = new int[height+1];

     x_limits[0]=0;
     for(int z=1;z<width;z++)
         x_limits[z]=(int)(z/double(width)*src.getWidth() + 0.5);
     x_limits[width]=src.getWidth();

     y_limits[0]=0;
     {for(int z=1;z<height;z++)
                 y_limits[z]=(int)(z/double(height)*src.getHeight() + 0.5);}
     y_limits[height]=src.getHeight();

     unsigned char* src_buffer=src.bits();
     for(int x=0;x<width;++x)
         for(int y=0;y<height;++y){
             int sum[4]={0,0,0,0};
             int number_pixels =0;
             for(int i=x_limits[x];i<x_limits[x+1];++i)
                 for(int j=y_limits[y];j<y_limits[y+1];++j){
                     unsigned char* pixel=&(src_buffer[(j*src.getWidth()+i)*4]);
                     sum[0]+=pixel[0];
                     sum[1]+=pixel[1];
                     sum[2]+=pixel[2];
                     sum[3]+=pixel[3];
                     number_pixels++;
                 }
             buffer[(y*width+x)*4]=(number_pixels>0)?sum[0]/number_pixels:0;
             buffer[(y*width+x)*4+1]=(number_pixels>0)?sum[1]/number_pixels:0;
             buffer[(y*width+x)*4+2]=(number_pixels>0)?sum[2]/number_pixels:0;
             buffer[(y*width+x)*4+3]=(number_pixels>0)?sum[3]/number_pixels:0;
         }

         delete x_limits;
         delete y_limits;
}
*/

} // namespace seemicro
