#define COLOR        1
#define NO_INTERLACE 0 

#include "V4L2Camera.h"

#ifdef HAVE_V4L2

#include <iostream>
#include <string.h>
#include <assert.h>

V4L2Camera v4l2camera;

static unsigned int nwidth = DEFAULT_IMAGE_WIDTH;
static unsigned int nheight = DEFAULT_IMAGE_HEIGHT;
static unsigned int nwidth1=168;
static unsigned int nheight1=134;  
pixel_format nfmt = pix_bgr32;
char *device = DEFAULT_VIDEO_DEVICE;

//==== Capture Class Implementation =======//
bool V4L2Camera::open(IPL_CONFIG *ipl_config)
{
  struct v4l2_requestbuffers req;
  int err;
  int i;

#if 0 
  nwidth=480;
  nheight=480;
#else
  nwidth=320;
  nheight=240;
#endif
  
  if (NULL == device)
    device = DEFAULT_VIDEO_DEVICE;
  if (0 == nwidth)
    nwidth = DEFAULT_IMAGE_WIDTH;
  if (0 == nheight)
    nheight = DEFAULT_IMAGE_HEIGHT;

  // Open the video device
  vid_fd = ::open(device, O_RDONLY);
  if(vid_fd == -1){
    printf("Could not open video device [%s]\n",device);
    return(false);
  }

  /* Turn off capture */
  i = V4L2_BUF_TYPE_CAPTURE;
  err = ioctl(vid_fd, VIDIOC_STREAMOFF, &i);
  if(err){
    printf("VIDIOC_STREAMOFF returned error: %s\n",strerror(errno));
  }

  fmt.type = V4L2_BUF_TYPE_CAPTURE;
  err = ioctl(vid_fd, VIDIOC_G_FMT, &fmt);
  if(err){
    printf("G_FMT returned error: %s\n",strerror(errno));
    return(false);
  }

  fmt.fmt.pix.width = nwidth;
  fmt.fmt.pix.height = nheight;
  fmt.fmt.pix.pixelformat = map_to_v4l2_format(nfmt);
#if NO_INTERLACE  
  fmt.fmt.pix.flags=0;
#else
  fmt.fmt.pix.flags=4;
#endif

  // need to repeat following twice?
  err = ioctl(vid_fd, VIDIOC_S_FMT, &fmt);
  if(err){
    printf("S_FMT returned error %s\n",strerror(errno));
    return(false);
  }
  assert(fmt.fmt.pix.width == nwidth && fmt.fmt.pix.height == nheight);

  // Request mmap-able capture buffers
  req.count = STREAMBUFS;
  req.type  = V4L2_BUF_TYPE_CAPTURE;
  err = ioctl(vid_fd, VIDIOC_REQBUFS, &req);
  if(err < 0 || req.count < 1){
    printf("REQBUFS returned error '%s', count %d\n",
	   strerror(errno),req.count);
    return(false);
  }

  for(i=0; i<req.count; i++){
    vimage[i].vidbuf.index = i;
    vimage[i].vidbuf.type = V4L2_BUF_TYPE_CAPTURE;
    err = ioctl(vid_fd, VIDIOC_QUERYBUF, &vimage[i].vidbuf);
    if(err < 0){
      printf("QUERYBUF returned error %s\n",strerror(errno));
      return(false);
    }

    vimage[i].data = (char*)mmap(0, vimage[i].vidbuf.length, PROT_READ,
				 MAP_SHARED, vid_fd, vimage[i].vidbuf.offset);
    if(vimage[i].data == MAP_FAILED){
      printf("mmap() returned error %s\n", strerror(errno));
      return(false);
    }
  }

  for(i=0; i<req.count; i++){
    if((err = ioctl(vid_fd, VIDIOC_QBUF, &vimage[i].vidbuf))){
      printf("QBUF returned error %s\n",strerror(errno));
      return(false);
    }
  }

  // Turn on streaming capture
  err = ioctl(vid_fd, VIDIOC_STREAMON, &vimage[0].vidbuf.type);
  if(err){
    printf("STREAMON returned error %s\n",strerror(errno));
    return(false);
  }

  //Important !
  ipl_config->colorBlack  = 0;
  ipl_config->colorWhite  = 255;
  ipl_config->imageWidth  = nwidth1;
  ipl_config->imageHeight = nheight1;
  
  //Only area around the center is interesting
  pic_size_x= ipl_config->imageWidth;
  pic_size_y= ipl_config->imageHeight;
  pic_size=pic_size_x*pic_size_y;

  x_offset=(nwidth-pic_size_x)>>1;
  y_offset=(nheight-pic_size_y)>>1;

#if !COLOR
  ipl_config->imageType   = pix_grey;  
#else  
  ipl_config->imageType   = pix_rgb24; 
#endif
  
  return(true);
}

