#include "innomakerov9281.h"

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

#include <iostream>
#include <span>

#include <QElapsedTimer>

#include "constants.h"
// #include "rotaryencoder.h"

#define LOGD(...) \
    do \
    { \
        printf(__VA_ARGS__); \
        printf("\n"); \
    } while (0)

#define DBG(fmt, args...) LOGD("%s:%d, " fmt, __FUNCTION__, __LINE__, ##args);

extern uint64_t dq_elapsed_ns;
extern uint64_t get_elapsed_ns;
extern uint64_t sum_elapsed_ns;
extern uint64_t corr_elapsed_ns;
extern uint64_t max_elapsed_ns;
extern uint64_t value_elapsed_ns;
extern uint64_t rot_elapsed_ns;

// constexpr char videoDevice[] = "/dev/video0";

InnoMakerOV9281::InnoMakerOV9281() {}

InnoMakerOV9281::~InnoMakerOV9281()
{
    m_streamThread.request_stop();
    m_streamThread.join();

    m_someThread.request_stop();
    m_someThread.join();

    int ret{-1};

    for (int i = 0; i < BUFFER_COUNT; ++i)
    {
        ret = munmap(video_buffer_ptr[i], img_size);

        if (ret < 0)
        {
            DBG("Munmap failed!!.");
        }
    }

    // std::cout << __func__ << std::endl;
    if (m_cam_fd >= 0)
    {
        int ret = close(m_cam_fd);

        if (ret == -1)
        {
            // std::cout << __func__
            //           << ": cannot close camera: " << strerror(errno)
            //           << std::endl;
        }
    };

    // std::cout << __func__ << ": success" << std::endl;
}

std::vector<std::shared_ptr<ICamera> > InnoMakerOV9281::search()
{
    // return only one camera for now
    std::cout << std::boolalpha;
    auto cam = std::make_shared<InnoMakerOV9281>();

    if (!cam->init())
        return {};

    if (!cam->setExposureTimeUs(68000))
        return {};

    if (!cam->setLaserLevel(1))
        return {};

    if (!cam->setGain(2))
        return {};

    if (!cam->setSomething(0)) {
        return {};
    }

    // m_someThread = std::jthread{[=](std::stop_token stopToken) {
    //     std::cout << "InnoMakerOV9281: start stream" << std::endl;
    //     sleep(5);

    //     static int i = 0;
    //     while (!stopToken.stop_requested()) {
    //         cam->setSomething(i);
    //         i -= 1;
    //     }
    // }};

    return {cam};
}

bool InnoMakerOV9281::startStream()
{
    int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    auto ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);

    if (ret != 0) {
        DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
        return false;
    }

    m_streamThread = std::jthread{[&](std::stop_token stopToken) {
        std::cout << "InnoMakerOV9281: start stream" << std::endl;

        // Image image;
        std::shared_ptr<Image> imageCopy = std::make_shared<Image>();
        while (!stopToken.stop_requested())
        {
            size_t imageIndex{};

            // if (!getImage(image))
            if (!getImage(imageIndex))
                break;

            auto image = m_images[imageIndex];

            // // FIXME: backup emit value
            // #ifdef emit
            // #undef emit
            //             // std::cout << "emit new image" << std::endl << std::flush;
            //             // newImage.emit(image);
            if (newImageCallback) {
                // memcpy(imageCopy.get(), &image, sizeof(Image));
                // newImageCallback(imageCopy);
                newImageCallback(image);
            }
            // continue;
            //             // // newPixels.emit(pixels);
            // auto &img = *image;
            // image->rotate();
            // img.rotate();
            image.rotate();
            //             image.rotate();
            if (newPixelsCallback) {
                // auto pixels = image->pixels();
                auto pixels = image.pixels();
                newPixelsCallback(pixels);
            }
            continue;
        }
        // #define emit
        // #endif
        std::cout << "InnoMakerOV9281: stream interruption requested"
                  << std::endl;
    }};

    return true;
}

bool InnoMakerOV9281::init()
{
    if (!openCam())
        return false;

    if (!selectCam())
        return false;

    if (!initCam())
        return false;

    return true;
}

