/************************************************************************************
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.                     
************************************************************************************/

#include <X11/Xlib.h> 
#include <X11/Xutil.h> 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <getopt.h>			// getopt_long()
#include <fcntl.h>			// low-level I/O
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <asm/types.h>		// videodev2.h
#include <linux/videodev2.h>
#include <jpeglib.h>		// JPEG Library
#include <pthread.h>



#include "v4l2.h"

#define         PIXELFORMAT		V4L2_PIX_FMT_YUYV		// QCAM-200V
//#define	PIXELFORMAT		V4L2_PIX_FMT_YUV420		// BT878A
//#define	PIXELFORMAT		V4L2_PIX_FMT_BGR24              // BT878A

#define CONVERT_TO_RGB24    yuyv_to_rgb24


#define	CLEAR( x )		memset( &(x), 0, sizeof (x) )



v4l2::v4l2()
{
  dev_name[0] = 0;
  fd        = -1;
  buffers   = NULL;
  n_buffers = 0;
  yuyv      = NULL;
  rgb       = NULL;
  xrgb      = NULL;
  io        = IO_METHOD_MMAP;
  
  CAPTURE_WIDTH  = 320;
  CAPTURE_HEIGHT = 240;
}


void v4l2::Errno_Exit( const char *s )
{
  fprintf(stderr, "%s error %d, %s\n", s, errno, strerror (errno));
  exit( EXIT_FAILURE );
}


int v4l2::Xioctl( int fd, int request, void *arg )
{
  int r;
  
  do {
    r = ioctl( fd, request, arg );
  }
  while ( -1 == r && EINTR == errno );
  
  return r;
}




void v4l2::Process_Image( const void *p )
{
  yuyv = (unsigned char*) p;
  CONVERT_TO_RGB24(CAPTURE_WIDTH, CAPTURE_HEIGHT, (unsigned char*)p, (unsigned char*)rgb);
  rgb24_to_xrgb(   CAPTURE_WIDTH, CAPTURE_HEIGHT, (unsigned char*)rgb, (unsigned char*)xrgb);
}



int v4l2::Read_Frame( void )
{
  unsigned int			i;
  struct v4l2_buffer		buf;

  switch ( io ) {
  case IO_METHOD_READ:
    if ( -1 == read(fd, buffers[0].start, buffers[0].length) )
      {
	switch ( errno )
	  {
	  case EAGAIN:
	    return 0;
	  case EIO:
	    // Could ignore EIO, see spec.
	    // fall through
	  default:
	    Errno_Exit("read");
	  }
      }
    Process_Image( buffers[0].start );
    break;
  case IO_METHOD_MMAP:
    CLEAR( buf );
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    if ( -1 == Xioctl(fd, VIDIOC_DQBUF, &buf) )
      {
	switch ( errno )
	  {
	  case EAGAIN:
	    return 0;
	  case EIO:
	    // Could ignore EIO, see spec.
	    // fall through
	  default:
	    Errno_Exit("VIDIOC_DQBUF");
	  }
      }
    assert( buf.index < n_buffers );
    Process_Image( buffers[buf.index].start );
    if ( -1 == Xioctl(fd, VIDIOC_QBUF, &buf) )
      {
	Errno_Exit("VIDIOC_QBUF");
      }
    break;
  case IO_METHOD_USERPTR:
    CLEAR( buf );
    buf.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory	= V4L2_MEMORY_USERPTR;
    if ( -1 == Xioctl(fd, VIDIOC_DQBUF, &buf) )
      {
	switch ( errno )
	  {
	  case EAGAIN:
	    return 0;
	  case EIO:
	    // Could ignore EIO, see spec.
	    // fall through
	  default:
	    Errno_Exit("VIDIOC_DQBUF");
	  }
      }
    for (i = 0; i < n_buffers; ++i)
      {
	if ( buf.m.userptr == (unsigned long) buffers[i].start && buf.length == buffers[i].length )
	  {
	    break;
	  }
      }
    assert( i < n_buffers );
    Process_Image( (void *) buf.m.userptr );
    if ( -1 == Xioctl(fd, VIDIOC_QBUF, &buf) )
      {
	Errno_Exit("VIDIOC_QBUF");
      }
    break;
  }
  
  return 1;
}


bool v4l2::Capture()
{
    {
      fd_set		fds;
      struct timeval	tv;
      int		r;
      
      FD_ZERO( &fds );
      FD_SET( fd, &fds );
      // Timeout
      tv.tv_sec = 4;
      tv.tv_usec = 0;
      
      r = select( fd + 1, &fds, NULL, NULL, &tv );
      if ( r == -1 )
	{
	  if (EINTR == errno)
	    {
	      return true;
	    }
	  Errno_Exit("select");
	}
      if ( r == 0 )
	{
	  fprintf(stderr, "select timeout\n");
	  exit( EXIT_FAILURE );
	}
      
      if ( Read_Frame() )
	{
	  return true;
	}
      // EAGAIN - continue select loop
    }

    //sleep(1);
    //pthread_yield();

    //usleep(100*1000 * 1000); 

    return true;
}





