#include <string.h>
#include <fcntl.h>
#include <stdio.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>

#include "MmapDevice.h"
#include "Logger.h"

namespace Video
{
namespace V4L2
{
MmapDevice::MmapDevice(const DeviceParameters& params, v4l2_buf_type deviceType)
    : Device(params, deviceType), n_buffers(0)
{
    memset(&m_buffer, 0, sizeof(m_buffer));
}

bool MmapDevice::init(unsigned int mandatoryCapabilities)
{
    bool ret = Device::init(mandatoryCapabilities);
    if (ret)
    {
        ret = this->start();
    }
    return ret;
}

MmapDevice::~MmapDevice()
{
    this->stop();
}

bool MmapDevice::start()
{
    bool                       success = true;
    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count  = V4L2MMAP_NBBUFFER;
    req.type   = m_deviceType;
    req.memory = V4L2_MEMORY_MMAP;

    if (-1 == ioctl(m_fd, VIDIOC_REQBUFS, &req))
    {
        if (EINVAL == errno)
        {
            logger().error("%s: does not support memory mapping", m_params.m_devName);
            success = false;
        }
        else
        {
            logger().error("VIDIOC_REQBUFS");
            success = false;
        }
    }
    else
    {
        memset(&m_buffer, 0, sizeof(m_buffer));
        for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
        {
            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof(buf));
            buf.type   = m_deviceType;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index  = n_buffers;

            if (-1 == ioctl(m_fd, VIDIOC_QUERYBUF, &buf))
            {
                logger().error("VIDIOC_QUERYBUF");
                success = false;
            }
            else
            {
                logger().debug("%s: [%u]: size: %u offset: %u", m_params.m_devName, (uint32_t)n_buffers,
                               (uint32_t)buf.length, (uint32_t)buf.m.offset);

                m_buffer[n_buffers].length = buf.length;
                if (!m_buffer[n_buffers].length)
                {
                    m_buffer[n_buffers].length = buf.bytesused;
                }
                m_buffer[n_buffers].start =
                    mmap(NULL, m_buffer[n_buffers].length, PROT_READ | PROT_WRITE, MAP_SHARED, m_fd, buf.m.offset);

                if (MAP_FAILED == m_buffer[n_buffers].start)
                {
                    logger().error("mmap");
                    success = false;
                }
            }
        }

        for (unsigned int i = 0; i < n_buffers; ++i)
        {
            struct v4l2_buffer buf;
            memset(&buf, 0, sizeof(buf));
            buf.type   = m_deviceType;
            buf.memory = V4L2_MEMORY_MMAP;
            buf.index  = i;

            if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
            {
                logger().error("VIDIOC_QBUF");
                success = false;
            }
        }

        int type = m_deviceType;
        if (-1 == ioctl(m_fd, VIDIOC_STREAMON, &type))
        {
            logger().error("VIDIOC_STREAMON");
            success = false;
        }
    }
    return success;
}

bool MmapDevice::stop()
{
    bool success = true;

    int type = m_deviceType;
    if (-1 == ioctl(m_fd, VIDIOC_STREAMOFF, &type))
    {
        logger().error("VIDIOC_STREAMOFF");
        success = false;
    }

    for (unsigned int i = 0; i < n_buffers; ++i)
    {
        if (-1 == munmap(m_buffer[i].start, m_buffer[i].length))
        {
            logger().error("munmap");
            success = false;
        }
    }

    struct v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count  = 0;
    req.type   = m_deviceType;
    req.memory = V4L2_MEMORY_MMAP;
    if (-1 == ioctl(m_fd, VIDIOC_REQBUFS, &req))
    {
        logger().error("VIDIOC_REQBUFS");
        success = false;
    }

    n_buffers = 0;
    return success;
}

size_t MmapDevice::readInternal(char* buffer, size_t bufferSize)
{
    size_t size = 0;
    if (n_buffers > 0)
    {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type   = m_deviceType;
        buf.memory = V4L2_MEMORY_MMAP;

        if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &buf))
        {
            if (errno == EAGAIN)
            {
                size = 0;
            }
            else
            {
                logger().error("VIDIOC_DQBUF");
                size = -1;
            }
        }
        else if (buf.index < n_buffers)
        {
            size = buf.bytesused;
            if (size > bufferSize)
            {
                size = bufferSize;
                logger().warning("%s: buffer truncated available: %u needed: %u", m_params.m_devName,
                                 (uint32_t)bufferSize, (uint32_t)buf.bytesused);
            }
            memcpy(buffer, m_buffer[buf.index].start, size);

            if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
            {
                logger().error("VIDIOC_QBUF");
                size = -1;
            }
        }
    }
    return size;
}

