#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>                 /* exit() */
#include <sys/ioctl.h>              /* ioctl() */
#include <libv4lconvert.h>
#include <unistd.h>
#include <sys/mman.h>               /* mmap() */
#include <errno.h>
#include <iostream>
#include "WebcamTracker.h"
using namespace std;

int main(int argc, char * argv[])
{
    WebcamTracker("/dev/video0");
}

WebcamTracker::WebcamTracker(const char * device)
{
    m_open = false;
    m_fd = open(device, O_RDWR);
    if (m_fd >= 0)
    {
        m_open = true;

        struct v4l2_capability cap;
        int ret = ioctl(m_fd, VIDIOC_QUERYCAP, &cap);
        if (ret != 0)
        {
            cerr << "VIDIOC_QUERYCAP ioctl failed: " << ret << endl;
        }
        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
        {
            cerr << "Warning: V4L2_CAP_VIDEO_CAPTURE not supported!" << endl;
        }

        v4l2_requestbuffers reqbuf;
        reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        reqbuf.count = 1;
        reqbuf.memory = V4L2_MEMORY_MMAP;
        ret = ioctl(m_fd, VIDIOC_REQBUFS, &reqbuf);
        if (ret != 0)
        {
            cerr << "VIDIOC_REQBUFS ioctl failed: returned " << ret
                 << ", errno " << errno;
            if (errno == EBUSY)
                cerr << " (EBUSY)";
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            cerr << endl;
        }

        struct v4l2_buffer buf;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.index = 0;
        ret = ioctl(m_fd, VIDIOC_QUERYBUF, &buf);
        if (ret != 0)
        {
            cerr << "VIDIOC_QUERYBUF ioctl failed: returned " << ret
                 << ", errno: " << errno;
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            cerr << endl;
        }

        m_vidbuflen = buf.length;
        m_vidbuf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED,
                        m_fd, buf.m.offset);
        if (m_vidbuf == MAP_FAILED)
        {
            cerr << "mmap() failed: errno " << errno;
            if (errno == EBADF)
                cerr << " (EBADF)";
            if (errno == EACCES)
                cerr << " (EACCES)";
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            if (errno == ENOMEM)
                cerr << " (ENOMEM)";
            cerr << endl;
        }

        v4l2_buffer qbuf;
        qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        qbuf.memory = V4L2_MEMORY_MMAP;
        qbuf.index = 0;
        ret = ioctl(m_fd, VIDIOC_QBUF, &qbuf);
        if (ret != 0)
        {
            cerr << "VIDIOC_QBUF failed! errno = " << errno;
            if (errno == EAGAIN)
                cerr << " (EAGAIN)";
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            if (errno == ENOMEM)
                cerr << " (ENOMEM)";
            if (errno == EIO)
                cerr << " (EIO)";
            cerr << endl;
        }

        int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        ret = ioctl(m_fd, VIDIOC_STREAMON, &type);
        if (ret != 0)
        {
            cerr << "VIDIOC_STREAMON ioctl failed! errno = " << errno;
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            cerr << endl;
        }

        struct v4l2_buffer querybuf;
        querybuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        querybuf.index = 0;
        ret = ioctl(m_fd, VIDIOC_QUERYBUF, &querybuf);
        if (ret != 0)
        {
            cerr << "VIDIOC_QUERYBUF ioctl failed: returned " << ret
                 << ", errno: " << errno;
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            cerr << endl;
        }
        if (querybuf.flags & V4L2_BUF_FLAG_DONE)
        {
            cout << "buf done" << endl;
        }

        v4l2_buffer dqbuf;
        dqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        dqbuf.memory = V4L2_MEMORY_MMAP;
        ret = ioctl(m_fd, VIDIOC_DQBUF, &dqbuf);
        if (ret != 0)
        {
            cerr << "VIDIOC_DQBUF failed! errno = " << errno;
            if (errno == EAGAIN)
                cerr << " (EAGAIN)";
            if (errno == EINVAL)
                cerr << " (EINVAL)";
            if (errno == ENOMEM)
                cerr << " (ENOMEM)";
            if (errno == EIO)
                cerr << " (EIO)";
            cerr << endl;
        }

    }
    else
    {
        cerr << "Could not open device '" << device << "'" << endl;
    }
}

WebcamTracker::~WebcamTracker()
{
    if (m_open)
    {
        munmap(m_vidbuf, m_vidbuflen);

        v4l2_requestbuffers reqbuf;
        reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        reqbuf.count = 0;
        reqbuf.memory = V4L2_MEMORY_MMAP;
        int ret = ioctl(m_fd, VIDIOC_REQBUFS, &reqbuf);
        if (ret != 0)
        {
            cerr << "~WebcamTracker(): VIDIOC_REQBUFS ioctl failed: "
                 << ret << endl;
            if (errno == EBUSY)
                cerr << "EBUSY" << endl;
            if (errno == EINVAL)
                cerr << "EINVAL" << endl;
        }
        close(m_fd);
    }
}