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

#include "vidfile.h"
#include "display-X.h"
#include "ycbcr-rgb-cvt.h"

extern const char *__progname;

#define EXPECTED_X 640
#define EXPECTED_Y 480
#define EXPECTED_SIZE (EXPECTED_X * EXPECTED_Y * 2)

typedef struct pkt PKT;

struct pkt {
  unsigned char *b;
  int a;
  int l;
  } ;

static const char *videodev = "/dev/video0";
static int echovideo = 0;
static int thin = 1;
static double fps = -1;
static int maxdebt;
static char *arg1 = 0;

static unsigned long long int fpsbase;
static unsigned int fpsframes;
static unsigned long long int recordstart;
static int recordfd;
static VFW_HANDLE *recordh;
static int vfd;
static int vidthin;
static int nfbufs;
static unsigned char **fbufmaps;
static unsigned char *dbufs;
static unsigned int dbuffree;
static int disppipe;

#define MAXPOLL 8

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;
 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));
 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);
     }
    if (b.length != EXPECTED_SIZE)
     { fprintf(stderr,"%s: VIDIOC_QUERYBUF: buffer %d length %ld, expected %d\n",__progname,i,(long int)b.length,EXPECTED_SIZE);
       exit(1);
     }
    offsets[i] = b.m.offset;
  }
#ifdef DEBUG
 printf("queried\n");
 for (i=nfbufs-1;i>=0;i--)
  { printf("%lu @ %lu\n",(unsigned long int)EXPECTED_SIZE,(unsigned long int)offsets[i]);
  }
#endif
 fbufmaps = malloc(nfbufs*sizeof(*fbufmaps));
 for (i=nfbufs-1;i>=0;i--)
  { mmrv = mmap(0,EXPECTED_SIZE,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;
  }
 free(offsets);
#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
 vidthin = 0;
}

static void start_video(void)
{
 int i;

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

static void rcvn(int fd, void *buf, int l)
{
 int r;

 r = recv(fd,buf,l,MSG_WAITALL);
 if (r < 0)
  { fprintf(stderr,"%s: recv: %s\n",__progname,strerror(errno));
    exit(1);
  }
 if (r < l)
  { fprintf(stderr,"%s: recv wanted %d, got %d\n",__progname,l,r);
    exit(1);
  }
 if (r > l)
  { fprintf(stderr,"%s: impossible recv wanted %d, got %d\n",__progname,l,r);
    exit(1);
  }
}

static void record_frame(int fno)
{
 struct timeval now;
 unsigned long long int nowus;

 gettimeofday(&now,0);
 nowus = now.tv_usec + (now.tv_sec * 1000000ULL);
 if (recordstart == 0) recordstart = nowus;
 nowus -= recordstart;
 write_vidfile_frame(recordh,fbufmaps[fno],nowus);
}

static int fps_drop(void)
{
 struct timeval nowtv;
 unsigned long long int now;
 int desframes;

 gettimeofday(&nowtv,0);
 now = nowtv.tv_usec + (nowtv.tv_sec * 1000000ULL);
 if (fpsbase == 0)
  { fpsbase = now;
    fpsframes = 1;
    return(0);
  }
 desframes = floor(fps*(now-fpsbase)*1e-6);
 if (desframes <= fpsframes) return(1);
 if (desframes-fpsframes > maxdebt)
  { fpsbase = now - ((fpsframes+maxdebt) / (fps * 1e-6));
  }
 fpsframes ++;
 return(0);
}

static void fetch_frame(void)
{
 unsigned char f;
 unsigned char df;

 f = get_frame();
 if (vidthin)
  { queue_buffer(f);
    vidthin --;
    return;
  }
 else
  { vidthin = thin - 1;
  }
 if (fps_drop())
  { queue_buffer(f);
    return;
  }
 record_frame(f);
 if (echovideo && dbuffree)
  { if (dbuffree & 1)
     { df = 0;
       dbuffree &= ~1;
     }
    else if (dbuffree & 2)
     { df = 1;
       dbuffree &= ~2;
     }
    else
     { abort();
     }
    dbuffree &= ~(1 << df);
    bcopy(fbufmaps[f],dbufs+(df*EXPECTED_SIZE),EXPECTED_SIZE);
    wrt(disppipe,&df,1);
  }
 queue_buffer(f);
}

static void get_dispframe(void)
{
 unsigned char fno;

 rcvn(disppipe,&fno,1);
 dbuffree |= 1 << fno;
}

static void do_something(void)
{
 struct pollfd pfds[MAXPOLL];
 short int pev[MAXPOLL];
 void (*handler[MAXPOLL])(void);
 int npfd;
 int rv;
 int i;
 struct timeval now;

 void register_poll(int fd, short ev, void (*fn)(void))
  { if (npfd >= MAXPOLL) abort();
    pfds[npfd].fd = fd;
    pfds[npfd].events = ev;
    pev[npfd] = ev;
    handler[npfd] = fn;
    npfd ++;
  }

 gettimeofday(&now,0);
 npfd = 0;
 register_poll(vfd,POLLIN|POLLRDNORM,&fetch_frame);
 if (echovideo) register_poll(disppipe,POLLIN|POLLRDNORM,&get_dispframe);
 rv = poll(&pfds[0],npfd,INFTIM);
 if (rv < 0)
  { if (errno == EINTR) return;
    fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno));
    exit(1);
  }
 for (i=npfd-1;i>=0;i--)
  { if (pfds[i].revents & (pev[i]|POLLHUP|POLLERR|POLLNVAL))
     { (*handler[i])();
       return;
     }
  }
}

