/************************************************************************************
libsj
Copyright (c) 2010, Segway Japan, Ltd.
All rights reserved.

Contact us if you use this software for sell.
If you use this software not for sell, you can use this software under BSD lisence.
See the files LICENSE.TXT and LICENSE-BSD.TXT for more details.                     
************************************************************************************/
#ifndef __V4L2_H__
#define __V4L2_H__


#ifndef WIN32

class v4l2
{
  typedef enum {
    IO_METHOD_READ,
    IO_METHOD_MMAP,
    IO_METHOD_USERPTR,
  } io_method;

  struct buffer {
    void	*start;
    size_t	length;
  };

  char		dev_name[64];//	= NULL;
  int		fd;//		= -1;
  buffer	*buffers;//	= NULL;
  unsigned int	n_buffers;//	= 0;

  unsigned char* yuyv;
  unsigned char* rgb;
  unsigned char* xrgb;
  io_method     io;//	        = IO_METHOD_MMAP;


  int CAPTURE_WIDTH;
  int CAPTURE_HEIGHT;


  void Open_Device( void );
  void Close_Device( void );
  void Init_Device( void );
  void Init_userp( unsigned int );
  void Init_mmap( void );
  void Init_read( unsigned int );
  void UnInit_Device( void );
  void Start_Capture( void );
  void Stop_Capture( void );
  bool MainLoop( void );
  int Read_Frame( void );
  void Process_Image( const void * );
  int Xioctl( int, int, void * );
  void Errno_Exit( const char * );
  void GetImageMemory( void );

 public:

  v4l2();

  bool Capture();

  unsigned char* GetYUYVbufptr()
  {
    return yuyv;
  }
  
  unsigned char* GetRGBbufptr()
  {
    return rgb;
  }
  unsigned char* GetXRGBbufptr()
  {
    return xrgb;
  }

  void GetHSV(float* Hbuf, float* Sbuf, float* Vbuf);

  void Init(int w, int h, char* devname);
  void End();
};


#endif

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

inline void move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, int rowPixels, unsigned char * rgb)
{
    const int rvScale = 91881;
    const int guScale = -22553;
    const int gvScale = -46801;
    const int buScale = 116129;
    const int yScale  = 65536;
    int r, g, b;

    g = guScale * u + gvScale * v;
//  if (force_rgb) {
//      r = buScale * u;
//      b = rvScale * v;
//  } else {
#if 0
        r = rvScale * v;
        b = buScale * u;
#else
        b = rvScale * v;
        r = buScale * u;	
#endif
//  }

    yTL *= yScale; yTR *= yScale;
    yBL *= yScale; yBR *= yScale;

    /* Write out top two pixels */
    rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
    rgb[2] = LIMIT(r+yTL);

    rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
    rgb[5] = LIMIT(r+yTR);

    /* Skip down to next line to write out bottom two pixels */
    rgb += 3 * rowPixels;
    rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
    rgb[2] = LIMIT(r+yBL);

    rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
    rgb[5] = LIMIT(r+yBR);
}

inline void yuv420p_to_rgb24(int width, int height, unsigned char *pIn0, unsigned char *pOut0)
{


    const int numpix = width * height;
    const int bytes = 24 >> 3;
    int i, j, y00, y01, y10, y11, u, v;
    unsigned char *pY = pIn0;
    unsigned char *pU = pY + numpix;
    unsigned char *pV = pU + numpix / 4;
    unsigned char *pOut = pOut0;

    for (j = 0; j <= height - 2; j += 2) {
        for (i = 0; i <= width - 2; i += 2) {
            y00 = *pY;
            y01 = *(pY + 1);
            y10 = *(pY + width);
            y11 = *(pY + width + 1);
            u = (*pU++) - 128;
            v = (*pV++) - 128;

            move_420_block(y00, y01, y10, y11, u, v,
                       width, pOut);
    
            pY += 2;
            pOut += 2 * bytes;

        }
        pY += width;
        pOut += width * bytes;
    }
}


inline void yuyv_to_rgb24(int width, int height, unsigned char *src, unsigned char *dst)
{
#define SAT(c) \
        if (c & (~255)) { if (c < 0) c = 0; else c = 255; }


   unsigned char *s;
   unsigned char *d;
   int l, c;
   int r, g, b, cr, cg, cb, y1, y2;
   
   l = height;
   s = src;
   d = dst;
   while (l--) {
      c = width >> 1;
      while (c--) {
         y1 = *s++;
         cb = ((*s - 128) * 454) >> 8;
         cg = (*s++ - 128) * 88;
         y2 = *s++;
         cr = ((*s - 128) * 359) >> 8;
         cg = (cg + (*s++ - 128) * 183) >> 8;

         r = y1 + cr;
         b = y1 + cb;
         g = y1 - cg;
         SAT(r);
         SAT(g);
         SAT(b);
#if 0
	 *d++ = b;
	 *d++ = g;
	 *d++ = r;
#else
	 *d++ = r;
	 *d++ = g;
	 *d++ = b;
#endif
         r = y2 + cr;
         b = y2 + cb;
         g = y2 - cg;
         SAT(r);
         SAT(g);
         SAT(b);
#if 0
	 *d++ = b;
	 *d++ = g;
	 *d++ = r;
#else
	 *d++ = r;
	 *d++ = g;
	 *d++ = b;
#endif
      }
   }
} 


inline void rgb24_to_xrgb(int width, int height, unsigned char* rgb, unsigned char* xrgb)
{
  for (int i = 0; i < height; i++) {
    for (int j = 0; j < width; j++) {

      unsigned int pixeldib = 4*(j + i*width);
      unsigned int pixelbuf = 3*(j + i*width);

      xrgb[pixeldib + 0] = 0xFF;
      xrgb[pixeldib + 1] = rgb[pixelbuf + 0];
      xrgb[pixeldib + 2] = rgb[pixelbuf + 1];
      xrgb[pixeldib + 3] = rgb[pixelbuf + 2];
    }
  }
}




#endif
