/*

 * capturing from UVC cam

 * requires: libjpeg-dev

 * build: gcc -std=c99 capture.c -ljpeg -o capture

 */

#include <stdint.h>

#include <stdlib.h>

#include <stdio.h>

#include <string.h>

#include <errno.h>

#include <fcntl.h>

#include <sys/ioctl.h>

#include <sys/mman.h>

#include <asm/types.h>

#include <linux/videodev2.h>

#include <sys/time.h>

#include <sys/types.h>

#include <unistd.h>

#include <jpeglib.h>

void quit(const char * msg)

{

  fprintf(stderr, "[%s] %d: %s\n", msg, errno, strerror(errno));

  exit(EXIT_FAILURE);

}

int xioctl(int fd, int request, void* arg)

{

  for (int i = ; i < ; i++) {

    int r = ioctl(fd, request, arg);

    if (r != - || errno != EINTR) return r;

  }

  return -;

}

typedef struct {

  uint8_t* start;

  size_t length;

} buffer_t;

typedef struct {

  int fd;

  uint32_t width;

  uint32_t height;

  size_t buffer_count;

  buffer_t* buffers;

  buffer_t head;

} camera_t;

camera_t* camera_open(const char * device, uint32_t width, uint32_t height)

{

  int fd = open(device, O_RDWR | O_NONBLOCK, );

  if (fd == -) quit("open");

  camera_t* camera = malloc(sizeof (camera_t));

  camera->fd = fd;

  camera->width = width;

  camera->height = height;

  camera->buffer_count = ;

  camera->buffers = NULL;

  camera->head.length = ;

  camera->head.start = NULL;

  return camera;

}

void camera_init(camera_t* camera) {

  struct v4l2_capability cap;

  if (xioctl(camera->fd, VIDIOC_QUERYCAP, &cap) == -) quit("VIDIOC_QUERYCAP");

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) quit("no capture");

  if (!(cap.capabilities & V4L2_CAP_STREAMING)) quit("no streaming");

  struct v4l2_cropcap cropcap;

  memset(&cropcap, , sizeof cropcap);

  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (xioctl(camera->fd, VIDIOC_CROPCAP, &cropcap) == ) {

    struct v4l2_crop crop;

    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    crop.c = cropcap.defrect;

    if (xioctl(camera->fd, VIDIOC_S_CROP, &crop) == -) {

      // cropping not supported

    }

  }

  struct v4l2_format format;

  memset(&format, , sizeof format);

  format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  format.fmt.pix.width = camera->width;

  format.fmt.pix.height = camera->height;

  format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;

  format.fmt.pix.field = V4L2_FIELD_NONE;

  if (xioctl(camera->fd, VIDIOC_S_FMT, &format) == -) quit("VIDIOC_S_FMT");

  struct v4l2_requestbuffers req;

  memset(&req, , sizeof req);

  req.count = ;

  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  req.memory = V4L2_MEMORY_MMAP;

  if (xioctl(camera->fd, VIDIOC_REQBUFS, &req) == -) quit("VIDIOC_REQBUFS");

  camera->buffer_count = req.count;

  camera->buffers = calloc(req.count, sizeof (buffer_t));

  size_t buf_max = ;

  for (size_t i = ; i < camera->buffer_count; i++) {

    struct v4l2_buffer buf;

    memset(&buf, , sizeof buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    buf.memory = V4L2_MEMORY_MMAP;

    buf.index = i;

    if (xioctl(camera->fd, VIDIOC_QUERYBUF, &buf) == -)

      quit("VIDIOC_QUERYBUF");

    if (buf.length > buf_max) buf_max = buf.length;

    camera->buffers[i].length = buf.length;

    camera->buffers[i].start =

      mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED,

           camera->fd, buf.m.offset);

    if (camera->buffers[i].start == MAP_FAILED) quit("mmap");

  }

  camera->head.start = malloc(buf_max);

}

void camera_start(camera_t* camera)

{

  for (size_t i = ; i < camera->buffer_count; i++) {

    struct v4l2_buffer buf;

    memset(&buf, , sizeof buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    buf.memory = V4L2_MEMORY_MMAP;

    buf.index = i;

    if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -) quit("VIDIOC_QBUF");

  }

  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (xioctl(camera->fd, VIDIOC_STREAMON, &type) == -)

    quit("VIDIOC_STREAMON");

}

void camera_stop(camera_t* camera)

{

  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (xioctl(camera->fd, VIDIOC_STREAMOFF, &type) == -)

    quit("VIDIOC_STREAMOFF");

}

void camera_finish(camera_t* camera)