//--------------------------------
// HSV Converter
//--------------------------------
inline float Max(float a,float b,float c)
{
  if(a >= b && a >= c)
    return a;
  if(b >= a && b >= c)
    return b;
  return c;
}

inline float Min(float a,float b,float c)
{
  if(a <= b && a <= c)
    return a;
  if(b <= a && b <= c)
    return b;
  return c;
}

inline void RGBtoHSV(unsigned int* xrgb, float& H, float& S, float& V)
{
  float	R;
  float	G;
  float	B;
  float	max;
  float	min;
  float	diff;

  //RGB->HSV変換時のR,G,Bは 0.0～1.0
  R = (float) (((*xrgb) & 0x0000FF00) >> 8);
  G = (float) (((*xrgb) & 0x00FF0000) >> 16);
  B = (float) (((*xrgb) & 0xFF000000) >> 24);

  R /= 255.0f;
  G /= 255.0f;
  B /= 255.0f;

  max = Max(R,G,B);
  min = Min(R,G,B);

  if (max == 0) {
      S = 0.0f;
      V = 0.0f;
      return;
  }

  diff = max - min;
  V = max;
  S = diff / max;
  //dS = dMax - dMin;			//円錐色空間時のS

  if (diff == 0) {
    H = 0.0f;
    return;
  }

  if(R == max) {
    H = 60.0f * (G - B) / diff;
  }
  else if(G == max) {
    H = 60.0f * (B - R) / diff + 120.0f;
  }
  else {
    H = 60.0f * (R - G) / diff + 240.0f;
  }

  if (H < 0.0f) {
    H = H + 360.0f;
  }
}


void v4l2::GetHSV(float* Hbuf, float* Sbuf, float* Vbuf)
{
  unsigned int* buf = (unsigned int*) xrgb;

  for (int i = 0; i < CAPTURE_HEIGHT; i++) {
    for (int j = 0; j < CAPTURE_WIDTH; j++) {
      unsigned int* p = &buf[CAPTURE_WIDTH * i + j];
      float H = 0.0f;
      float S = 0.0f;
      float V = 0.0f;

      RGBtoHSV(p, H, S, V);

      if (Hbuf) {
	Hbuf[CAPTURE_WIDTH * i + j] = H;
      }
      if (Sbuf) {
	Sbuf[CAPTURE_WIDTH * i + j] = S;
      }
      if (Vbuf) {
	Vbuf[CAPTURE_WIDTH * i + j] = V;
      }
    }
  }
}





//--------------------------------------
// Init and End
//--------------------------------------
void v4l2::GetImageMemory( void )
{
  rgb = ( unsigned char *)malloc( 4 * CAPTURE_WIDTH * CAPTURE_HEIGHT );
  if ( ! rgb )
    {
      fprintf(stderr, "Out of memory rgb \n");
      exit( EXIT_FAILURE );
    }

  xrgb = ( unsigned char *)malloc( 4 * CAPTURE_WIDTH * CAPTURE_HEIGHT );
  if ( ! xrgb )
    {
      fprintf(stderr, "Out of memory xrgb\n");
      exit( EXIT_FAILURE );
    }
}

void v4l2::Stop_Capture( void )
{
  enum v4l2_buf_type	type;
  
  switch ( io )
    {
    case IO_METHOD_READ:
      // Nothing to do
      break;
    case IO_METHOD_MMAP:
    case IO_METHOD_USERPTR:
      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      if ( -1 == Xioctl(fd, VIDIOC_STREAMOFF, &type) )
	{
	  Errno_Exit("VIDIOC_STREAMOFF");
	}
      break;
    }
}

void v4l2::Start_Capture( void )
{
  unsigned int		i;
  enum v4l2_buf_type	type;
  
  switch ( io )
    {
    case IO_METHOD_READ:
      break;
    case IO_METHOD_MMAP:
      for (i = 0; i < n_buffers; ++i)
	{
	  struct v4l2_buffer	buf;
	  
	  CLEAR( buf );
	  buf.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	  buf.memory	= V4L2_MEMORY_MMAP;
	  buf.index	= i;
	  
	  if ( -1 == Xioctl(fd, VIDIOC_QBUF, &buf) )
	    {
	      Errno_Exit("VIDIOC_QBUF");
	    }
	}
      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      if ( -1 == Xioctl(fd, VIDIOC_STREAMON, &type) )
	{
	  Errno_Exit("VIDIOC_STREAMON");
	}
      break;
    case IO_METHOD_USERPTR:
      for (i = 0; i < n_buffers; ++i)
	{
	  struct v4l2_buffer	buf;
	  
	  CLEAR( buf );
	  buf.type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	  buf.memory		= V4L2_MEMORY_USERPTR;
	  buf.m.userptr	= (unsigned long) buffers[i].start;
	  buf.length		= buffers[i].length;
	  
	  if ( -1 == Xioctl(fd, VIDIOC_QBUF, &buf) )
	    {
	      Errno_Exit("VIDIOC_QBUF");
	    }
	}
      type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      if ( -1 == Xioctl(fd, VIDIOC_STREAMON, &type) )
	{
	  Errno_Exit("VIDIOC_STREAMON");
	}
      break;
    }
}




