#include "sv4l2device.h"

#include <stdlib.h>
#include <assert.h>
#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <string.h> // strerrno
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

//#include <stdexcept>
#include <sys/klog.h>
#include <linux/videodev2.h>

#include <libv4l2.h>

#include "mainwindow.h"
#include "commondata.h"

#include <QElapsedTimer>

int SV4L2Device::m_nDeviceID = -1;

SV4L2Device::SV4L2Device()
{
    m_nVideoWidth = 3840;
    m_nVideoHeight = 2160;

    m_nDeviceID = 0;
}

SV4L2Device::~SV4L2Device()
{

}

bool SV4L2Device::CheckDevice(QString strDevice)
{
    bool bCheck = false;

    return bCheck;
}

int SV4L2Device::xioctl(int fh, unsigned long int request, void *arg)
{
      int r = 0;

      int nCount = 0;

      do {
            r = ioctl(fh, request, arg);
            nCount++;
      } while (-1 == r && EINTR == errno && nCount<50);

      if(nCount>=50)
      {
          r = -1;
      }

      return r;
}

int SV4L2Device::open_device(QString strDevice)
{
    //m_nDeviceID = v4l2_open(strDevice.toStdString().c_str(), O_RDWR | O_NONBLOCK, 0);

    struct stat st;
    int fd = 0;

    if (-1 == stat(strDevice.toStdString().c_str(), &st)) {
        return -1;
    }

    if (!S_ISCHR(st.st_mode)) {
        return -2;
    }

    fd = open(strDevice.toStdString().c_str(), O_RDWR | O_NONBLOCK, 0);

    if (-1 == fd) {
        return -3;
    }

    m_nDeviceID = fd;

    v4l2_format format = {0};
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width = m_nVideoWidth;
    format.fmt.pix.height = m_nVideoHeight;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;//V4L2_PIX_FMT_YUYV //V4L2_PIX_FMT_RGB24
    format.fmt.pix.field = V4L2_FIELD_NONE; //V4L2_FIELD_NONE
    xioctl(m_nDeviceID, VIDIOC_S_FMT, &format);

    memset(&m_RequestBuffer, 0, sizeof(v4l2_requestbuffers));
    m_RequestBuffer.count = 1;
    m_RequestBuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    m_RequestBuffer.memory = V4L2_MEMORY_MMAP;
    //xioctl(m_nDeviceID, VIDIOC_REQBUFS, &m_RequestBuffer);
    ioctl(m_nDeviceID, VIDIOC_REQBUFS, &m_RequestBuffer);

    ///mapping buffers
    unsigned int i;
    m_pBufferLMS = (LMSBBB_buffer*) calloc(m_RequestBuffer.count, sizeof(*m_pBufferLMS));
    for (i = 0; i < m_RequestBuffer.count; i++) {
        clear_memmory(&(m_Buffer));

        (m_Buffer).type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        (m_Buffer).memory      = V4L2_MEMORY_MMAP;
        (m_Buffer).index       = i;

        xioctl(m_nDeviceID, VIDIOC_QUERYBUF, &m_Buffer);

        m_pBufferLMS[i].length = (m_Buffer).length;
        printf("A buff has a len of: %i\n",m_pBufferLMS[i].length);
        m_pBufferLMS[i].start = v4l2_mmap(NULL, (m_Buffer).length, PROT_READ | PROT_WRITE, MAP_SHARED, m_nDeviceID, (m_Buffer).m.offset);

        if (MAP_FAILED == m_pBufferLMS[i].start) {
            perror("Can not map the m_pBufferLMS.");
            exit(EXIT_FAILURE);
        }
    }

    for (i = 0; i < m_RequestBuffer.count; i++) {
        clear_memmory(&(m_Buffer));
        (m_Buffer).type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        (m_Buffer).memory = V4L2_MEMORY_MMAP;
        (m_Buffer).index = i;
        ioctl(m_nDeviceID,VIDIOC_QBUF, &(m_Buffer));
    }



    return m_nDeviceID;

    /*
    struct stat st;
    int fd = 0;

    if (-1 == stat(strDevice.c_str(), &st)) {
        return -1;
    }

    if (!S_ISCHR(st.st_mode)) {
        return -2;
    }

    fd = open(strDevice.c_str(), O_RDWR | O_NONBLOCK, 0);

    if (-1 == fd) {
        return -3;
    }

    return fd;
    */
}



