/*
 *  V4L2 interface to the sn9c102 webcam
 *  by Bram Stolk
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
#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>          /* for videodev2.h */

#include <linux/videodev2.h>


#include "sonix_compress.h"
#include "controls.h"
#include "bayer.h"

#include "sonixcam.h"



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

// For people who have not yet patched their /usr/include/linux/videodev2.h

#ifndef V4L2_PIX_FMT_SN9C10X
#define V4L2_PIX_FMT_SN9C10X  v4l2_fourcc('S','9','1','0')
#endif

#ifndef V4L2_PIX_FMT_SBGGR8
#define V4L2_PIX_FMT_SBGGR8   v4l2_fourcc('B','A','8','1')
#endif


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


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


bool SonixCam::ProcessImage(unsigned char *p)
{
  unsigned char *src = (unsigned char *) p;
  unsigned char *reader = src;
  if (do_compression)
  {
    sonix_decompress(352, 288, src, uncompressed_src);
    reader = uncompressed_src;
  }

  if (w==176)
  {
    unsigned char *writer = rgb;
    for (int y=0; y<144; y++)
    {
      for (int x=0; x<176; x++)
      {
        unsigned char b  = reader[2*x];
        unsigned char g1 = reader[2*x+1];
        unsigned char g2 = reader[352+2*x];
        unsigned char r  = reader[352+2*x+1];
        *writer++ = r;
        *writer++ = (g1+g2)/2;
        *writer++ = b;
      }
      reader += 2*352;
    }
  }
  else
  {
    bayer2rgb24(rgb, reader, w, h);
  }

  static int histo_pre[256];
  static int histo_post[256];
  static unsigned char grn[3]={0,255,0};
  static unsigned char red[3]={255,0,0};
  if (do_auto_gain || do_normalize || show_hists)
    Analyze(rgb, histo_pre, false);
  if (do_normalize)
  {
    AnalyzeNormalize(rgb, histo_pre);
    Analyze(rgb, histo_post, true);
  }
  if (show_hists)
  {
    if (do_normalize)
      AnalyzeSuperimpose(rgb, histo_post, grn);
    AnalyzeSuperimpose(rgb, histo_pre,  red);
  }

  if (sonix_unknown)
  {
    fprintf(stderr,"sonix_unknown=%x\n", sonix_unknown);
    sonix_unknown=0;
  }
  return true;
}


bool SonixCam::ReadImage(void)
{
  struct v4l2_buffer buf;

  CLEAR (buf);

  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;

  int rv = ioctl(fd, VIDIOC_DQBUF, &buf);
  if (rv==-1)
  {
    switch (errno) 
    {
      case EAGAIN:
     	return false;
      default:
        errno_exit ("VIDIOC_DQBUF");
    }
  }

  assert (buf.index < n_buffers);
  ProcessImage((unsigned char*) buffers[buf.index].start);
  if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
    errno_exit ("VIDIOC_QBUF");

  return true;
}



// Returns true if new img was read.
bool SonixCam::Sustain(void)
{
  if (!capturing)
    return false;
  return ReadImage();
}


bool SonixCam::StopCapture(void)
{
  capturing = false;
  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
    errno_exit ("VIDIOC_STREAMOFF");

  return true;
}


bool SonixCam::StartCapture(void)
{
  capturing = true;
  for (unsigned int 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");
  }
		
  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
    errno_exit ("VIDIOC_STREAMON");
  return true;
}



SonixCam::~SonixCam()
{
  unsigned int i;

  for (i = 0; i < n_buffers; ++i)
    if (-1 == munmap (buffers[i].start, buffers[i].length))
      errno_exit ("munmap");

  free (buffers);

  delete rgb;
  delete uncompressed_src;

  // close device
  if (-1 == close (fd))
    errno_exit ("close");

  fd = -1;
}


SonixCam::SonixCam(const char *dev_name, int width, int height) :
  w(width),
  h(height),
  fd(-1),
  buffers(0),
  n_buffers(0),
  rgb(0),
  uncompressed_src(0),
  capturing(false),
  show_hists(false),
  do_compression(false),
  do_normalize(false),
  do_auto_gain(false)
{
  // open device

  struct stat st; 

  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 (-1 == fd) 
  {
    fprintf (stderr, "Cannot open '%s': %d, %s\n", dev_name, errno, strerror (errno));
    exit (EXIT_FAILURE);
  }


  struct v4l2_capability cap;
  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");
    }
  }

  strncpy(name, (char*)cap.card, 128);
  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) 
  {
    fprintf (stderr, "%s is no video capture device\n", dev_name);
    exit (EXIT_FAILURE);
  }

  if (!(cap.capabilities & V4L2_CAP_STREAMING)) 
  {
    fprintf (stderr, "%s does not support streaming i/o\n", dev_name);
    exit (EXIT_FAILURE);
  }

  controls_enumerate(fd);
  int q=controls_get_jpegcompr(fd);

  /* Select video input, video standard and tune here. */

  struct v4l2_cropcap cropcap;
  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (-1 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) 
  {
    /* Errors ignored. */
  }

  struct v4l2_crop crop;
  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:
      /* Cropping not supported. */
        break;
     default:
       /* Errors ignored. */
        break;
    }
  }

  SetCompression(true);

  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 = (struct buffer*)calloc (req.count, sizeof (*buffers));

  if (!buffers) 
  {
    fprintf (stderr, "Out of memory\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");

    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");
  }

  rgb = new unsigned char [w*h*3];
  uncompressed_src = new unsigned char [352*288];

  sonix_decompress_init();
  SetGain(0.5);
}


bool SonixCam::SetGain(float v)
{
  gain = v;
  static int max = controls_get_max_gain(fd);
  unsigned char val = (unsigned char)(max*v);
  controls_set_gain(fd, val);
  return true;
}


bool SonixCam::SetCompression(bool v)
{
  do_compression = v;
  struct v4l2_format fmt;
  CLEAR (fmt);
  fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width       = 352; 
  fmt.fmt.pix.height      = 288;

  if (do_compression)
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SN9C10X;
  else
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;
  fmt.fmt.pix.field       = V4L2_FIELD_NONE;

  if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
  {
    perror("VIDIOC_S_FMT");
    return false;
  }

  // fprintf(stderr,"After compression setting, wxh is %dx%d\n",fmt.fmt.pix.width,fmt.fmt.pix.height);
  assert(fmt.fmt.pix.width == 352);
  // Sometimes, pix.height is mangled to 16, WHY???
  assert(fmt.fmt.pix.height == 288);

  return true;
}


bool SonixCam::SetJPEG(int v)
{
  controls_set_jpegcompr(fd, v);
}