void V4L2Camera::close()
{
  int i,t;

  if(vid_fd >= 0){
    t = V4L2_BUF_TYPE_CAPTURE;
    ioctl(vid_fd, VIDIOC_STREAMOFF, &t);

    for(i=0; i<STREAMBUFS; i++){
      if(vimage[i].data){
        munmap(vimage[i].data,vimage[i].vidbuf.length);
      }
    }
  }

  ::close(vid_fd);
  vid_fd = -1;

}

static struct v4l2_buffer tempbuf;
static int oldauto=0;
extern int autobrightness;
void V4L2Camera::read(Picture *p_frame)
{
  register int i;
  
  if (-1 == vid_fd)
    {
      fprintf(stdout,"-1 == vid_fd\n");
      //return -1;
    }
  // struct v4l2_buffer tempbuf;
  int err;

  fd_set          rdset;
  struct timeval  timeout;
  int		  n;

  FD_ZERO(&rdset);
  FD_SET(vid_fd, &rdset);
  timeout.tv_sec = 1;
  timeout.tv_usec = 0;
  n = select(vid_fd + 1, &rdset, NULL, NULL, &timeout);
  err = -1;
  if (n == -1)
    fprintf(stderr, "capture select error: %s.\n", strerror(errno));
  else if (n == 0)
    fprintf(stderr, "capture select timeout: %s\n", strerror(errno));
  else if (FD_ISSET(vid_fd, &rdset))
    err = 0;
  
  if(err) 
    {
      fprintf(stdout,"err\n");
    }
  else
    {  
      // Grab last frame
      tempbuf.type = vimage[0].vidbuf.type;
      err = ioctl(vid_fd, VIDIOC_DQBUF, &tempbuf);
      
      if(err) 
	printf("DQBUF returned error %s\n",strerror(errno));
      
      // Set current to point to captured frame data
      BYTE *tmp1=p_frame->data;
      BYTE *tmp2=(unsigned char *)vimage[tempbuf.index].data;
      
      tmp2+=((x_offset+y_offset*nwidth)<<2);
      //bgr _-_ rgb
      for(i=1; i <= pic_size; i++)
	{
#if COLOR
	*tmp1++=*(tmp2+2); //bgr _-_ rgb
	*tmp1++=*(tmp2+1);
	*tmp1++=*(tmp2);
	tmp2+=4;
#else
	*tmp1++=(*(tmp2++)+*(tmp2++)+*(tmp2++)) / 3;
	tmp2++;
#endif  
	if(i%(pic_size_x)==0)
	  tmp2+=((x_offset)<<3);
	}
#if 0
      //Turn on auto white balance
      if (oldauto!=autobrightness)
	{
	  struct v4l2_control id;
	  
	  id.id = V4l2_CID_AUTO_WHITE_BALANCE; 
	  
	  if(autobrightness==1)
	    id.value = true;
	  else
	    id.value = false;
	  
	  ioctl(vid_fd, VIDIOC_S_CTRL, &id);
	  oldauto=autobrightness;
	}
      
#else
      if (autobrightness==1)
	{
	  int i;
	  long sum = 0;
	  double mean;
	  static double tempz = 0.0;
	  static double tempint = 0.0;
	  int MAX_AUTO_ADJUST = 255;
	  int val1=99; int val=100;
	  struct v4l2_control id;
	  id.id =  V4L2_CID_BRIGHTNESS;
	  
	  if(p_frame->format==pix_grey)
	    {
	      for (i = 0; i < p_frame->datasize; i++)
		sum += p_frame->data[i];
	      
	      mean = (double)sum / p_frame->datasize;
	      
	      if (mean > val)
		tempint = - MAX_AUTO_ADJUST * ((mean - val) / (val1));
	      else if (mean < (val1))
		tempint = MAX_AUTO_ADJUST * (1 - (mean / (val1)));
	      
	      if (mean > val || mean < (val1))
		{
		  
		  ioctl(vid_fd,VIDIOC_G_CTRL, &id);
		  
		  tempz =id.value;
		  
		  tempz += tempint;
		  
		  if (tempz >= 255)
		    tempz = 254;
		  
		  if (tempz <= 0)
		    tempz = 1;
		  
		  //set brightness
		  id.value = (int) tempz;
		  ioctl(vid_fd,VIDIOC_S_CTRL, &id);
		}
	    }
	  else if(p_frame->format==pix_rgb24)
	    {//Color pic
	      for (i = 0; i < p_frame->datasize/3; i+=3)
		sum += ((p_frame->data[i] + 
			p_frame->data[i+1] + 
			p_frame->data[i+2])/3);
	      
	      mean = (double)sum / (p_frame->datasize/3);
	      
	      if (mean > val)
		tempint = - MAX_AUTO_ADJUST * ((mean - val) / (val1));
	      else if (mean < val1)
		tempint = MAX_AUTO_ADJUST * (1 - (mean / (val1)));
	      
	      if (mean > val || mean < (val1))
		{
		  ioctl(vid_fd,VIDIOC_G_CTRL, &id);
		  //fprintf(stdout,"%d\n",id.value);
		  tempz =id.value;
		  
		  tempz += tempint;
		  
		  if (tempz >= 255)
		    tempz = 254;
		  
		  if (tempz <= 0)
		    tempz = 1;
		  
		  //set brightness
		  id.value = (int) tempz;
		  ioctl(vid_fd,VIDIOC_S_CTRL, &id);
		}
	    }
	}
#endif	  
      // Initiate the next capture
      err = ioctl(vid_fd, VIDIOC_QBUF, &tempbuf);
      
      if(err) 
	  printf("QBUF returned error %s\n",strerror(errno));
    }
  
  if (NULL == p_frame->data)
      fprintf(stdout,"NULL == p_frame->data\n");
  
  frame_count++;
  
}