{

  for (size_t i = ; i < camera->buffer_count; i++) {

    munmap(camera->buffers[i].start, camera->buffers[i].length);

  }

  free(camera->buffers);

  camera->buffer_count = ;

  camera->buffers = NULL;

  free(camera->head.start);

  camera->head.length = ;

  camera->head.start = NULL;

}

void camera_close(camera_t* camera)

{

  if (close(camera->fd) == -) quit("close");

  free(camera);

}

int camera_capture(camera_t* camera)

{

  struct v4l2_buffer buf;

  memset(&buf, , sizeof buf);

  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  buf.memory = V4L2_MEMORY_MMAP;

  if (xioctl(camera->fd, VIDIOC_DQBUF, &buf) == -) return FALSE;

  memcpy(camera->head.start, camera->buffers[buf.index].start, buf.bytesused);

  camera->head.length = buf.bytesused;

  if (xioctl(camera->fd, VIDIOC_QBUF, &buf) == -) return FALSE;

  return TRUE;

}

int camera_frame(camera_t* camera, struct timeval timeout) {

  fd_set fds;

  FD_ZERO(&fds);

  FD_SET(camera->fd, &fds);

  int r = select(camera->fd + , &fds, , , &timeout);

  if (r == -) quit("select");

  if (r == ) return FALSE;

  return camera_capture(camera);

}

void jpeg(FILE* dest, uint8_t* rgb, uint32_t width, uint32_t height, int quality)

{

  JSAMPARRAY image;

  image = calloc(height, sizeof (JSAMPROW));

  for (size_t i = ; i < height; i++) {

    image[i] = calloc(width * , sizeof (JSAMPLE));

    for (size_t j = ; j < width; j++) {

      image[i][j *  + ] = rgb[(i * width + j) *  + ];

      image[i][j *  + ] = rgb[(i * width + j) *  + ];

      image[i][j *  + ] = rgb[(i * width + j) *  + ];

    }

  }

  struct jpeg_compress_struct compress;

  struct jpeg_error_mgr error;

  compress.err = jpeg_std_error(&error);

  jpeg_create_compress(&compress);

  jpeg_stdio_dest(&compress, dest);

  compress.image_width = width;

  compress.image_height = height;

  compress.input_components = ;

  compress.in_color_space = JCS_RGB;

  jpeg_set_defaults(&compress);

  jpeg_set_quality(&compress, quality, TRUE);

  jpeg_start_compress(&compress, TRUE);

  jpeg_write_scanlines(&compress, image, height);

  jpeg_finish_compress(&compress);

  jpeg_destroy_compress(&compress);

  for (size_t i = ; i < height; i++) {

    free(image[i]);

  }

  free(image);

}

int minmax(int min, int v, int max)

{

  return (v < min) ? min : (max < v) ? max : v;

}

uint8_t* yuyv2rgb(uint8_t* yuyv, uint32_t width, uint32_t height)

{

  uint8_t* rgb = calloc(width * height * , sizeof (uint8_t));

  for (size_t i = ; i < height; i++) {

    for (size_t j = ; j < width; j += ) {

      size_t index = i * width + j;

      int y0 = yuyv[index *  + ] << ;

      int u = yuyv[index *  + ] - ;

      int y1 = yuyv[index *  + ] << ;

      int v = yuyv[index *  + ] - ;

      rgb[index *  + ] = minmax(, (y0 +  * v) >> , );

      rgb[index *  + ] = minmax(, (y0 +  * v -  * u) >> , );

      rgb[index *  + ] = minmax(, (y0 +  * u) >> , );

      rgb[index *  + ] = minmax(, (y1 +  * v) >> , );

      rgb[index *  + ] = minmax(, (y1 +  * v -  * u) >> , );

      rgb[index *  + ] = minmax(, (y1 +  * u) >> , );

    }

  }

  return rgb;

}

int main()

{

  camera_t* camera = camera_open("/dev/video0", , );

  camera_init(camera);

  camera_start(camera);

  struct timeval timeout;

  timeout.tv_sec = ;

  timeout.tv_usec = ;

  /* skip 5 frames for booting a cam */

  for (int i = ; i < ; i++) {

    camera_frame(camera, timeout);

  }

  camera_frame(camera, timeout);

  unsigned char* rgb =

    yuyv2rgb(camera->head.start, camera->width, camera->height);

  FILE* out = fopen("result.jpg", "w");

  jpeg(out, rgb, camera->width, camera->height, );

  fclose(out);

  free(rgb);

  camera_stop(camera);

  camera_finish(camera);

  camera_close(camera);

  return ;

}