/************************************************************************
  			videodevice.cpp - Copyright micha

**************************************************************************/

#include "videodevice.h"
#include "../opencvext.h"

extern "C"
{
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <stdio.h>
#include <sys/time.h>
#include <errno.h>
#include <math.h>
}

namespace seemicro
{

const int MAX_FRAMES4RATE=10;

char *palette2string(int palette)
{
  switch(palette)
  {
    case VIDEO_PALETTE_GREY: return "GREY";
    case VIDEO_PALETTE_HI240: return "HI240";
    case VIDEO_PALETTE_RGB565: return "RGB565";
    case VIDEO_PALETTE_RGB24: return "RGB24";
    case VIDEO_PALETTE_RGB32: return "RGB32";
    case VIDEO_PALETTE_RGB555: return "RGB555";
    case VIDEO_PALETTE_YUV422: return "YUV422";
    case VIDEO_PALETTE_YUYV: return "YUYV";
    case VIDEO_PALETTE_UYVY: return "UYVY";
    case VIDEO_PALETTE_YUV420: return "YUV420";
    case VIDEO_PALETTE_YUV411: return "YUV411";
    case VIDEO_PALETTE_RAW: return "RAW";
    case VIDEO_PALETTE_YUV422P: return "YUV422P";
    case VIDEO_PALETTE_YUV411P: return "YUV411P";
    case VIDEO_PALETTE_YUV420P: return "YUV420P";
    case VIDEO_PALETTE_YUV410P: return "YUV410P";
    default: return "?";
  }

  // "mplayer -vf format=fmt=help" returns:
  // Available formats: 444p 422p 411p yuy2 uyvy yvu9 if09
  // yv12 i420 iyuv clpl hm12 y800 y8 nv12 nv21 bgr24 bgr32
  // bgr16 bgr15 bgr8 bgr4 bg4b bgr1 rgb24 rgb32 rgb16 rgb15
  // rgb8 rgb4 rg4b rgb1

  // mplayer     / v4l     / v4l2:
  // planar yv12 / yuv420p / yvu420
  // bgr24       / rgb24   / bgr24
  //               yuv422  / yuyv
  return "??";
}

string types2string(int type)
{
  string s;
  if(type&VID_TYPE_CAPTURE) s+= "Can capture to memory,";
  if(type&VID_TYPE_TUNER) s+="Has a tuner of some form,";
  if(type&VID_TYPE_TELETEXT) s+="Has teletext capability,";
  if(type&VID_TYPE_OVERLAY) s+="Can overlay its image onto the frame buffer,";
  if(type&VID_TYPE_CHROMAKEY) s+="Overlay is Chromakeyed,";
  if(type&VID_TYPE_CLIPPING) s+="Overlay clipping is supported,";
  if(type&VID_TYPE_FRAMERAM) s+="Overlay overwrites frame buffer memory,";
  if(type&VID_TYPE_SCALES) s+="The hardware supports image scaling,";
  if(type&VID_TYPE_MONOCHROME) s+="Image capture is grey scale only,";
  if(type&VID_TYPE_SUBCAPTURE) s+="Capture can be of only part of the image,";

  return s;
}

videoDevice::videoDevice(string device, int imgFmt, int channel,
 unsigned int reqWidth, unsigned int reqHeight) :
 video_fd(device.c_str(), O_RDWR)
{
  // for video_fd we did resource acquisition according to B. Stroustrup
  // "The C++ Programming Language" 14.4

  /* Erforderte V4L2-Header
  struct v4l2_capability *argp;
  if(ioctl(video_fd, VIDIOC_QUERYCAP, &argp) != 0)
  {
    if(errno==EINVAL) cerr << "Device is not V4L2 compatible.\n";
    else perror("init_camera() / VIDIOC_QUERYCAP");
  }
  else cerr << "Device is V4L2 compatible!\n";
  */

  if(ioctl(video_fd, VIDIOCGCAP, &capability)!=0)
  {
    perror("init_camera() / VIDIOCGCAP failed");
  }

  if(DEBUGLEVEL>=2)
  {
    cout << "Canonical name for this interface: " << capability.name << endl
      << "\tType of interface: " << types2string(capability.type) << endl
      << "\tNumber of radio/tv channels if appropriate: " << capability.channels << endl
      << "\tNumber of audio devices if appropriate: " << capability.audios << endl
      << "\tMaximum capture width in pixels: " << capability.maxwidth << endl
      << "\tMaximum capture height in pixels: " << capability.maxheight << endl
      << "\tMinimum capture width in pixels: " << capability.minwidth << endl
      << "\tMinimum capture height in pixels: " << capability.minheight << endl;
  }

 if(ioctl(video_fd, VIDIOCGCHAN, &vc)!=0)
  {
    perror("init_camera() / VIDIOCGCHAN failed");
  }
  vc.norm=VIDEO_MODE_PAL;// user can set this manually after init also
  if(channel!=-1) vc.channel=channel;
  if(ioctl(video_fd, VIDIOCSCHAN, &vc)!=0)
  {
    perror("init_camera() / VIDIOCSCHAN failed");
  }

  if(DEBUGLEVEL>=2  && capability.type&VID_TYPE_TUNER)
  {
    unsigned long int freq;
    if(ioctl(video_fd, VIDIOCGFREQ, &freq)!=0)
    {
      perror("init_camera() / VIDIOCGFREQ failed");
    }
    else cerr << "Queried frequency: " << freq << endl;
  }

  // Capture window holen
  if(0 != ioctl(video_fd, VIDIOCGWIN, &win))
  {
    perror("init_camera() / VIDIOCGWIN failed");
  }
  if(DEBUGLEVEL>=2)
    printf("win (voreingestellt): x=%d y=%d width=%d height=%d chromakey=%d flags=%d\n",
      win.x, win.y, win.width, win.height, win.chromakey, win.flags);

  // Angefragte Bildgröße setzen
  win.x = 0;
  win.y = 0;
  if(reqWidth!=0 || reqHeight!=0)
  {
    if(DEBUGLEVEL>=2) cerr << "Requesting " << reqWidth << "x"
      << reqHeight << endl;
    win.width = reqWidth;
    win.height = reqHeight;
  }
  win.chromakey = 0;
  win.flags = 0;
  if(0 != ioctl(video_fd, VIDIOCSWIN, &win))
  {
    perror("init_camera() / VIDIOCSWIN failed");
  }

  // get win again to see whether device supports our request
  if(reqWidth!=0 || reqHeight!=0)
  {
    if(0 != ioctl(video_fd, VIDIOCGWIN, &win))
    {
      perror("init_camera() / VIDIOCGWIN failed");
    }
    if(win.width==reqWidth && win.height==reqHeight && DEBUGLEVEL>=2)
    {
      cout << "Device supports requested window size.\n";
    }
    else if(DEBUGLEVEL>=1)
    {
      cout << "Device does not support requested window size. "
        << "Size adjusted to " << size()->width << 'x'
        << size()->height << "\n";
    }
  }
  if(DEBUGLEVEL>=2)
    printf("win (Rueckgabe VIDIOCGWIN): x=%d y=%d width=%d height=%d chromakey=%d flags=%d\n",
      win.x, win.y, win.width, win.height, win.chromakey, win. flags);

  // Bildeigenschaften holen
  if(0 != ioctl(video_fd, VIDIOCGPICT, &pict))
  {
    perror("init_camera() / VIDIOCGPICT failed");
  }

  if(DEBUGLEVEL>=2)
    printf("v4l: channel=%d colour=%d hue=%d brightness=%d "
      "constrast=%d whiteness=%d palette=%s (%d)\n",
      vc.channel,
      pict.colour,
      pict.hue,
      pict.brightness,
      pict.contrast,
      pict.whiteness,
      palette2string(pict.palette), pict.palette);

  this->imgFmt = imgFmt;
  // ist imgFmt==RGB, we try rgb first then yuv420p, then fail
  // ist imgFmt==gray, we try grey first, then yuv420p, then rgb, then fail
  // YUV420P == 15, RGB24 == 4
  int status = 0;// OK
  if(DEBUGLEVEL>=2)
    cerr << "Angefordertes Bildformat: " << palette2string(imgFmt) << endl;

  if(imgFmt==VIDEO_PALETTE_GREY)
  {
    if(DEBUGLEVEL>=2)
      cerr << "\tVersuche VIDEO_PALETTE_GREY...";
    fmt = VIDEO_PALETTE_GREY;
    if(!probeFmt(fmt))
    {
      if(DEBUGLEVEL>=2)
        cerr << "Nein.\n\tVersuche VIDEO_PALETTE_YUV420P...";
      fmt = VIDEO_PALETTE_YUV420P;
      if(!probeFmt(fmt))
      {
        if(DEBUGLEVEL>=2)
          cerr << "Nein.\n\tVersuche VIDEO_PALETTE_RGB24...";
        fmt = VIDEO_PALETTE_RGB24;
        if(!probeFmt(fmt))
	{
	  status = 1;// Videoformat nicht unterstützt
	}
	else if(DEBUGLEVEL>=2) cerr << "Ja!\n";
      }
      else if(DEBUGLEVEL>=2) cerr << "Ja!\n";
    }
    else if(DEBUGLEVEL>=2) cerr << "Ja!\n";
  }
  else if(imgFmt==VIDEO_PALETTE_RGB24)
  {
    fmt = VIDEO_PALETTE_RGB24;
    if(!probeFmt(fmt))
    {
      fmt = VIDEO_PALETTE_YUV420P;
      if(!probeFmt(fmt))
      {
	status = 1;// Videoformat nicht unterstützt
      }
    }
  }
  else
  {
    status = 1;
  }
  if(DEBUGLEVEL>=1 && status!=0)
  {
    cerr << "Angefordertes Videoformat '"
      << palette2string(imgFmt) << "' wird vom Gerät/Programm nicht unterstützt. Ignoring :)\n";
  }

  if(ioctl(video_fd, VIDIOCGMBUF, &vm)!=0)
  {
    perror("videoDevice::videoDevice() / VIDIOCGMBUF");
    throw bad_alloc();
  }

  if(DEBUGLEVEL>=2)
  {
    printf("vm: frames=%d size=%d bytes, offsets[0]=%i\n",
      vm.frames, vm.size, vm.offsets[0]);

    // calculate io bandwidth by size*frames/s
    printf("Estimated IO Bandwidth: %d MB/s\n",
      vm.size/vm.frames*25/(2<<20));
  }

  if(vm.frames<2)
  {
    cerr << "VIDIOCGMBUF returned " << vm.frames << " frames. We need at "
      "least 2 frames for capturing!\n";
    throw bad_alloc();
  }

  pict_buf = (unsigned char*) mmap(0, vm.size, PROT_READ|PROT_WRITE,
    MAP_SHARED, video_fd, 0);
  if(pict_buf==MAP_FAILED)
  {
    perror("videoDevice::videoDevice() / mmap");
    throw bad_alloc();
  }

  // ask video device to fill two buffers
  mm.frame = 0;
  mm.height = win.height;
  mm.width = win.width;
  mm.format = fmt;

  if(DEBUGLEVEL>=2)
    printf("mm: frame: %d height: %d width: %d format: %s\n",
      mm.frame, mm.height, mm.width, palette2string(mm.format));

  if(ioctl(video_fd, VIDIOCMCAPTURE, &mm)!=0)
    perror("init_camera() / VIDIOCMCAPTURE (1)");

  if(DEBUGLEVEL>=2)
    printf("mm: frame: %d height: %d width: %d format: %s\n",
      mm.frame, mm.height, mm.width, palette2string(mm.format));

  mm.frame = 1;
  if(ioctl(video_fd, VIDIOCMCAPTURE, &mm)!=0)
    perror("init_camera() / VIDIOCMCAPTURE (2)");

  // fetch first frame so that searchRadius will have
  // useful data to work on (not sure :) )
  mm.frame = 0;
  if(ioctl(video_fd, VIDIOCSYNC, &mm.frame) != 0)
  {
    perror("grab_from_camera() / VIDIOCSYNC");
  }
  if(ioctl(video_fd, VIDIOCMCAPTURE, &mm)!=0)
    perror("init_camera() / VIDIOCMCAPTURE (3)");
  mm.frame = 1;

  last_VIDIOCMCAPTURE_was_successful=1;

  // real resource aquisition would be nice
  img = cvCreateImage(cvSize(win.width, win.height), IPL_DEPTH_8U,
   (imgFmt==VIDEO_PALETTE_RGB24) ? 3 : 1);
  framesCaptured = 0;
  keyFrameRate = 25;
  verbogen = false;
}

bool videoDevice::probeFmt(int fmt)
{
  // SPICT liefert ioctl()!=0 und errno==EINVAL, falls fmt
  // nicht akzeptiert wird
  if(setPicture(&fmt, true)==0) return true;
  if(errno==EINVAL) return false;
  perror("videoDevice::probeFmt() / VIDIOCSPICT failed");
  return false;
}

videoDevice::~videoDevice()
{
  if(verbogen && img) img->imageData = (char *)imageDataOld;
  if(munmap(pict_buf, vm.size)!=0)
    perror("~videoDevice() / munmap()");
  cvReleaseImage(&img);
  close(video_fd);
}

int videoDevice::setPicture(int *reqFmt, bool quiet)
{
  if(reqFmt) pict.palette = *reqFmt;
  // optional: VIDIOGPICT, um Änderungen von anderen
  // Anwendungen nicht zu überschreiben
  int ret = ioctl(video_fd, VIDIOCSPICT, &pict);
  if(0 != ret && !quiet)
  {
    perror("videoDevice::setPicture() / VIDIOCSPICT failed");
  }
  if(reqFmt) *reqFmt = pict.palette;
  return ret;
}

CvSize* videoDevice::size()
{
  static CvSize s = cvSize(win.width-win.x, win.height-win.y);
  return &s;
}

int videoDevice::captureFrame(void)
{
#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "->captureFrame()\n";
#endif

  if(!last_VIDIOCMCAPTURE_was_successful) return -2;

  if(ioctl(video_fd, VIDIOCMCAPTURE, &mm) != 0)
  {
    perror("videoDevice::captureFrame() / VIDIOCMCAPTURE");
    last_VIDIOCMCAPTURE_was_successful=0;
    if(munmap(0, vm.size)!=0)
      perror("videoDevice::captureFrame() / munmap()");
    if(close(video_fd)!=0)
      perror("videoDevice::captureFrame() / close()");
    return -1;
  }

  last_VIDIOCMCAPTURE_was_successful=1;
  mm.frame++;
  mm.frame = mm.frame % 2;// or % vm.frames, but only 2 needed

#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "<-captureFrame()\n";
#endif
  return 0;
}

int videoDevice::syncFrame(void)
{
#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "->syncFrame()\n";
#endif

  if(last_VIDIOCMCAPTURE_was_successful == 1)
  {
    // check whether device is still valid
    if(0 != ioctl(video_fd, VIDIOCGWIN, &win))
    {
      perror("grab_from_camera() / VIDIOCGWIN failed");
      return -1;
    }

    if(ioctl(video_fd, VIDIOCSYNC, &mm.frame) != 0)
    {
      perror("grab_from_camera() / VIDIOCSYNC");
      return -2;
    }
    framesCaptured++;

    /* für bt848: nicht unterstützt
    if(ioctl(video_fd, VIDIOCGPLAYINFO, &vinfo) != 0)
    {
      perror("grab_from_camera() / VIDIOCGPLAYINFO");
    }
    else
    {
      cerr << "smpte_timecode: " << vinfo.smpte_timecode
	<< " temporal_reference: " << vinfo.temporal_reference << endl;
    }
    */
  }
  return 0;
}

/*
IplImage *videoDevice::asIplImage()
{
#ifdef DEBUG
  cerr << "->asIplImage: fmt=" << palette2string(fmt)
    << " mm.frame=" << mm.frame << endl;
#endif

  if(fmt == VIDEO_PALETTE_YUV420P)
    yuv420p_unpack_2_rgb24(pict_buf + vm.offsets[mm.frame],
	img->imageData, win.width, win.height);

  // why memcpy()? we could let imageData point to
  // the buffer area
  else memcpy(img->imageData, pict_buf + vm.offsets[mm.frame],
    img->imageSize);

#ifdef DEBUG
  cerr << "<-asIplImage\n";
#endif
  return img;
}
*/

// pict_buf kommt von syncFrame()
// kopiert/konvertiert den durch ROI
// angegebenen Bereich (alles, wenn ROI==NULL)
// und speichert ihn in "in"
// Breite und Höhe von pict_buf und "in" müssen übereinstimmen
// Annahme: In pict_buf liegt ein Bild im RGB-Format vor
/*
void videoDevice::capture2in(CvRect *aROI, unsigned char *pict_buf, IplImage *in)
{
  assert(pict_buf);
  assert(in);
  int w = in->width;
  int h = in->height;

  unsigned char* buffer=pict_buf;
  unsigned char* rgbbuffer=NULL;

  if(fmt == VIDEO_PALETTE_YUV420P){
    rgbbuffer = new (unsigned char)[w*h*3];
    yuv420p_unpack_2_rgb24(buffer,
	(char*)rgbbuffer, w, h);
    buffer=rgbbuffer;
    }

  myrect ROI;
  if(aROI) ROI = myrect(*aROI);
  else ROI = cvRect(0, 0, w, h);
//  cerr << "-> capture2in: ROI=" << ROI << endl;

  for(int y=0; y<ROI.r.height; y++)
  {
    int line = ROI.r.y + y;
    unsigned char *src = buffer + (line * w + ROI.r.x) * 3;
    unsigned char *dst = (unsigned char *)in->imageData + line * in->widthStep + ROI.r.x;
    for(int x=0;x<ROI.r.width; x++)
    {
      // pixel hernehmen
      // farbraum konvertieren
      // yuv2gray wäre übrigens einfacher, da "y" der Grauwert ist
      // und "uv" nicht gebraucht werden
      // rgb2gray = (r+g+b)/3
      // into in->imgData speichern
      *dst++ = (src[0]+src[1]+src[2])/3;
      src += 3;
    }
  }
  if(rgbbuffer) delete [] rgbbuffer;

//  cerr << "<- capture2in\n";
}
*/

float videoDevice::framerate()
{
    static struct timeval history[MAX_FRAMES4RATE];
    static int lastframe=0;
    if(0 != gettimeofday(&history[lastframe], 0))
      fprintf(stderr, "gettimeofday() failed: %d\n", errno);

    float rate=0;
    for(int i=1;i<MAX_FRAMES4RATE;i++)
    {
       int f1 = (lastframe+i) % MAX_FRAMES4RATE;
       int f2 = (lastframe+i+1) % MAX_FRAMES4RATE;
       long diff = 1000000*(history[f2].tv_sec-history[f1].tv_sec) +
	 history[f2].tv_usec-history[f1].tv_usec;
       rate += diff;
       //if(i==1) fprintf(stderr,"%li ", diff);
    }

    ++lastframe %= MAX_FRAMES4RATE;
    return 1.0/(rate/(MAX_FRAMES4RATE-1)/1000000);
}

} // namespace seemicro