void
V4L2Camera::get_info(CamInfo *info)
{
  info->width = nwidth1; 
  info->height = nheight1;
  
  if (iscolor() == true)
    info->bpp = 24;
  else
    info->bpp = 8;  /* for the quickcam on xena(Thu Jul 13 14:39:26 WST 2000)*/
}

bool 
V4L2Camera::get_IPL(IPL_CONFIG *ipl_config)
{
  ipl_config->imageWidth  = nwidth1;
  ipl_config->imageHeight = nheight1;
  ipl_config->imageType   = pix_rgb24;
  ipl_config->image_ratio = (float) ((float)ipl_config->imageHeight / 
				     (float) ipl_config->imageWidth);
}

bool
V4L2Camera::iscolor(void)
{
  return(true);
}


int
V4L2Camera::map_to_v4l2_format(pixel_format format)
{
  int v4l2_format = 0;
  switch (format)
    {
    case pix_yuv9:
      v4l2_format = V4L2_PIX_FMT_YUV410; /* YUV 4:1:0 planar */
      break;
    case pix_yuv12:
      v4l2_format = V4L2_PIX_FMT_YUV420; /* YUV 4:2:0 planar */
      break;
    case pix_yuyv:
      v4l2_format = V4L2_PIX_FMT_YUYV;   /* YUV 4:2:2 packed */
      break;
    case pix_rgb24:
      v4l2_format = V4L2_PIX_FMT_RGB24;
      break;
    case pix_rgb32:
      v4l2_format = V4L2_PIX_FMT_RGB32;
      break;
    case pix_bgr24:
      v4l2_format = V4L2_PIX_FMT_BGR24;
      break;
    case pix_bgr32:
      v4l2_format = V4L2_PIX_FMT_BGR32;
      break;
    }
  fprintf(stderr, "Mapped %d to %d\n", format, v4l2_format);
  return v4l2_format;
}   

void V4L2Camera::params(double *p)
{
  struct v4l2_control id;
  
  //Set Brightness
  id.id =  V4L2_CID_BRIGHTNESS;
  id.value = (int) p[3];
  ioctl(vid_fd,VIDIOC_S_CTRL, &id);
  
  //Set Contrast
  id.id =  V4L2_CID_CONTRAST;
  id.value = (int) p[4];
  ioctl(vid_fd,VIDIOC_S_CTRL, &id);
  
  //Set Saturation
  id.id =  V4L2_CID_SATURATION;
  id.value = (int) p[5];
  ioctl(vid_fd,VIDIOC_S_CTRL, &id);

}

#endif /* HAVE_V4L2 */



