#include "project.h"

camera * camera_alloc() {
  camera * cam = malloc(sizeof(camera));
  return(cam);
}

static void camera_init_error(const char * msg) {
  fprintf(stderr, "Error: "); perror(msg);
  exit(-1);
}

void camera_init(camera * cam, char * dev, char * height, char * width, char * nbufs) {
  unsigned int i;
  char errmsg[256];
  struct colorspace * col;

  cam->filt = NULL;
  cam->fild = NULL;

  /* zero flags */
  cam->flags ^= cam->flags;
  
  /* open device */
  cam->fd = open(dev?dev:DEFAULT_DEVICE, O_RDONLY); 
  if( cam->fd == -1 ) { 
    sprintf(errmsg, "%s", dev?dev:DEFAULT_DEVICE);
    camera_init_error(errmsg); 
  } 

  /* query capabilities */
  if( ioctl(cam->fd, VIDIOC_QUERYCAP, &cam->caps) < 0 ) { 
    close(cam->fd); camera_init_error("query capabilities failed"); 
  }  
  printf("V4L2: "); fwrite(cam->caps.card, 32, 1, stdout); printf("\n");
  printf("V4L2: "); fwrite(cam->caps.bus_info, 32, 1, stdout); printf("\n");
 
  /* get the pixelformats supported by the device */
  if( !(cam->caps.capabilities & V4L2_CAP_VIDEO_CAPTURE) ) {
    close(cam->fd); camera_init_error("device does not capture video"); 
  }
 
  cam->ifmt = -1;
  cam->nfmts = -1;
  cam->fmts = NULL;
  do {
    cam->nfmts++;
    cam->fmts = (struct v4l2_fmtdesc *) realloc( (void *) cam->fmts, (cam->nfmts + 1) * sizeof(struct v4l2_fmtdesc) );
    memset( (void *) (cam->fmts + cam->nfmts) , 0, sizeof(struct v4l2_fmtdesc));
    cam->fmts[cam->nfmts].type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    cam->fmts[cam->nfmts].index = cam->nfmts; 
  } while( ioctl(cam->fd, VIDIOC_ENUM_FMT, &cam->fmts[cam->nfmts]) >= 0 ); 
  for( i = 0; i < cam->nfmts; i++ ) {
    for( col = colorspaces; col->val != -1; col++) { 
      if( cam->fmts[i].pixelformat == col->val ) { 
        cam->ifmt = i; 
        cam->cmap = col->map;
      }
    }
  }
  if( cam->ifmt < 0 ) { close(cam->fd); camera_init_error("pixel format not supported"); }
  else { printf("V4L2: "); fwrite(cam->fmts[cam->ifmt].description, 32, 1, stdout); printf("\n"); }

  /* attempt to set the format */
  cam->fmt.fmt.pix.pixelformat = cam->fmts[cam->ifmt].pixelformat;
  cam->fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
  cam->fmt.fmt.pix.height      = height?atoi(height):DEFAULT_HEIGHT;
  cam->fmt.fmt.pix.width       = width?atoi(width):DEFAULT_WIDTH;
  cam->fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if( ioctl(cam->fd, VIDIOC_S_FMT, &cam->fmt) < 0) {
    sprintf(errmsg, "set format failed: h: %d   w: %d",   \
                    height?atoi(height):DEFAULT_HEIGHT, \
                    width?atoi(width):DEFAULT_WIDTH);
    close(cam->fd); camera_init_error(errmsg); 
  }
  else { printf("V4L2: height: %d   width: %d\n",                 \
                 cam->fmt.fmt.pix.height, \
                 cam->fmt.fmt.pix.width); }

  /* request driver set up buffers */
  memset( (void *) &cam->reqs, 0, sizeof(struct v4l2_requestbuffers));
  cam->reqs.count   = nbufs?atoi(nbufs):DEFAULT_NBUFS;
  cam->reqs.type    = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  cam->reqs.memory  = V4L2_MEMORY_MMAP;
  if( ioctl(cam->fd, VIDIOC_REQBUFS, &cam->reqs) < 0 ) {
    sprintf(errmsg, "request buffers failed: count: %d", \
                    nbufs?atoi(nbufs):DEFAULT_NBUFS);
    close(cam->fd); camera_init_error(errmsg); 
  }
  else { printf("V4L2: buffer count: %d\n", \
                cam->reqs.count); }

  /* map program buffers */
  cam->bufs = (struct buffer *) calloc(cam->reqs.count, sizeof(struct buffer));
  for(cam->nbufs = 0; cam->nbufs < cam->reqs.count; ++(cam->nbufs)) {
    memset( (void *) &cam->ibuf, 0, sizeof(struct v4l2_buffer));
    cam->ibuf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    cam->ibuf.memory = V4L2_MEMORY_MMAP;
    cam->ibuf.index  = cam->nbufs;
    if( ioctl(cam->fd, VIDIOC_QUERYBUF, &cam->ibuf) < 0 ) {
      close(cam->fd); camera_init_error("query buffer failed"); 
    }
    cam->bufs[cam->nbufs].length = cam->ibuf.length;
    cam->bufs[cam->nbufs].start = mmap( NULL, cam->ibuf.length, PROT_READ, MAP_SHARED, cam->fd, cam->ibuf.m.offset);
    if( cam->bufs[cam->nbufs].start == MAP_FAILED ) {
      close(cam->fd); camera_init_error("memory map failed"); 
    }
  }
  
  /* queue buffers */
  for(i = 0; i < cam->nbufs; ++i) {
    memset( (void *) &cam->ibuf, 0, sizeof(struct v4l2_buffer));
    cam->ibuf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    cam->ibuf.memory = V4L2_MEMORY_MMAP;
    cam->ibuf.index  = i;
    if( ioctl(cam->fd, VIDIOC_QBUF, &cam->ibuf) < 0 ) {
      close(cam->fd); camera_init_error("queue buffer failed"); 
    }
  }
    
  /* start streaming video */
  cam->ibuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if( ioctl(cam->fd, VIDIOC_STREAMON, &(cam->ibuf.type)) < 0 ) {
    close(cam->fd); camera_init_error("start streaming video failed"); 
  }


  cam->ibuf.type      = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  cam->ibuf.memory    = V4L2_MEMORY_MMAP;
  cam->ibuf.index     = 0;
  cam->rgb.buf.start  = NULL;
  cam->rgb.height     = cam->fmt.fmt.pix.height;
  cam->rgb.width      = cam->fmt.fmt.pix.width;
  cam->rgb.size       = cam->rgb.height * cam->rgb.width * 3;
}