size_t MmapDevice::writeInternal(char* buffer, size_t bufferSize)
{
    size_t size = 0;
    if (n_buffers > 0)
    {
        struct v4l2_buffer buf;
        memset(&buf, 0, sizeof(buf));
        buf.type   = m_deviceType;
        buf.memory = V4L2_MEMORY_MMAP;

        if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &buf))
        {
            logger().error("VIDIOC_DQBUF");
            size = -1;
        }
        else if (buf.index < n_buffers)
        {
            size = bufferSize;
            if (size > buf.length)
            {
                logger().warning("%s: buffer truncated available: %u needed: %u", m_params.m_devName,
                                 (uint32_t)buf.length, (uint32_t)size);
                size = buf.length;
            }
            memcpy(m_buffer[buf.index].start, buffer, size);
            buf.bytesused = size;

            if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
            {
                logger().error("VIDIOC_QBUF");
                size = -1;
            }
        }
    }
    return size;
}

bool MmapDevice::startPartialWrite()
{
    if (n_buffers <= 0)
        return false;
    if (m_partialWriteInProgress)
        return false;
    memset(&m_partialWriteBuf, 0, sizeof(m_partialWriteBuf));
    m_partialWriteBuf.type   = m_deviceType;
    m_partialWriteBuf.memory = V4L2_MEMORY_MMAP;
    if (-1 == ioctl(m_fd, VIDIOC_DQBUF, &m_partialWriteBuf))
    {
        logger().error("VIDIOC_DQBUF");
        return false;
    }
    m_partialWriteBuf.bytesused = 0;
    m_partialWriteInProgress    = true;
    return true;
}

size_t MmapDevice::writePartialInternal(char* buffer, size_t bufferSize)
{
    size_t new_size = 0;
    size_t size     = 0;
    if ((n_buffers > 0) && m_partialWriteInProgress)
    {
        if (m_partialWriteBuf.index < n_buffers)
        {
            new_size = m_partialWriteBuf.bytesused + bufferSize;
            if (new_size > m_partialWriteBuf.length)
            {
                logger().warning("%s: buffer truncated available: %u needed: %u", m_params.m_devName,
                                 (uint32_t)m_partialWriteBuf.length, (uint32_t)new_size);
                new_size = m_partialWriteBuf.length;
            }
            size = new_size - m_partialWriteBuf.bytesused;
            memcpy(&((char*)m_buffer[m_partialWriteBuf.index].start)[m_partialWriteBuf.bytesused], buffer, size);

            m_partialWriteBuf.bytesused += size;
        }
    }
    return size;
}

bool MmapDevice::endPartialWrite()
{
    if (!m_partialWriteInProgress)
        return false;
    if (n_buffers <= 0)
    {
        m_partialWriteInProgress = false;
        return true;
    }
    if (-1 == ioctl(m_fd, VIDIOC_QBUF, &m_partialWriteBuf))
    {
        logger().error("VIDIOC_QBUF");
        m_partialWriteInProgress = false;
        return true;
    }
    m_partialWriteInProgress = false;
    return true;
}
}
}