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

extern const char *__progname;

static const char *videodev = "viddev";

static int vfd;
static int nfbufs;
static unsigned char **fbufmaps;
static unsigned int *fbuflens;

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 queue_buffer(int inx)
{
 struct v4l2_buffer b;

 b.index = inx;
 b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 b.memory = V4L2_MEMORY_MMAP;
 if (ioctl(vfd,VIDIOC_QBUF,&b) < 0)
  { fprintf(stderr,"%s: VIDIOC_QBUF: %s\n",__progname,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);
  }
 nfbufs = rb.count;
#ifdef DEBUG
 fprintf(stderr,"buffer count %d\n",nfbufs);
#endif
 offsets = malloc(nfbufs*sizeof(*offsets));
 lengths = malloc(nfbufs*sizeof(*lengths));
 for (i=nfbufs-1;i>=0;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;
  }
#ifdef DEBUG
 printf("queried\n");
 for (i=nfbufs-1;i>=0;i--)
  { printf("%lu @ %lu\n",(unsigned long int)lengths[i],(unsigned long int)offsets[i]);
  }
#endif
 fbufmaps = malloc(nfbufs*sizeof(*fbufmaps));
 fbuflens = malloc(nfbufs*sizeof(*fbuflens));
 for (i=nfbufs-1;i>=0;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);
     }
    fbufmaps[i] = mmrv;
    fbuflens[i] = lengths[i];
  }
 free(offsets);
 free(lengths);
#ifdef DEBUG
 printf("mapped\n");
 for (i=nfbufs-1;i>=0;i--)
  { printf("%lu @ %p\n",(unsigned long int)buflens[i],(void *)bufmaps[i]);
  }
#endif
 for (i=nfbufs-1;i>=0;i--) queue_buffer(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);
  }
#ifdef DEBUG
 printf("started\n");
#endif
}

#define wrt(fd,buf,len) wrt_(#fd "," #buf "," #len,(fd),(buf),len)
static void wrt_(const char *s, int fd, const void *buf, int len)
{
 int n;

 n = write(fd,buf,len);
 if (n < 0)
  { fprintf(stderr,"write(%s): %s\n",s,strerror(errno));
    exit(1);
  }
 if (n != len)
  { fprintf(stderr,"write(%s): wrote %d not %d\n",s,n,len);
    exit(1);
  }
}

static int get_frame(void)
{
 struct v4l2_buffer b;
 int n;

 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);
  }
 return(b.index);
}

int main(void);
int main(void)
{
 open_video();
 setup_video();
 while (1)
  { int f;
    f = get_frame();
    wrt(1,fbufmaps[f],fbuflens[f]);
    queue_buffer(f);
  }
}