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

struct image_buf {
	void*			lpBase;
	size_t			sLen;
};

int ioctl_wrapper(int fd, int req, void *arg) {
	int r = ioctl(fd, req, arg);
	if (r == -1) {
        perror("Error");
        close(fd);
        exit(EXIT_FAILURE);
    }

	return r;
}

void get_camera_info(int fd) {
    struct v4l2_capability dev_inf;
    ioctl_wrapper(fd, VIDIOC_QUERYCAP, &dev_inf);
    printf("-----------------------------------------------------------\n");
    printf("DEVICE INFO:\n");
    printf("-----------------------------------------------------------\n");
    printf("Driver: %s\n", dev_inf.driver);
    printf("Card: %s\n", dev_inf.card);
    printf("Bus info: %s\n", dev_inf.bus_info);
    printf("Version: %d\n", dev_inf.version);
    printf("Capabilities: %x\n", dev_inf.capabilities);
    printf("Device caps: %x\n", dev_inf.device_caps);
    printf("Capture device: %d\n", dev_inf.capabilities & V4L2_CAP_VIDEO_CAPTURE);
    printf("Read and write: %d\n", dev_inf.capabilities & V4L2_CAP_READWRITE);
    printf("Sreaming: %d\n\n", dev_inf.capabilities & V4L2_CAP_STREAMING);
    printf("-----------------------------------------------------------\n");

    struct v4l2_cropcap cc;
    memset(&cc, 0, sizeof(cc));
    cc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    ioctl_wrapper(fd, VIDIOC_CROPCAP, &cc);
    printf("SUPPORTED FORMATS:\n");
    printf("-----------------------------------------------------------\n");

    for(int idx = 0;; idx = idx + 1) {
		struct v4l2_fmtdesc fmt;

		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.index = idx;

		if(ioctl(fd, VIDIOC_ENUM_FMT, &fmt) < 0) {
			/* Failed, usually one should check the error code ... */
			break;
		}

		/* We got some format information. For demo purposes just display it */
		printf("Detected format %08x (is compressed: %s): %s\n", fmt.pixelformat, ((fmt.flags & V4L2_FMT_FLAG_COMPRESSED) != 0) ? "yes" : "no", fmt.description);
	}
    printf("\n-----------------------------------------------------------\n");
}