bool InnoMakerOV9281::setExposureTimeUs(int valueUs)
{
    std::cout << __func__ << ": " << valueUs << std::endl << std::flush;

    /*
     * Shutter Time. Value is from 8721ns to 8721*885ns, must be integral
     * multiple of 8721ns .
     * 8721xN(N =1,2,3,4,5.....855)
     */
    // constexpr int exposureStep{8721};
    // constexpr int maxExposureStepMultiplier{885};
    // auto valueNs = valueUs;
    // valueNs = (valueNs / exposureStep) * exposureStep;

    // std::clamp(valueNs, exposureStep, exposureStep * maxExposureStepMultiplier);
    // setGain(rand() % 254);
    // setGain(3);
    // setLaserLevel(rand() % 0x7fffffff);
    // setLaserLevel(rand() % 100);
    // int exp = rand() % 10;
    // return setCamParam(V4L2_CID_EXPOSURE, exp * exp * exp * exp * exp * exp);
    return setCamParam(V4L2_CID_EXPOSURE, valueUs);
}

bool InnoMakerOV9281::setGain(int value)
{
    std::cout << __func__ << ": " << value << std::endl << std::flush;

    return setCamParam(V4L2_CID_GAIN, value);
}

bool InnoMakerOV9281::setLaserLevel(int value)
{
    std::cout << __func__ << ": " << value << std::endl << std::flush;

    return setCamParam(V4L2_CID_FLASH_TIMEOUT, value);
}

bool InnoMakerOV9281::setSomething(int value)
{
    std::cout << __func__ << ": " << value << std::endl << std::flush;

    return setCamParam(V4L2_CID_FLASH_INTENSITY, value);
}

bool InnoMakerOV9281::setCamParam(unsigned int v4l2controlId, int value)
{
    v4l2_control ctl{v4l2controlId, value};

    int ret = ioctl(m_cam_fd, VIDIOC_S_CTRL, &ctl);

    if (ret < 0)
    {
        fprintf(stderr,
                "cannot set cam param: id - %d, error - '%s'\n",
                v4l2controlId,
                strerror(errno));
        fflush(stderr);

        return false;
    }

    ret = ioctl(m_cam_fd, VIDIOC_G_CTRL, &ctl);

    if (ret < 0)
    {
        fprintf(stderr,
                "cannot get cam param: id - %d, error - '%s'\n",
                v4l2controlId,
                strerror(errno));

        fflush(stderr);

        return false;
    }

    std::cout << __func__ << ": new value is " << ctl.value << std::endl;

    return true;
}

bool InnoMakerOV9281::openCam()
{
    m_cam_fd = open(videoDevice, O_RDWR);

    if (m_cam_fd < 0)
    {
        fprintf(stderr,
                "cannot open cam '%s', error: '%s'\n",
                videoDevice,
                strerror(errno));
        return false;
    }

    return true;
}

bool InnoMakerOV9281::selectCam(int camIdx)
{
    int input = camIdx;
    int ret = ioctl(m_cam_fd, VIDIOC_S_INPUT, &input);

    if (ret < 0)
    {
        fprintf(stderr,
                "cannot select cam: idx - %d, error - '%s'\n",
                camIdx,
                strerror(errno));

        return false;
    }

    return true;
}

bool InnoMakerOV9281::initCam()
{
    v4l2_format format;
    memset(&format, 0, sizeof(v4l2_format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
    format.fmt.pix.width = img_width;
    format.fmt.pix.height = img_height;

    int ret = ioctl(m_cam_fd, VIDIOC_TRY_FMT, &format);

    if (ret < 0)
    {
        fprintf(stderr,
                "cannot try cam format: error - '%s'\n",
                strerror(errno));

        return false;
    }

    // TODO: remove this?
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    ret = ioctl(m_cam_fd, VIDIOC_S_FMT, &format);

    if (ret < 0)
    {
        fprintf(stderr,
                "cannot set cam format: error - '%s'\n",
                strerror(errno));

        return false;
    }

    struct v4l2_requestbuffers request;
    request.count = BUFFER_COUNT;
    request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    request.memory = V4L2_MEMORY_MMAP;
    ret = ioctl(m_cam_fd, VIDIOC_REQBUFS, &request);

    if (ret < 0) {
        fprintf(stderr, "cannot set cam request buffers: ioctl error - '%s'\n", strerror(errno));

        return false;
    }

    if (request.count < BUFFER_COUNT)
    {
        fprintf(stderr, "cannot set cam request buffers\n");

        return false;
    }

    struct v4l2_buffer buffer;
    memset(&buffer, 0, sizeof(buffer));
    buffer.type = request.type;
    buffer.memory = V4L2_MEMORY_MMAP;

    for (uint32_t i = 0; i < request.count; i++)
    {
        buffer.index = i;
        ret = ioctl(m_cam_fd, VIDIOC_QUERYBUF, &buffer);

        if (ret < 0)
        {
            DBG("ioctl(VIDIOC_QUERYBUF) failed %d(%s)", errno, strerror(errno));
            return false;
        }

        DBG("buffer.length: %d", buffer.length);
        DBG("buffer.m.offset: %d", buffer.m.offset);
        video_buffer_ptr[i] = (uint8_t *)
            mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, m_cam_fd, buffer.m.offset);
        if (video_buffer_ptr[i] == MAP_FAILED) {
            DBG("mmap() failed %d(%s)", errno, strerror(errno));
            return false;
        }

        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.memory = V4L2_MEMORY_MMAP;
        buffer.index = i;
        ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);
        if (ret != 0)
        {
            DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno));
            return false;
        }
    }

    // int buffer_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    // ret = ioctl(m_cam_fd, VIDIOC_STREAMON, &buffer_type);
    // if (ret != 0)
    // {
    //     DBG("ioctl(VIDIOC_STREAMON) failed %d(%s)", errno, strerror(errno));
    //     return false;
    // }

    DBG("cam init done.");

    return true;
}

