// C++ / V4L2 Includes
#include <linux/videodev2.h>
#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <sstream>
#include <vector>

int main(int argc, char *argv[])
{
    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);
    }

    struct v4l2_format format;
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG;
    format.fmt.pix.width = 2592;
    format.fmt.pix.height = 1944;

    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;
    int old_count = bufrequest.count = 3;

    if(ioctl(fd, VIDIOC_REQBUFS, &bufrequest) < 0){
        perror("VIDIOC_REQBUFS");
        exit(1);
    }
    std::cout << "old bufrequest.count = " << old_count
              << ", new bufrequest.count = " << bufrequest.count << std::endl;

    struct buffer_t {
        struct v4l2_buffer info;
        void* start;
    };
    std::vector<struct buffer_t> buffers;
    for (int i = 0; i < bufrequest.count; i++) {
        struct v4l2_buffer bufferinfo;
        memset(&bufferinfo, 0, sizeof(bufferinfo));
        bufferinfo.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        bufferinfo.memory = V4L2_MEMORY_MMAP;
        bufferinfo.index = i;
        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);
        // Put a buffer in the incoming queue.
        if(ioctl(fd, VIDIOC_QBUF, &bufferinfo) < 0){
            perror("VIDIOC_QBUF");
            exit(1);
        }
        buffers.emplace_back(buffer_t { bufferinfo, buffer_start });
    }

    // Activate streaming
    int type = buffers[0].info.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. */

    bool capture_is_running = true;
    int counter = 0;
    while(capture_is_running){
        for(int i = 0; i < bufrequest.count; i++){
            // The buffer's waiting in the outgoing queue.
            if(ioctl(fd, VIDIOC_DQBUF, &buffers[i].info) != 0){
                perror("VIDIOC_DQBUF");
                exit(1);
            }

            int jpgfile;
            std::stringstream ss;
            ss << "somefile" << counter << ".jpg";
            if ((jpgfile = open(ss.str().c_str(), O_WRONLY | O_CREAT, 0660)) < 0) {
                perror("open");
                exit(1);
            }
            write(jpgfile, buffers[i].start, buffers[i].info.length);
            close(jpgfile);
            std::cout << "wrote file " << ss.str() << std::endl;
            if (++counter == 10) {
                capture_is_running = false;
                break;
            }

            // Put a buffer in the incoming queue.
            if(ioctl(fd, VIDIOC_QBUF, &buffers[i].info) < 0){
                perror("VIDIOC_QBUF");
                exit(1);
            }
        }

    }

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

    close(fd);
    exit(0);
}