void v4l2::UnInit_Device( void )
{
  unsigned int	i;

  switch ( io )
    {
    case IO_METHOD_READ:
      free ( buffers[0].start );
      break;
    case IO_METHOD_MMAP:
      for (i = 0; i < n_buffers; ++i)
	{
	  if ( -1 == munmap (buffers[i].start, buffers[i].length) )
	    {
	      Errno_Exit("munmap");
	    }
	}
      break;
    case IO_METHOD_USERPTR:
      for (i = 0; i < n_buffers; ++i)
	{
	  free( buffers[i].start );
	}
      break;
    }
  
  free( buffers );
  free( rgb );
  free( xrgb );
}

void v4l2::Init_read( unsigned int buffer_size )
{
  // ヒープメモリからsize[byte]のブロックをn個割り当てる
  // void *calloc(size_t n, size_t size);
  buffers = (buffer*) calloc(1, sizeof (*buffers));
  
  if ( ! buffers )
    {
      fprintf(stderr, "Out of memory\n");
      exit( EXIT_FAILURE );
    }
  
  buffers[0].length	= buffer_size;
  buffers[0].start	= malloc( buffer_size );
  
  if ( ! buffers[0].start )
    {
      fprintf(stderr, "Out of memory\n");
      exit( EXIT_FAILURE );
    }
}


void v4l2::Init_mmap( void )
{
  struct v4l2_requestbuffers		req;
  
  CLEAR( req );
  req.count	= 4;
  req.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory	= V4L2_MEMORY_MMAP;
  
  if ( -1 == Xioctl(fd, VIDIOC_REQBUFS, &req) )
    {
      if ( EINVAL == errno )
	{
	  fprintf(stderr, "%s does not support " "memory mapping\n", dev_name);
	  exit( EXIT_FAILURE );
	}
      else
	{
	  Errno_Exit("VIDIOC_REQBUFS");
	}
    }
  
  if ( req.count < 2 )
    {
      fprintf(stderr, "Insufficient buffer memory on %s\n", dev_name);
      exit( EXIT_FAILURE );
    }
  
  buffers = (buffer*)calloc(req.count, sizeof (*buffers));
  if ( ! buffers )
    {
      fprintf(stderr, "Out of memory mmap \n");
      exit( EXIT_FAILURE );
    }
  
  for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
    {
      struct v4l2_buffer		buf;
      
      CLEAR( buf );
      buf.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory	= V4L2_MEMORY_MMAP;
      buf.index	= n_buffers;
      
      if ( -1 == Xioctl(fd, VIDIOC_QUERYBUF, &buf) )
	{
	  Errno_Exit("VIDIOC_QUERYBUF mmap");
	}
      buffers[n_buffers].length = buf.length;
      buffers[n_buffers].start =
	mmap (	NULL,					// Start Anywhere
		buf.length,
		PROT_READ | PROT_WRITE,	// required
		MAP_SHARED,				// recommended
		fd, buf.m.offset );
      if ( MAP_FAILED == buffers[n_buffers].start )
	{
	  Errno_Exit("mmap");
	}
    }
}


void v4l2::Init_userp( unsigned int buffer_size )
{
  struct v4l2_requestbuffers		req;
  
  CLEAR( req );
  req.count	= 4;
  req.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory	= V4L2_MEMORY_USERPTR;
  
  if ( -1 == Xioctl(fd, VIDIOC_REQBUFS, &req) )
    {
      if ( EINVAL == errno )
	{
	  fprintf(stderr, "%s does not support " "user pointer i/o\n", dev_name);
	  exit( EXIT_FAILURE );
	}
      else
	{
	  Errno_Exit("VIDIOC_REQBUFS");
	}
    }
  
  buffers = (buffer*)calloc(4, sizeof (*buffers));
  if ( ! buffers )
    {
      fprintf(stderr, "Out of memory\n");
      exit( EXIT_FAILURE );
    }
  for (n_buffers = 0; n_buffers < 4; ++n_buffers)
    {
      buffers[n_buffers].length	= buffer_size;
      buffers[n_buffers].start	= malloc( buffer_size );
      
      if ( ! buffers[n_buffers].start )
	{
	  fprintf(stderr, "Out of memory\n");
	  exit( EXIT_FAILURE );
	}
    }
}


