#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <iostream>
#include <string.h>
#include <errno.h>

char v4l_output_dev[100] = "/dev/video0";
int fd_output_v4l = 0;
int g_output = 1;
int g_fmt = V4L2_PIX_FMT_UYVY;
int g_frame_size = 0;
int g_output_num_buffers = 1;


using namespace std;

int main() {

    if ((fd_output_v4l = open(v4l_output_dev, O_RDWR, 0)) < 0)
    {
        cout << "Unable to open " << v4l_output_dev << ": " << strerror(errno) << endl;
        return -1;
    }

    struct v4l2_control ctrl;
    struct v4l2_format fmt;
    struct v4l2_framebuffer fb;
    struct v4l2_cropcap cropcap;
    struct v4l2_crop crop;
    struct v4l2_requestbuffers buf_req;

    if (ioctl(fd_output_v4l, VIDIOC_S_OUTPUT, &g_output) < 0)
    {
            cout << "set output failed: " << strerror(errno) << endl;
            return -1;
    }

    memset(&cropcap, 0, sizeof(cropcap));
    cropcap.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;

    if (ioctl(fd_output_v4l, VIDIOC_CROPCAP, &cropcap) < 0)
    {
            cout << "get crop capability failed: " << strerror(errno) << endl;
            return -1;
    }

    crop.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
    crop.c.top = 0;
    crop.c.left = 0;
    crop.c.width = 800;
    crop.c.height = 480;

    if (ioctl(fd_output_v4l, VIDIOC_S_CROP, &crop) < 0)
    {
            cout << "set crop failed: " << strerror(errno) << endl;
            return -1;
    }

    ctrl.id = V4L2_CID_PRIVATE_BASE;
    ctrl.value = 0;

    if (ioctl(fd_output_v4l, VIDIOC_S_CTRL, &ctrl) < 0)
    {
        cout << "set ctrl failed: " << strerror(errno) << endl;
        return -1;
    }

    ctrl.id = V4L2_CID_PRIVATE_BASE + 3;
    ctrl.value = 0;

    if (ioctl(fd_output_v4l, VIDIOC_S_CTRL, &ctrl) < 0)
    {
        cout << "set ctrl failed: " << strerror(errno) << endl;
        return -1;
    }

    fb.flags = V4L2_FBUF_FLAG_OVERLAY;
    ioctl(fd_output_v4l, VIDIOC_S_FBUF, &fb);

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
    fmt.fmt.pix.width= 800;
    fmt.fmt.pix.height= 480;
    fmt.fmt.pix.pixelformat = g_fmt;
    fmt.fmt.pix.bytesperline = 480;
    fmt.fmt.pix.priv = 0;
    fmt.fmt.pix.sizeimage = 0;

    //if (g_tb)
        fmt.fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
    //else fmt.fmt.pix.field = V4L2_FIELD_INTERLACED_BT;

    if (ioctl(fd_output_v4l, VIDIOC_S_FMT, &fmt) < 0)
    {
        cout << "set format failed: " << strerror(errno) << endl;
        return -1;
    }

    if (ioctl(fd_output_v4l, VIDIOC_G_FMT, &fmt) < 0)
    {
        cout << "get format failed: " << strerror(errno) << endl;
        return -1;
    }

    g_frame_size = fmt.fmt.pix.sizeimage;

    memset(&buf_req, 0, sizeof(buf_req));
    buf_req.count = g_output_num_buffers;
    buf_req.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
    buf_req.memory = V4L2_MEMORY_MMAP;

    if (ioctl(fd_output_v4l, VIDIOC_REQBUFS, &buf_req) < 0)
    {
        cout << "request buffers failed: " << strerror(errno) << endl;
        return -1;
    }

    //close(fd_output_v4l);

    return 0;
}