#include <stdio.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/videodev2.h>
#include <string.h>
#include <sys/mman.h>

int main(void) {
	int fd;
	if((fd = open("/dev/video0", O_RDWR)) < 0) {
		perror("open");
		exit(1);
		}

	struct v4l2_capability cap;
	if(ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
		perror("VIDIOC_QUERYCAP");
		exit(1);
		}

	if(!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		fprintf(stderr, "The device does not handle single-planar video capture.\n");
		exit(1);
		}

	printf("Video card detected: %s\n", cap.card);

	struct v4l2_format format;

	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	if(ioctl(fd, VIDIOC_G_FMT, &format) < 0) { // get format
		perror("VIDIOC_G_FMT");
		exit(1);
		}

	printf("width %d\n", format.fmt.pix.width);
	printf("height %d\n", format.fmt.pix.height);
	printf("colorspace %d\n", format.fmt.pix.colorspace);
	printf("sizeimage %d\n", format.fmt.pix.sizeimage);
	printf("pixelformat %d\n", format.fmt.pix.pixelformat);
	printf("bytesperline %d\n", format.fmt.pix.bytesperline);

//printf("Format sampling rate %d\n", format.fmt.vbi.sampling_rate);

	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	format.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;

// V4L2_PIX_FMT_RGB24

//        format.fmt.vbi.sampling_rate = 5;


	format.fmt.pix.width = 800;
	format.fmt.pix.height = 600;
	format.fmt.pix.width = 640;
	format.fmt.pix.height = 480;
	format.fmt.pix.width = 320;
	format.fmt.pix.height = 240;

	format.fmt.pix.width = 640;
	format.fmt.pix.height = 360;

	if(ioctl(fd, VIDIOC_S_FMT, &format) < 0) {
		perror("VIDIOC_S_FMT");
		exit(1);
		}

	struct v4l2_requestbuffers bufrequest;
	bufrequest.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	bufrequest.memory = V4L2_MEMORY_MMAP;
	bufrequest.count = 1;

	if(ioctl(fd, VIDIOC_REQBUFS, &bufrequest) < 0) {
		perror("VIDIOC_REQBUFS");
		exit(1);
		}

	struct v4l2_buffer bufferinfo;
	memset(&bufferinfo, 0, sizeof(bufferinfo));

	bufferinfo.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	bufferinfo.memory = V4L2_MEMORY_MMAP;
	bufferinfo.index = 0; /* Queueing buffer index 0. */

	if(ioctl(fd, VIDIOC_QUERYBUF, &bufferinfo) < 0) {
		perror("VIDIOC_QUERYBUF");
		exit(1);
		}

	void* buffer_start = mmap(
	                         NULL,
	                         bufferinfo.length,
	                         PROT_READ | PROT_WRITE,
	                         MAP_SHARED,
	                         fd,
	                         bufferinfo.m.offset
	                     );

	if(buffer_start == MAP_FAILED) {
		perror("mmap");
		exit(1);
		}

	memset(buffer_start, 0, bufferinfo.length);

	memset(buffer_start, 0, bufferinfo.length);

// Activate streaming
	int type = bufferinfo.type;
	if(ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
		perror("VIDIOC_STREAMON");
		exit(1);
		}

	/* Here is where you typically start two loops:
	 * - One which runs for as long as you want to
	 *   capture frames (shoot the video).
	 * - One which iterates over your buffers everytime. */

	while(1) {

// Put the buffer in the incoming queue.
		if(ioctl(fd, VIDIOC_QBUF, &bufferinfo) < 0) {
			perror("VIDIOC_QBUF");
			exit(1);
			}

// The buffer's waiting in the outgoing queue.
		if(ioctl(fd, VIDIOC_DQBUF, &bufferinfo) < 0) {
			perror("VIDIOC_QBUF");
			exit(1);
			}

		int jpgfile;
		if((jpgfile = open("/tmp/myimage.jpeg", O_WRONLY | O_CREAT, 0660)) < 0) {
			perror("open");
			exit(1);
			}

		write(jpgfile, buffer_start, bufferinfo.length);
		close(jpgfile);

		/* Your loops end here. */

		}

// Deactivate streaming
	if(ioctl(fd, VIDIOC_STREAMOFF, &type) < 0) {
		perror("VIDIOC_STREAMOFF");
		exit(1);
		}


	close(fd);
	return EXIT_SUCCESS;
	}