template <typename typeXX>
void SV4L2Device::clear_memmory(typeXX* x)
{
    memset(x, 0, sizeof(*x));
}


void SV4L2Device::StreamOn()
{
    v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl(m_nDeviceID, VIDIOC_STREAMON, &type);


    int pic_count=0;
    ///CAPTURE
    fd_set fds;
    struct timeval tv;
    int r;
    char out_name[256];
    FILE* fout;



    do
    {
        FD_ZERO(&fds);
        FD_SET(m_nDeviceID, &fds);

        // Timeout.
        tv.tv_sec = 2;
        tv.tv_usec = 16;

        r = select(m_nDeviceID + 1, &fds, NULL, NULL, &tv);
    } while ((r == -1 && (errno = EINTR)));

    if (r == -1) {
        perror("select");
        exit(EXIT_FAILURE);
    }

    int i=0;
    vector<uint8_t*> listData;

    for(i=0 ; i<60 ; i++)
    {
        uint8_t* pData = new uint8_t[3840*2160*2];
        memcpy(pData, m_pBufferLMS[(m_Buffer).index].start, (m_Buffer).bytesused);
        listData.push_back(pData);
    }

    QElapsedTimer timeCheck;

    QElapsedTimer timeCheckTotal;

    timeCheck.restart();
    timeCheckTotal.restart();

    int nReceivedCount = 0;
    int nStoreCount = 0;


    for(i=0 ; i<600 ; i++)
    {



        clear_memmory(&(m_Buffer));
        (m_Buffer).type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        (m_Buffer).memory = V4L2_MEMORY_MMAP;
        //(m_Buffer).index = i%10;

        int nRetry = 0;
        do{
            //xioctl(m_nDeviceID, VIDIOC_DQBUF, &(m_Buffer));
            ioctl(m_nDeviceID, VIDIOC_DQBUF, &(m_Buffer));
            nRetry++;
        }while((m_Buffer).bytesused==0);

        //qDebug() << nRetry;

        ioctl(m_nDeviceID,VIDIOC_QBUF, &(m_Buffer));

        /*

        printf("Buff index: %i\n",(m_Buffer).index);
        sprintf(out_name, "image%03d_%03d_%x.ppm",pic_count, (m_Buffer).index, m_pBufferLMS[(m_Buffer).index].start);
        fout = fopen(out_name, "w");
        if (!fout) {
            perror("Cannot open image");
            exit(EXIT_FAILURE);
        }
        fprintf(fout, "P6\n%d %d 255\n",m_nVideoWidth, m_nVideoHeight);
        fwrite(m_pBufferLMS[(m_Buffer).index].start, (m_Buffer).bytesused, 1, fout);
        fclose(fout);

        */

        timeCheck.restart();
        uint8_t* pData = listData[i%60];
        memcpy(pData, m_pBufferLMS[(m_Buffer).index].start, (m_Buffer).bytesused);
        usleep(10);

        int nTime = timeCheck.elapsed();
        qDebug() << nTime;



        pic_count++;

    }

    int nTimeTotal = timeCheckTotal.elapsed();
    qDebug() << nTimeTotal;

    pic_count = 0;
    for(i=0 ; i<listData.size() ; i++)
    {
        uint8_t* pData = listData[i];
        sprintf(out_name, "image%03d.ppm",pic_count);
        fout = fopen(out_name, "w");
        if (!fout) {
            perror("Cannot open image");
            exit(EXIT_FAILURE);
        }
        fprintf(fout, "P6\n%d %d 255\n",m_nVideoWidth, m_nVideoHeight);
        fwrite(pData, 3840*2160*2, 1, fout);
        fclose(fout);
        pic_count++;
    }


    clear_memmory(&(m_Buffer));
    (m_Buffer).type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    (m_Buffer).memory = V4L2_MEMORY_MMAP;
    xioctl(m_nDeviceID, VIDIOC_DQBUF, &(m_Buffer));
    printf("Buff index: %i\n",(m_Buffer).index);
    sprintf(out_name, "image%03d.ppm",pic_count);
    fout = fopen(out_name, "w");
    if (!fout) {
        perror("Cannot open image");
        exit(EXIT_FAILURE);
    }
    fprintf(fout, "P6\n%d %d 255\n",m_nVideoWidth, m_nVideoHeight);
    fwrite(m_pBufferLMS[(m_Buffer).index].start, (m_Buffer).bytesused, 1, fout);
    fclose(fout);
    pic_count++;


}