static int set_fps_arg(const char *arg)
{
 double v1;
 char *endp1;
 double v2;
 char *endp2;

 v1 = strtod(arg,&endp1);
 if (endp1 == arg)
  { fprintf(stderr,"%s: %s: bad -fps argument (no first value)\n",__progname,arg);
    return(1);
  }
 switch (*endp1)
  { default:
       fprintf(stderr,"%s: %s: bad -fps argument (/ isn't)\n",__progname,arg);
       return(1);
       break;
    case '\0':
       fps = v1;
       if (fps < 1) maxdebt = 1; else maxdebt = fps;
       return(0);
       break;
    case '/':
       v2 = strtod(endp1+1,&endp2);
       if (endp2 == endp1+1)
	{ fprintf(stderr,"%s: %s: bad -fps argument (no second value)\n",__progname,arg);
	  return(1);
	}
       if (*endp2)
	{ fprintf(stderr,"%s: %s: bad -fps argument (trash after second value)\n",__progname,arg);
	  return(1);
	}
       fps = v1;
       maxdebt = ceil(v1*v2);
       if (maxdebt < 1) maxdebt = 1;
       return(0);
       break;
  }
}

static void handleargs(int ac, char **av)
{
 int skip;
 int errs;

 skip = 0;
 errs = 0;
 for (ac--,av++;ac;ac--,av++)
  { if (skip > 0)
     { skip --;
       continue;
     }
    if (**av != '-')
     { if (! arg1)
	{ arg1 = *av;
	}
       else
	{ fprintf(stderr,"%s: extra argument `%s'\n",__progname,*av);
	  errs = 1;
	}
       continue;
     }
    if (0)
     {
needarg:;
       fprintf(stderr,"%s: %s needs a following argument\n",__progname,*av);
       errs = 1;
       continue;
     }
#define WANTARG() do { if (++skip >= ac) goto needarg; } while (0)
    if (!strcmp(*av,"-camera"))
     { WANTARG();
       videodev = av[skip];
       continue;
     }
    if (!strcmp(*av,"-fps"))
     { WANTARG();
       errs |= set_fps_arg(av[skip]);
       continue;
     }
    if (!strcmp(*av,"-thin"))
     { WANTARG();
       thin = atoi(av[skip]);
       if (thin < 1)
	{ fprintf(stderr,"%s: -thin value must be >= 1\n",__progname);
	  errs = 1;
	}
       continue;
     }
    if (!strcmp(*av,"-echo"))
     { echovideo = 1;
       continue;
     }
#undef WANTARG
    fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av);
    errs = 1;
  }
 if (errs) exit(1);
}

static void setup_dbufs(void)
{
 void *mmrv;

 mmrv = mmap(0,EXPECTED_SIZE*2,PROT_READ|PROT_WRITE,MAP_ANON|MAP_SHARED,-1,0);
 if (mmrv == MAP_FAILED)
  { fprintf(stderr,"%s: buffer mmap (%u): %s\n",__progname,(unsigned int)(EXPECTED_SIZE*2),strerror(errno));
    exit(1);
  }
 dbufs = mmrv;
 bzero(dbufs,EXPECTED_SIZE*2);
}

static void displayer_main(void)
{
 unsigned char c;
 CVTPARAMS p;

 setup_X();
 p.Kr = .299;
 p.Kb = .114;
 p.minY = 0;
 p.maxY = 255;
 p.maxCC = 127;
 reset_cvt(&p);
 wrt(disppipe,"\0\1",2);
 while (1)
  { rcvn(disppipe,&c,1);
    display_frame(dbufs+(c*EXPECTED_SIZE));
    wrt(disppipe,&c,1);
  }
}

static void fork_displayer(void)
{
 int p[2];
 int pid;

 if (socketpair(AF_LOCAL,SOCK_STREAM,0,&p[0]) < 0)
  { fprintf(stderr,"%s: socketpair: %s\n",__progname,strerror(errno));
    exit(1);
  }
 fflush(0);
 pid = fork();
 if (pid == 0)
  { close(p[0]);
    disppipe = p[1];
    displayer_main();
    _exit(0);
  }
 close(p[1]);
 disppipe = p[0];
 dbuffree = 0;
}

static void record_writefn(void *cookie __attribute__((__unused__)), const void *buf, int len)
{
 if (len < 0) abort();
 write(recordfd,buf,len);
}

static void setup_recording(const char *to)
{
 recordfd = open(to,O_WRONLY|O_CREAT|O_TRUNC,0666);
 if (recordfd < 0)
  { fprintf(stderr,"%s: %s: %s\n",__progname,to,strerror(errno));
    exit(1);
  }
 recordstart = 0;
 recordh = open_vidfile_write(&record_writefn,0,EXPECTED_X,EXPECTED_Y,VF_FMT_YCbCr422_YCbYCr_0_255,VFF_TIMESTAMPS);
}

int main(int, char **);
int main(int ac, char **av)
{
 handleargs(ac,av);
 if (! arg1)
  { fprintf(stderr,"Usage: %s [options] file\n",__progname);
    exit(1);
  }
 setup_recording(arg1);
 open_video();
 setup_video();
 if (echovideo)
  { setup_dbufs();
    fork_displayer();
  }
 start_video();
 while (1) do_something();
}