void set_frame_params(int fd, int width, int height, int pxfmt) {
    struct v4l2_format fmt;
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = width;
	fmt.fmt.pix.height = height;
	fmt.fmt.pix.pixelformat = pxfmt;
	fmt.fmt.pix.field = 3;
    ioctl_wrapper(fd, VIDIOC_S_FMT, &fmt);
    
    printf("FRAME INFO:\n");
    printf("-----------------------------------------------------------\n");
    printf("Resolution: %d x %d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
    if (fmt.fmt.pix.pixelformat == v4l2_fourcc('M', 'J', 'P', 'G')) {
        printf("Format: Motion-JPEG\n");
    }
    else if (fmt.fmt.pix.pixelformat == v4l2_fourcc('Y', '2', '1', '0') || 
             fmt.fmt.pix.pixelformat == v4l2_fourcc('Y', '2', '1', '2') || 
             fmt.fmt.pix.pixelformat == v4l2_fourcc('Y', '2', '1', '6')) {
        printf("Format: YUYV 4:2:2\n");        
    }
    else {
        printf("unknown\n");
    }
    printf("Field order: %d\n\n", fmt.fmt.pix.field);
}

struct image_buf map_camera_mem(int fd) {
    //----------------------------------------------------------------------
    // Checking MMAP support
    //----------------------------------------------------------------------
    struct v4l2_requestbuffers rqBuffers;

	rqBuffers.count = 1;
	rqBuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	rqBuffers.memory = V4L2_MEMORY_MMAP;

	ioctl_wrapper(fd, VIDIOC_REQBUFS, &rqBuffers);

	int bufferCount = rqBuffers.count;
    printf("Number of buffers: %d\n", bufferCount);
    if (bufferCount != 1) {
        printf("Number of buffers is not equal to 1!\n");
        exit(EXIT_FAILURE);
    }

    //----------------------------------------------------------------------
    // Mapping
    //----------------------------------------------------------------------

    struct image_buf buf;
	struct v4l2_buffer vBuffer;
	memset(&vBuffer, 0, sizeof(struct v4l2_buffer));
	/*
		Query a buffer identifying magic cookie from the driver
	*/
	vBuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vBuffer.memory = V4L2_MEMORY_MMAP;
	vBuffer.index = 0;
	ioctl_wrapper(fd, VIDIOC_QUERYBUF, &vBuffer);
	/*
		Use the mmap syscall to map the drivers buffer into our
		address space at an arbitrary location.
	*/
	buf.lpBase = mmap(NULL, vBuffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, vBuffer.m.offset);
	buf.sLen = vBuffer.length;
	if(buf.lpBase == MAP_FAILED) {
	    if (close(fd) == -1) {
            perror("Error occured: ");
        }
		exit(EXIT_FAILURE);
	} else {
        printf("Base addr: %p\n", buf.lpBase);
    }

    return buf;
}

void start_capturing(int fd) {
    //----------------------------------------------------------------------
    //  Enqueue a buffer in the driver’s incoming queue
    //----------------------------------------------------------------------
    struct v4l2_buffer vBuffer;
    memset(&vBuffer, 0, sizeof(struct v4l2_buffer));
    vBuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vBuffer.memory = V4L2_MEMORY_MMAP;
	vBuffer.index = 0;
    ioctl_wrapper(fd, VIDIOC_QBUF, &vBuffer);
    //----------------------------------------------------------------------
    //  Start video capturing
    //----------------------------------------------------------------------
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl_wrapper(fd, VIDIOC_STREAMON, &type);
}

void stop_capturing(int fd) {
    //----------------------------------------------------------------------
    //  Stop video capturing
    //----------------------------------------------------------------------
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl_wrapper(fd, VIDIOC_STREAMOFF, &type);
}

int get_frame(int fd, struct image_buf buf) {
    struct v4l2_buffer vBuffer;

    vBuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    vBuffer.memory = V4L2_MEMORY_MMAP;

    int ret = ioctl(fd, VIDIOC_DQBUF, &vBuffer);

    if (errno == EAGAIN) {
        return 0;
    } else {
        perror("Error: ");
        exit(EXIT_FAILURE);
    }
    printf("Deque: %d; Index: %d\n", ret, vBuffer.index);

    return 1;
}

int main(int argc, char* argv[]) {
    // Open binary file
    int fd2 = open("out.bin", O_RDWR | O_CREAT);
    if (fd2 == -1) {
        perror("Error occured:2 ");
        exit(EXIT_FAILURE);
    }
    // Open device
    int fd = open("/dev/video0", O_RDWR | O_NONBLOCK, 0);
    if (fd == -1) {
        perror("Error occured: ");
        exit(EXIT_FAILURE);
    }
    printf("Opened device\n");
    // Get information about device
    get_camera_info(fd);
    // Set parameters of frames (resolution and format - YUYV or MJPEG)
    set_frame_params(fd, 640, 480, V4L2_PIX_FMT_MJPEG);
    // Get buffer mapped to device memory
    struct image_buf buf;
    buf = map_camera_mem(fd);
    printf("Len: %ld\n", buf.sLen);
    // Start video capturing
    start_capturing(fd);
    printf("Started video capturing\n");
    // Getting frame
    int cnt = 0;
    
    for (int i = 0; i < 1; ++i) {
        while(1) {
            if (get_frame(fd, buf) || cnt > 10000000)
                break;
            cnt++;
        }
        printf("Cnt: %d\n", cnt);
        //write(fd2, buf.lpBase, buf.sLen);
    }
    // Unmap
    if (munmap(buf.lpBase, buf.sLen) == -1) {
        printf("munmap failed\n");
    }
    printf("successfull munmap\n");
    //Stop video capturing
    stop_capturing(fd);
    printf("Stopped video capturing\n");

    if (close(fd2) == -1) {
        perror("Error occured: ");
    }

    printf("Closed device\n");

    if (close(fd) == -1) {
        perror("Error occured: ");
    }

    return 0;
}