#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 "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 sum_elapsed_ns;
extern uint64_t corr_elapsed_ns;
extern uint64_t max_elapsed_ns;
extern uint64_t value_elapsed_ns;

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

InnoMakerOV9281::InnoMakerOV9281() {}

InnoMakerOV9281::~InnoMakerOV9281()
{
    m_streamThread.request_stop();
    m_streamThread.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(3000))
        return {};

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

    return {cam};
}

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

        while (!stopToken.stop_requested())
        {
            std::shared_ptr<Image> image = std::make_shared<Image>();
            getImage(*image);
// FIXME: backup emit value
#ifdef emit
#undef emit
            // std::cout << "emit new image" << std::endl << std::flush;
            // newImage.emit(image);
            if (newImageCallback)
            {
                newImageCallback(image);
            }
            // newPixels.emit(pixels);
            if (newPixelsCallback)
            {
                auto pixels = image->pixels();
                newPixelsCallback(pixels);
            }
        }
#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);

    return setCamParam(V4L2_CID_EXPOSURE, valueUs);
}

bool InnoMakerOV9281::setGain(int value)
{
    return setCamParam(V4L2_CID_GAIN, 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));

        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));

        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;
}

bool InnoMakerOV9281::getImage(Image &image)
{
    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, sec: %d\n", counter, curr.tv_sec);
        fprintf(stderr,
                "sum: %d,\tcorr: %d,\tval: %d\n",
                sum_elapsed_ns / 1000 / counter,
                corr_elapsed_ns / 1000 / counter,
                // max_elapsed_ns / 1000 / counter,
                value_elapsed_ns / 1000 / counter);
        sum_elapsed_ns = 0;
        corr_elapsed_ns = 0;
        max_elapsed_ns = 0;
        value_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;

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

    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;
    }

    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;
    memcpy(image.data, video_buffer_ptr[buffer.index], img_size);

    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;
}