// Image &InnoMakerOV9281::getImage()
// bool InnoMakerOV9281::getImage(Image &image)
bool InnoMakerOV9281::getImage(size_t &imageIndex)
// TODO: get Image from video_buffer_ptr
{
    static struct timeval curr, prev;
    static uint16_t counter = 0;
    gettimeofday(&curr, NULL);
    ++counter;

    double elapsedTime = (curr.tv_sec - prev.tv_sec) * 1000.0; // sec to ms
    elapsedTime += (curr.tv_usec - prev.tv_usec) / 1000.0;     // us to ms

    if (elapsedTime > 1000.)
    {
        fprintf(stderr,
                "fps: %d\tsec: %d\tdq: %d\tget: %d\trot: %d\tsum: %d,\tcorr: %d,\tval: %d\n",
                counter,
                curr.tv_sec % 1000,
                dq_elapsed_ns / 1000 / counter,
                get_elapsed_ns / 1000 / counter,
                rot_elapsed_ns / 1000 / counter,
                sum_elapsed_ns / 1000 / counter,
                corr_elapsed_ns / 1000 / counter,
                // max_elapsed_ns / 1000 / counter,
                value_elapsed_ns / 1000 / counter);

        dq_elapsed_ns = 0;
        get_elapsed_ns = 0;
        sum_elapsed_ns = 0;
        corr_elapsed_ns = 0;
        max_elapsed_ns = 0;
        value_elapsed_ns = 0;
        rot_elapsed_ns = 0;

        counter = 0;
        prev = curr;
    }

    int ret;
    struct v4l2_buffer buffer;

    memset(&buffer, 0, sizeof(buffer));
    buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buffer.memory = V4L2_MEMORY_MMAP;
    buffer.index = BUFFER_COUNT;

    {
        QElapsedTimer t;
        t.start();
        ret = ioctl(m_cam_fd, VIDIOC_DQBUF, &buffer);
        dq_elapsed_ns += t.nsecsElapsed();
    }

    if (ret != 0) {
        DBG("ioctl(VIDIOC_DQBUF) failed %d(%s)", errno, strerror(errno));
        return false;
    }

    if (buffer.index < 0 || buffer.index >= BUFFER_COUNT)
    {
        DBG("invalid buffer index: %d", buffer.index);
        return false;
    }

    // auto &image = video_buffer_ptr[buffer.index];
    imageIndex = buffer.index;
    auto &image = m_images[buffer.index];
    image.height = img_height;
    image.width = img_width;
    // TODO: fill
    // image.counters.encoderPosition = RotaryEncoder::instance()->position();
    image.counters.measurementCounter = buffer.sequence;
    image.counters.timestampUs = buffer.timestamp.tv_sec * 1000 * 1000 + buffer.timestamp.tv_usec;

    {
        QElapsedTimer t;
        t.start();
        memcpy(&image.data, video_buffer_ptr[buffer.index], img_size);
        // std::cout << (void *) video_buffer_ptr[buffer.index] << std::endl;
        get_elapsed_ns += t.nsecsElapsed();
    }

    static bool done{false};

    if (!done) {
        // memcpy(image.data.data(), video_buffer_ptr[buffer.index], img_size);
        done = true;
    }

    ret = ioctl(m_cam_fd, VIDIOC_QBUF, &buffer);

    if (ret != 0) {
        DBG("ioctl(VIDIOC_QBUF) failed %d(%s)", errno, strerror(errno));
        return false;
    }

    return true;
}