#include "V4l2MmapDevice.h"
#include "CCycleBuffer.h"

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

// libv4l2
#include <linux/videodev2.h>

// project
#include "../kylinlogger/kylinlogger_CPP/kylinlogger_cpp.h"
#include <chrono>


int g_index = 0;
FILE *g_fp= NULL;
V4l2MmapDevice::V4l2MmapDevice(const V4L2DeviceParameters & params, v4l2_buf_type deviceType) : V4l2Device(params, deviceType), n_buffers(0)
{
    memset(&m_buffer, 0, sizeof(m_buffer));
}

bool V4l2MmapDevice::init(unsigned int mandatoryCapabilities)
{
    g_index =0;
    g_fp = NULL;
    bool ret = V4l2Device::init(mandatoryCapabilities);
    if (ret)
    {
        ret = this->start();
        ret = this->stop();
        ret = this->start();
    }
    return ret;
}

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


bool V4l2MmapDevice::start()
{
    LOG(V4L2_LOG,NOTICE) << "Device " << m_params.m_devName;
    std::cout.flush();
    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)
        {
            LOG(V4L2_LOG,ERROR) << "Device " << m_params.m_devName << " does not support memory mapping";
            success = false;
        }
        else
        {
            perror("VIDIOC_REQBUFS");
            success = false;
        }
    }
    else
    {
        LOG(V4L2_LOG,NOTICE) << "Device " << m_params.m_devName << " nb buffer:" << req.count;

        // allocate buffers
        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))
            {
                perror("VIDIOC_QUERYBUF");
                success = false;
            }
            else
            {
                LOG(V4L2_LOG,INFO) << "Device " << m_params.m_devName << " buffer idx:" << n_buffers << " size:" << buf.length << " offset:" << 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 /* start anywhere */,
                                            m_buffer[n_buffers].length,
                                            PROT_READ | PROT_WRITE /* required */,
                                            MAP_SHARED /* recommended */,
                                            m_fd,
                                            buf.m.offset);

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

        // queue buffers
        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))
            {
                perror("VIDIOC_QBUF");
                success = false;
            }
        }

        // start stream
        int type = m_deviceType;
        if (-1 == ioctl(m_fd, VIDIOC_STREAMON, &type))
        {
            perror("VIDIOC_STREAMON");
            success = false;
        }
    }
    return success;
}

bool V4l2MmapDevice::stop()
{
    LOG(V4L2_LOG,NOTICE) << "Device " << m_params.m_devName;

    bool success = true;

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

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

    // free buffers
    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))
    {
        perror("VIDIOC_REQBUFS");
        success = false;
    }

    n_buffers = 0;
    return success;
}

size_t V4l2MmapDevice::readInternal(char* buffer, size_t bufferSize)
{
    static int iRecvCnt = 0;
    static  auto lastTime = std::chrono::high_resolution_clock::now();
    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))
        {
            perror("VIDIOC_DQBUF");
            size = -1;
        }
        else if (buf.index < n_buffers)
        {
          auto end = std::chrono::high_resolution_clock::now();
            size = buf.bytesused;
            {
              iRecvCnt ++;
              std::chrono::duration<double, std::milli> elapsed =std::chrono::high_resolution_clock::now() - lastTime;
              if (elapsed.count()>=1000) {
                  printf("%s: fps = %d \n" , __func__ , iRecvCnt);
                  iRecvCnt = 0;
                  lastTime = std::chrono::high_resolution_clock::now();
              }
            }
            if (size > bufferSize)
            {
                size = bufferSize;
                LOG(V4L2_LOG,WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << bufferSize << " needed:" << buf.bytesused;
            }
            //memcpy(buffer, m_buffer[buf.index].start, size);
            ((CCycleBuffer *)buffer)->Write((uint8_t *)m_buffer[buf.index].start, size);
            {

              if (0 && g_index<10)
              {

                g_index++;

                // if(g_fp == NULL)
                //   g_fp= fopen("./001.mp4", "wb+");
                // if (g_fp)
                // {
                //   fwrite(m_buffer[buf.index].start, size, 1, g_fp);
                //   fflush(g_fp);
                // }


                  // char chFileName[100];
                  // sprintf(chFileName , "./00%d.jpg" , g_index);
                  // FILE *fpJpeg=fopen(chFileName , "wb");
                  // if (fpJpeg) {
                  //     fwrite(m_buffer[buf.index].start, size, 1, fpJpeg);
                  //     fclose(fpJpeg);
                  // }

              }
            }
            if (-1 == ioctl(m_fd, VIDIOC_QBUF, &buf))
            {
                perror("VIDIOC_QBUF");
                size = -1;
            }

        }
    }
    return size;
}

size_t V4l2MmapDevice::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))
        {
            perror("VIDIOC_DQBUF");
            size = -1;
        }
        else if (buf.index < n_buffers)
        {
            size = bufferSize;
            if (size > buf.length)
            {
                LOG(V4L2_LOG,WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << buf.length << " needed:" << size;
                size = buf.length;
            }
            memcpy(m_buffer[buf.index].start, buffer, size);
            buf.bytesused = size;

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

bool V4l2MmapDevice::startPartialWrite(void)
{
    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))
    {
        perror("VIDIOC_DQBUF");
        return false;
    }
    m_partialWriteBuf.bytesused = 0;
    m_partialWriteInProgress = true;
    return true;
}

size_t V4l2MmapDevice::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)
            {
                LOG(V4L2_LOG,WARN) << "Device " << m_params.m_devName << " buffer truncated available:" << m_partialWriteBuf.length << " needed:" << 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 V4l2MmapDevice::endPartialWrite(void)
{
    if (!m_partialWriteInProgress)
        return false;
    if (n_buffers <= 0)
    {
        m_partialWriteInProgress = false; // abort partial write
        return true;
    }
    if (-1 == ioctl(m_fd, VIDIOC_QBUF, &m_partialWriteBuf))
    {
        m_partialWriteInProgress = false; // abort partial write
        return true;
    }
    m_partialWriteInProgress = false;
    return true;
}
