#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 <string.h>                 /* memset() */
#include <iostream>
#include "WebcamTracker.h"
#include "BMP.h"
using namespace std;

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

WebcamTracker::WebcamTracker()
{
    m_open = false;
    m_buffers = NULL;
    m_lengths = NULL;
}

bool WebcamTracker::open(const char * device)
{
    m_fd = ::open(device, O_RDWR);
    if (m_fd >= 0)
    {
        struct v4l2_capability cap;
        int ret = ioctl(m_fd, VIDIOC_QUERYCAP, &cap);
        if (ret != 0)
        {
            perror("VIDIOC_QUERYCAP");
        }
        else
        {
            if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
            {
                m_open = true;
            }
            else
            {
                cerr << "Warning: V4L2_CAP_VIDEO_CAPTURE not supported!" << endl;
            }
        }
    }
    else
    {
        cerr << "Could not open device '" << device << "'" << endl;
    }
    return m_open;
}

void WebcamTracker::start()
{
    int width = 640;
    int height = 480;
    setFormat(width, height);
    requestBuffers(20);
    for (int i = 0; i < m_numbufs; i++)
    {
        mapBuffer(i);
        queueBuffer(i);
    }
    beginStreaming();

    v4l2_buffer buf;
    dequeueBuffer(&buf);
    endStreaming();

    /* do something with the image referenced by buf */
    unsigned char * image_data = (unsigned char *) m_buffers[buf.index];
    unsigned char * bmp_data = new unsigned char[width * height * 3];
    for (int j = 0; j < height; j++)
    {
        for (int k = 0; k < width; k++)
        {
            unsigned char y = image_data[(width * j + k) * 2];
            bmp_data[width * j * 3 + k * 3] = y;
            bmp_data[width * j * 3 + k * 3 + 1] = y;
            bmp_data[width * j * 3 + k * 3 + 2] = y;
        }
    }
    BMP bmp("webcam.bmp", width, height, bmp_data);
    delete bmp_data;
}

void WebcamTracker::requestBuffers(int num)
{
    m_numbufs = 0;
    v4l2_requestbuffers reqbuf;
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.count = num;
    reqbuf.memory = V4L2_MEMORY_MMAP;
    int 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;
    }
    else
    {
        m_numbufs = reqbuf.count;
        m_buffers = new void *[m_numbufs];
        m_lengths = new int[m_numbufs];
    }
}

void WebcamTracker::mapBuffer(int i)
{
    struct v4l2_buffer buf;
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.index = i;
    int 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_buffers[i] = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
                        MAP_SHARED, m_fd, buf.m.offset);
    m_lengths[i] = buf.length;
    if (m_buffers[i] == 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;
    }
}

void WebcamTracker::queueBuffer(int i)
{
    v4l2_buffer qbuf;
    qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    qbuf.memory = V4L2_MEMORY_MMAP;
    qbuf.index = i;
    int 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;
    }
}

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

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

void WebcamTracker::dequeueBuffer(v4l2_buffer * dqbuf)
{
    dqbuf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    dqbuf->memory = V4L2_MEMORY_MMAP;
    int 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;
    }
}

void WebcamTracker::unmapBuffer(int i)
{
    munmap(m_buffers[i], m_lengths[i]);
}

void WebcamTracker::freeBuffers()
{
    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;
    }
}

WebcamTracker::~WebcamTracker()
{
    if (m_open)
    {
        if (m_buffers)
        {
            for (int i = 0; i < m_numbufs; i++)
            {
                unmapBuffer(i);
            }
            delete m_buffers;
            delete m_lengths;
            freeBuffers();
        }
        close(m_fd);
    }
}

void WebcamTracker::setFormat(int width, int height)
{
    v4l2_format fmt;
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(m_fd, VIDIOC_G_FMT, &fmt);
    if (ret != 0)
    {
        int err = errno;
        cerr << "Warning: VIDIOC_G_FMT ioctl failed, errno " << err;
        if (err == EBUSY)
            cerr << " (EBUSY)";
        if (err == EINVAL)
            cerr << " (EINVAL)";
        cerr << endl;
    }
    fmt.fmt.pix.width = width;
    fmt.fmt.pix.height = height;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    fmt.fmt.pix.bytesperline = width * 2;
    fmt.fmt.pix.sizeimage = fmt.fmt.pix.bytesperline * height;
    ret = ioctl(m_fd, VIDIOC_S_FMT, &fmt);
    if (ret != 0)
    {
        int err = errno;
        cerr << "Warning: VIDIOC_S_FMT ioctl failed, errno " << err;
        if (err == EBUSY)
            cerr << " (EBUSY)";
        if (err == EINVAL)
            cerr << " (EINVAL)";
        cerr << endl;
    }
}