void camera_grab_frame(camera * cam) { 
  if(cam->rgb.buf.start == NULL) {
    cam->rgb.buf.start = malloc(cam->rgb.size);
    cam->flags |= FLAG_RGB_MALLOC;
  }

  /* queue last buffer */
  if( cam->flags & FLAG_GRAB_FRAME ) { 
    if( ioctl(cam->fd, VIDIOC_QBUF, &cam->ibuf) < 0 ) {
      close(cam->fd); camera_init_error("queue buffer failed"); 
    }
  }
  else { cam->flags |= FLAG_GRAB_FRAME; }  

  /* dequeue next buffer */
  memset( (void *) &cam->ibuf, 0, sizeof(struct v4l2_buffer));
  cam->ibuf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  cam->ibuf.memory = V4L2_MEMORY_MMAP;
  cam->ibuf.index = (cam->ibuf.index + 1) % cam->nbufs;
  if( ioctl(cam->fd, VIDIOC_DQBUF, &cam->ibuf) < 0) {
    close(cam->fd); camera_init_error("dequeue buffer failed"); 
  }
   
  /* convert to rgb24 */
  (*cam->cmap)(&cam->rgb, (struct frame *) &cam->bufs[cam->ibuf.index]);
  
  /* apply filter */
  if(cam->filt != NULL) { (*cam->filt)(&cam->rgb, cam->fild); }
}

 
void camera_free(camera * cam) {
  close(cam->fd);
  free(cam->fmts);
  free(cam->bufs);
  if(cam->flags & FLAG_RGB_MALLOC) {
    free(cam->rgb.buf.start);
  }
  free(cam);
}