void v4l2::Init_Device( void )
{
  struct v4l2_capability		cap;
  struct v4l2_cropcap			cropcap;
  struct v4l2_crop			crop;
  struct v4l2_format			fmt;
  unsigned int				min;
  
  if ( -1 == Xioctl(fd, VIDIOC_QUERYCAP, &cap) )
    {
      if ( EINVAL == errno )
	{
	  fprintf(stderr, "%s is no V4L2 device\n", dev_name);
	  exit( EXIT_FAILURE );
	}
      else
	{
	  Errno_Exit("VIDIOC_QUERYCAP");
	}
    }
  
  if ( ! ( cap.capabilities & V4L2_CAP_VIDEO_CAPTURE ) )
    {
      fprintf(stderr, "%s is no video capture device\n", dev_name);
      exit( EXIT_FAILURE );
    }
  
  switch ( io )
    {
    case IO_METHOD_READ:
      if ( ! ( cap.capabilities & V4L2_CAP_READWRITE ) )
	{
	  fprintf(stderr, "%s does not support read i/o\n", dev_name);
	  exit( EXIT_FAILURE );
	}
      break;
    case IO_METHOD_MMAP:
    case IO_METHOD_USERPTR:
      if ( ! ( cap.capabilities & V4L2_CAP_STREAMING ) )
	{
	  fprintf(stderr, "%s does not support streaming i/o\n", dev_name);
	  exit( EXIT_FAILURE );
	}
      break;
    }
  
  // Select video input, video standard and tune here.
  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (-1 == Xioctl(fd, VIDIOC_CROPCAP, &cropcap))
    {
      // Errors ignored.
    }
  crop.type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
  crop.c		= cropcap.defrect;		// reset to default
  
  if ( -1 == Xioctl(fd, VIDIOC_S_CROP, &crop) )
    {
      switch ( errno )
	{
	case EINVAL:
	  break;	// Cropping not supported.
	default:
	  break;	// Errors ignored.
	}
    }
  
  CLEAR( fmt );
  fmt.type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width		= CAPTURE_WIDTH; 
  fmt.fmt.pix.height		= CAPTURE_HEIGHT;
  fmt.fmt.pix.pixelformat	= PIXELFORMAT;
  fmt.fmt.pix.field		= V4L2_FIELD_INTERLACED;
  
  if ( -1 == Xioctl(fd, VIDIOC_S_FMT, &fmt) )
    {
      Errno_Exit("VIDIOC_S_FMT");
    }
  
  // Note VIDIOC_S_FMT may change width and height.
  // Buggy driver paranoia.
  min = 2 * fmt.fmt.pix.width;
  if ( fmt.fmt.pix.bytesperline < min )
    {
      fmt.fmt.pix.bytesperline = min;
    }
  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  if ( fmt.fmt.pix.sizeimage < min )
    {
      fmt.fmt.pix.sizeimage = min;
    }
  
  switch ( io )
    {
    case IO_METHOD_READ:
      Init_read( fmt.fmt.pix.sizeimage );
      break;
    case IO_METHOD_MMAP:
      Init_mmap();
      break;
    case IO_METHOD_USERPTR:
      Init_userp( fmt.fmt.pix.sizeimage );
      break;
    }
}


void v4l2::Close_Device( void )
{
  if ( -1 == close (fd) )
    {
      Errno_Exit("close");
    }
  fd = -1;
}


void v4l2::Open_Device( void )
{
  struct stat		st; 

  // stat構造体を得る
  if ( -1 == stat(dev_name, &st) )
    {
      fprintf(stderr, "Cannot identify '%s': %d, %s\n", dev_name, errno, strerror (errno));
      exit( EXIT_FAILURE );
    }
  
  // キャラクタデバイスかどうかの検査
  if ( ! S_ISCHR( st.st_mode ) )
    {
      fprintf(stderr, "%s is no device\n", dev_name);
      exit( EXIT_FAILURE );
    }
  
  fd = open(dev_name, O_RDWR | O_NONBLOCK, 0);
  if ( fd == -1 )
    {
      fprintf(stderr, "Cannot open '%s': %d, %s\n", dev_name, errno, strerror (errno));
      exit( EXIT_FAILURE );
    }
}



void v4l2::Init(int w, int h, char* devname)
{
  strcpy(dev_name, devname);
  
  CAPTURE_WIDTH = w;
  CAPTURE_HEIGHT = h;

  Open_Device();
  Init_Device();
  GetImageMemory();
  Start_Capture();
}

void v4l2::End()
{
  Stop_Capture();
  UnInit_Device();
  Close_Device();
}





