#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/videoio.h>

extern const char *__progname;

static const char *videodev = "viddev";

/*
 * Based on the Wikipedia article YCbCr, R'G'B' (gamma-corrected RGB)
 *  is converted to Y'PbPr as
 *
 *	Y' = KR R' + (1 - KR - KB) G' + KB B'
 *	Pb = (1/2) (B' - Y') / (1 - KB)
 *	Pr = (1/2) (R' - Y') / (1 - KR)
 *
 *  where KB and KR are constants which depend on the definition of the
 *  RGB space in use.  R', G', B', and Y' range over [0,1], while Pb
 *  and Pr range over [-.5,.5].
 *
 * When applied to digital signals, the terminology changes from Pb/Pr
 *  to Cb/Cr, and there are scaling and rounding to deal with.  There
 *  are also headroom and footroom in the Y' values in some formats -
 *  in these, Y' ranges over [16,235], not [0,255], while Cb and Cr are
 *  scaled into [16,240].
 *
 * Some of the transformation matrices:
 *
 *	[   Y  ' ]   [  .299    .587    .114   ] [ R' ]
 *	[ Cb-128 ] = [ -.14713 -.28886  .436   ] [ G' ]
 *	[ Cr-128 ]   [  .615   -.51499 -.10001 ] [ B' ]
 * ->
 *	[ R' ]   [ 1  0      1.13983 ] [   Y'   ]
 *	[ G' ] = [ 1 -.39465 -.5806  ] [ Cb-128 ]
 *	[ B' ]   [ 1 2.03211  0      ] [ Cr-128 ]
 *
 * We see the values come in packed, not planar.
 *
 * Other possibilities
 */

static int vfd;
static int nbufs;
static unsigned char **bufmaps;
static unsigned int *buflens;

static void open_video(void)
{
 vfd = open(videodev,O_RDWR,0);
 if (vfd < 0)
  { fprintf(stderr,"%s: %s: %s\n",__progname,videodev,strerror(errno));
    exit(1);
  }
}

static void setup_video(void)
{
 struct v4l2_requestbuffers rb;
 struct v4l2_buffer b;
 __typeof__(b.m.offset) *offsets;
 __typeof__(b.length) *lengths;
 int i;
 void *mmrv;

 rb.count = 8;
 rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 rb.memory = V4L2_MEMORY_MMAP;
 if (ioctl(vfd,VIDIOC_REQBUFS,&rb) < 0)
  { fprintf(stderr,"%s: VIDIOC_REQBUFS: %s\n",__progname,strerror(errno));
    exit(1);
  }
 nbufs = rb.count;
 printf("nbufs=%d\n",nbufs);
 offsets = malloc(nbufs*sizeof(*offsets));
 lengths = malloc(nbufs*sizeof(*lengths));
 for (i=0;i<nbufs;i++)
  { b.index = i;
    b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    b.memory = V4L2_MEMORY_MMAP;
    if (ioctl(vfd,VIDIOC_QUERYBUF,&b) < 0)
     { fprintf(stderr,"%s: VIDIOC_QUERYBUF: %s\n",__progname,strerror(errno));
       exit(1);
     }
    offsets[i] = b.m.offset;
    lengths[i] = b.length;
  }
 printf("queried\n");
 for (i=0;i<nbufs;i++)
  { printf("%lu @ %lu\n",(unsigned long int)lengths[i],(unsigned long int)offsets[i]);
  }
 bufmaps = malloc(nbufs*sizeof(*bufmaps));
 buflens = malloc(nbufs*sizeof(*buflens));
 for (i=0;i<nbufs;i++)
  { mmrv = mmap(0,lengths[i],PROT_READ|PROT_WRITE,MAP_FILE|MAP_SHARED,vfd,offsets[i]);
    if (mmrv == MAP_FAILED)
     { fprintf(stderr,"%s: mmap buffer %d: %s\n",__progname,i,strerror(errno));
       exit(1);
     }
    bufmaps[i] = mmrv;
    buflens[i] = lengths[i];
  }
 free(offsets);
 free(lengths);
 printf("mapped\n");
 for (i=0;i<nbufs;i++)
  { printf("%lu @ %p\n",(unsigned long int)buflens[i],(void *)bufmaps[i]);
  }
 i = V4L2_BUF_TYPE_VIDEO_CAPTURE; // XXX undocumented!
 if (ioctl(vfd,VIDIOC_STREAMON,&i) < 0)
  { fprintf(stderr,"%s: VIDIOC_STREAMON: %s\n",__progname,strerror(errno));
    exit(1);
  }
 printf("started\n");
}

#if 0
static void writergb(FILE *f, unsigned char y, unsigned char cb, unsigned char cr)
{
 int r;
 int g;
 int b;

 r = (65536 * y) + (91881 * (((int)cr) - 128));
 g = (65536 * y) - (22554 * (((int)cb) - 128)) - (46802 * (((int)cr) - 128));
 b = (65536 * y) + (116130 * (((int)cb) - 128));
 if (r < 0) r = 0; else if (r > 0xffffff) r = 0xff; else r >>= 16;
 if (g < 0) g = 0; else if (g > 0xffffff) g = 0xff; else g >>= 16;
 if (b < 0) b = 0; else if (b > 0xffffff) b = 0xff; else b >>= 16;
 putc(r,f);
 putc(g,f);
 putc(b,f);
}
#endif

int main(void);
int main(void)
{
 struct timeval t;
 struct timeval now;
 int n;
 int i;

 open_video();
 setup_video();
 gettimeofday(&t,0);
 t.tv_sec += 10;
 i = 0;
 while (1)
  { struct v4l2_buffer b;
    gettimeofday(&now,0);
    if ( (now.tv_sec > t.tv_sec) ||
	 ( (now.tv_sec == t.tv_sec) &&
	   (now.tv_usec > t.tv_usec) ) ) break;
    b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; // XXX undocumented!
    b.memory = V4L2_MEMORY_MMAP; // XXX undocumented!
    n = ioctl(vfd,VIDIOC_DQBUF,&b);
    if (n < 0)
     { fprintf(stderr,"%s: VIDIOC_DQBUF: %s\n",__progname,strerror(errno));
       exit(1);
     }
    i ++;
    write(1,".",1);
  }
 printf("%d\n",i);
#if 0
 for (i=0;i<10;i++)
  { char *fn;
    FILE *f;
    int x;
    int y;
    unsigned char *ip;
    asprintf(&fn,"ppm.%d",i);
    f = fopen(fn,"w");
    if (! f)
     { fprintf(stderr,"%s: %s: %s\n",__progname,fn,strerror(errno));
       exit(1);
     }
    fprintf(f,"P6\n640 480\n255\n");
    ip = &frame[i][0];
    for (y=480;y>0;y--)
     { for (x=640;x>0;x-=2)
	{ unsigned char y1;
	  unsigned char y2;
	  unsigned char cb;
	  unsigned char cr;
	  y1 = *ip++;
	  cb = *ip++;
	  y2 = *ip++;
	  cr = *ip++;
	  writergb(f,y1,cb,cr);
	  writergb(f,y2,cb,cr);
	}
     }
    fclose(f);
  }
#endif
 return(0);
}