#include <cstdint>
#include <iostream> // For std::cout
#include <sstream> // For std::stringstream
#include <stdexcept> // For std::runtime_error

#include "k230_sensor.h"

#include "mpi_sensor_api.h"
#include "mpi_vb_api.h"

k_vicap_dev Sensor::s_vicapDevIndex = VICAP_DEV_ID_0;

Sensor::Sensor(int csiNum, int reqWidth, int reqHeight, int reqFps)
    : m_vicapDev(s_vicapDevIndex)
    , m_sensorInfo({})
    , m_deviceAttr({})
{
    for (int i = VICAP_CHN_ID_0; i < VICAP_CHN_ID_MAX; i++) {
        m_pixelFormat[i]      = SensorPixelFormat::RGB888;
        m_channelAttr[i]      = k_vicap_chn_attr {};
        m_dumpedVideoFrame[i] = std::make_pair(false, k_video_frame_info {});
    }

    m_started = false;
    m_reseted = false;

    m_probeConfig.csi_num = csiNum;
    m_probeConfig.width   = reqWidth;
    m_probeConfig.height  = reqHeight;
    m_probeConfig.fps     = reqFps;

    s_vicapDevIndex = (k_vicap_dev)((int)s_vicapDevIndex + 1);

    if (m_vicapDev >= VICAP_DEV_ID_MAX) {
        s_vicapDevIndex = (k_vicap_dev)((int)s_vicapDevIndex - 1);
        throw std::runtime_error("Sensor: Invalid VICAP device index assigned. Max devices reached.");
    }
}

Sensor::~Sensor() { s_vicapDevIndex = (k_vicap_dev)((int)s_vicapDevIndex - 1); }

int Sensor::reset()
{
    if (m_started) {
        std::cout << "Sensor: already run" << std::endl;
        return -1;
    }

    if (0x00 != kd_mpi_sensor_adapt_get(&m_probeConfig, &m_sensorInfo)) {
        std::cout << "Sensor: probe failed on " << m_probeConfig.csi_num << " with " << m_probeConfig.width << "x"
                  << m_probeConfig.height << "@" << m_probeConfig.fps << std::endl;
        return -1;
    }

    std::cout << "Sensor: probe " << m_sensorInfo.sensor_name << " with " << m_sensorInfo.width << "x" << m_sensorInfo.height
              << "@" << m_sensorInfo.fps << std::endl;

    m_reseted = true;

    return 0;
}

bool Sensor::can_set_framesize_and_format(k_vicap_chn channel)
{
    if (VICAP_CHN_ID_MAX <= channel) {
        std::cout << "Invalid sensor channel" << channel << std::endl;
        return false;
    }

    if (m_started) {
        std::cout << "Sensor: already run" << std::endl;
        return false;
    }

    if (!m_reseted) {
        std::cout << "Sensor not call reset()" << std::endl;
        return false;
    }

    return true;
}

int Sensor::set_framesize(int width, int height, int crop_x, int crop_y, int crop_width, int crop_height, k_vicap_chn channel)
{
    if (!can_set_framesize_and_format(channel)) {
        return -1;
    }

    k_vicap_chn_attr attr {};

    int inputWidth = m_sensorInfo.width, inputHeight = m_sensorInfo.height;
    int targetWidth = width, targetHeight = height;

    if (width % 16) {
        targetWidth = VB_ALIGN_UP(width, 16);
        std::cout << "set_framesize: algin up tp 16, channel " << channel << std::endl;
    }

    if (targetWidth > inputWidth) {
        std::cout << "set_framesize: invalid width " << targetWidth << std::endl;
        return -1;
    }

    if (targetHeight > inputHeight) {
        std::cout << "set_framesize: invalid height " << targetHeight << std::endl;
        return -1;
    }

    if ((0 > crop_x) || (0 > crop_y) || (inputWidth < (crop_x + crop_width)) || (inputHeight < (crop_y + crop_height))
        || (crop_width < width) || (crop_height < height)) {
        std::cout << "set_framesize: invalid crop setting " << std::endl;
        return -1;
    }

    attr.out_win.h_start = 0;
    attr.out_win.v_start = 0;
    attr.out_win.width   = targetWidth;
    attr.out_win.height  = targetHeight;

    if ((crop_x + crop_width) && (crop_y + crop_height)) {
        attr.crop_enable      = K_TRUE;
        attr.crop_win.h_start = crop_x;
        attr.crop_win.v_start = crop_y;
        attr.crop_win.width   = crop_width;
        attr.crop_win.height  = crop_height;

        // seems no need
        attr.scale_enable      = K_TRUE;
        attr.scale_win.h_start = 0;
        attr.scale_win.v_start = 0;
        attr.scale_win.width   = targetWidth;
        attr.scale_win.height  = targetHeight;
    } else {
        attr.crop_enable  = K_FALSE;
        attr.scale_enable = K_FALSE;

        memset(&attr.crop_win, 0x00, sizeof(k_vicap_window));
        memset(&attr.scale_win, 0x00, sizeof(k_vicap_window));
    }

    attr.chn_enable = K_TRUE;

    // attr.pix_format
    // attr.buffer_num
    // attr.buffer_size
    // attr.buffer_pool_id
    // attr.alignment
    // attr.fps

    m_channelAttr[channel] = attr;

    return 0;
}

std::tuple<int, int, int, int> Sensor::calculate_crop(int inputWidth, int inputHeight, int targetWidth, int targetHeight)
{
    int w_scale = inputWidth / targetWidth;
    int h_scale = inputHeight / targetHeight;
    int scale   = std::min(w_scale, h_scale);

    if (scale <= 0) {
        return std::make_tuple(0, 0, targetWidth, targetHeight);
    }

    int crop_width  = targetWidth * scale;
    int crop_height = targetHeight * scale;

    int crop_x = (inputWidth - crop_width) / 2;
    int crop_y = (inputHeight - crop_height) / 2;

    return std::make_tuple(crop_x, crop_y, crop_width, crop_height);
}

int Sensor::set_framesize(int width, int height, bool crop, k_vicap_chn channel)
{
    if (!can_set_framesize_and_format(channel)) {
        return -1;
    }

    int inputWidth  = m_sensorInfo.width;
    int inputHeight = m_sensorInfo.height;

    auto [crop_x, crop_y, crop_width, crop_height] = calculate_crop(inputWidth, inputHeight, width, height);

    if (!crop) {
        return set_framesize(width, height, 0, 0, inputWidth, inputHeight, channel);
    }

    return set_framesize(width, height, crop_x, crop_y, crop_width, crop_height, channel);
}

std::tuple<int, int> Sensor::framesize(k_vicap_chn channel) const
{
    if (VICAP_CHN_ID_MAX <= channel) {
        std::cout << "Invalid sensor channel" << channel << std::endl;
        return std::make_tuple(0, 0);
    }

    k_vicap_chn_attr attr = m_channelAttr[channel];

    return std::make_tuple(attr.out_win.width, attr.out_win.height);
}

int Sensor::set_pixformat(SensorPixelFormat pixelFormat, k_vicap_chn channel)
{
    if (!can_set_framesize_and_format(channel)) {
        return -1;
    }

    m_pixelFormat[channel] = pixelFormat;

    return 0;
}

SensorPixelFormat Sensor::pixformat(k_vicap_chn channel) const
{
    if (VICAP_CHN_ID_MAX <= channel) {
        std::cout << "Invalid sensor channel" << channel << std::endl;
        return SensorPixelFormat::InvaildPixelFormat;
    }

    return m_pixelFormat[channel];
}

int Sensor::set_hmirror(bool enable)
{
    if (!can_set_framesize_and_format(VICAP_CHN_ID_0)) {
        return -1;
    }

    if (enable) {
        m_probeConfig.mirror |= 0x1;
    } else {
        m_probeConfig.mirror &= ~(1);
    }
    return 0;
}

bool Sensor::hmirror()
{
    if (!m_reseted) {
        std::cout << "Sensor not call reset()" << std::endl;
        return -1;
    }

    return m_probeConfig.mirror & 0x01 ? true : false;
}

int Sensor::set_vflip(bool enable)
{
    if (!can_set_framesize_and_format(VICAP_CHN_ID_0)) {
        return -1;
    }

    if (enable) {
        m_probeConfig.mirror |= 0x2;
    } else {
        m_probeConfig.mirror &= ~(2);
    }
    return 0;
}

bool Sensor::vflip()
{
    if (!m_reseted) {
        std::cout << "Sensor not call reset()" << std::endl;
        return -1;
    }

    return m_probeConfig.mirror & 0x02 ? true : false;
}

int Sensor::_set_alignment(int alignment, k_vicap_chn channel)
{
    if (!can_set_framesize_and_format(channel)) {
        return -1;
    }

    m_channelAttr[channel].alignment = alignment;

    return 0;
}

int Sensor::_set_fps(int fps, k_vicap_chn channel)
{
    if (!can_set_framesize_and_format(channel)) {
        return -1;
    }

    m_channelAttr[channel].fps = fps;

    return 0;
}

int Sensor::run(k_vicap_work_mode mode)
{
    int channel_enable_cnt = 0;

    if (m_started) {
        std::cout << "Sensor: already run" << std::endl;
        return -1;
    }

    if (!m_reseted) {
        std::cout << "Sensor not call reset()" << std::endl;
        return -1;
    }

    for (int i = 0; i < VICAP_CHN_ID_MAX; i++) {
        if (m_channelAttr[i].chn_enable) {
            channel_enable_cnt++;
        }
    }

    if (0x00 == channel_enable_cnt) {
        std::cout << "Sensor not configure any channel" << std::endl;
        return -1;
    }

    if ((VICAP_WORK_ONLINE_MODE != mode) && (VICAP_WORK_OFFLINE_MODE != mode) && (VICAP_WORK_ONLY_MCM_MODE != mode)) {
        std::cout << "Sensor not support workmode" << mode << std::endl;
        return -1;
    }

    m_deviceAttr.acq_win.h_start = 0;
    m_deviceAttr.acq_win.v_start = 0;
    m_deviceAttr.acq_win.width   = m_sensorInfo.width;
    m_deviceAttr.acq_win.height  = m_sensorInfo.height;

    m_deviceAttr.mode       = mode;
    m_deviceAttr.input_type = VICAP_INPUT_TYPE_SENSOR;

    // m_deviceAttr.image_pat = {};

    m_deviceAttr.pipe_ctrl.data             = UINT32_MAX;
    m_deviceAttr.pipe_ctrl.bits.af_enable   = 0;
    m_deviceAttr.pipe_ctrl.bits.ahdr_enable = 0;
    m_deviceAttr.pipe_ctrl.bits.dnr3_enable = 0;

    memcpy(&m_deviceAttr.sensor_info, &m_sensorInfo, sizeof(m_deviceAttr.sensor_info));

    m_deviceAttr.cpature_frame = 0;
    m_deviceAttr.dw_enable     = K_FALSE;
    m_deviceAttr.dev_enable    = K_TRUE;

    if (VICAP_WORK_ONLINE_MODE != mode) {
        m_deviceAttr.buffer_num     = 4;
        m_deviceAttr.buffer_size    = VB_ALIGN_UP(m_sensorInfo.width * m_sensorInfo.height * 2, 4096);
        m_deviceAttr.buffer_pool_id = VB_INVALID_POOLID;
    }
    m_deviceAttr.mirror          = static_cast<k_vicap_mirror>(m_probeConfig.mirror);
    m_deviceAttr.fastboot_enable = K_FALSE;

    if (K_SUCCESS != kd_mpi_vicap_set_dev_attr(m_vicapDev, m_deviceAttr)) {
        std::cout << "Sensor set dev attr failed" << std::endl;
        return -1;
    }

    for (int i = 0; i < VICAP_CHN_ID_MAX; i++) {
        if (m_channelAttr[i].chn_enable) {
            k_u32 chn_width  = m_channelAttr[i].out_win.width;
            k_u32 chn_height = m_channelAttr[i].out_win.height;

            m_channelAttr[i].pix_format = to_k_pixel_format(m_pixelFormat[i]);

            if (PIXEL_FORMAT_BUTT == m_channelAttr[i].pix_format) {
                std::cout << "Invalid pixelformat" << std::endl;
                return -1;
            }
            m_channelAttr[i].buffer_num = 4;

            if (PIXEL_FORMAT_YUV_SEMIPLANAR_420 == m_channelAttr[i].pix_format) {
                m_channelAttr[i].buffer_size = VB_ALIGN_UP(chn_width * chn_height * 3 / 2, 4096);
            } else if (PIXEL_FORMAT_RGB_888 == m_channelAttr[i].pix_format) {
                m_channelAttr[i].buffer_size = VB_ALIGN_UP(chn_width * chn_height * 3, 4096);
            } else if (PIXEL_FORMAT_RGB_888_PLANAR == m_channelAttr[i].pix_format) {
                m_channelAttr[i].buffer_size = VB_ALIGN_UP(chn_width * chn_height * 3, 4096 * 3);
            } else {
                std::cout << "Invalid pixelformat" << std::endl;
                return -1;
            }
            m_channelAttr[i].buffer_pool_id = VB_INVALID_POOLID;
            // m_channelAttr[i].alignment      = 0;
            // m_channelAttr[i].fps = 0;

            if (K_SUCCESS != kd_mpi_vicap_set_chn_attr(m_vicapDev, static_cast<k_vicap_chn>(i), m_channelAttr[i])) {
                std::cout << "Sensor set channel attr failed" << std::endl;

                return -1;
            }
        }
    }

    if (K_SUCCESS != kd_mpi_vicap_init(m_vicapDev)) {
        std::cout << "Sensor init failed" << std::endl;
        return -1;
    }

    if (K_SUCCESS != kd_mpi_vicap_start_stream(m_vicapDev)) {
        std::cout << "Sensor start stream failed" << std::endl;
        return -1;
    }

    m_started = true;

    return 0;
}

int Sensor::stop()
{
    if (!m_started) {
        std::cout << "Sensor: not run" << std::endl;
        return -1;
    }

    if (K_SUCCESS != kd_mpi_vicap_stop_stream(m_vicapDev)) {
        std::cout << "Sensor stop stream failed" << std::endl;
    }

    if (K_SUCCESS != kd_mpi_vicap_deinit(m_vicapDev)) {
        std::cout << "Sensor deinit failed" << std::endl;
    }

    return 0;
}

int Sensor::snapshot(k_video_frame_info& info, k_vicap_chn channel, int timeoutMs)
{
    k_video_frame_info vf_info;

    memset(&info, 0x00, sizeof(k_video_frame_info));

    if (VICAP_CHN_ID_MAX <= channel) {
        std::cout << "Invalid sensor channel" << channel << std::endl;
        return -1;
    }

    if (!m_started) {
        std::cout << "Sensor: not run" << std::endl;
        return -1;
    }

    if (m_dumpedVideoFrame[channel].first) {
        vf_info = m_dumpedVideoFrame[channel].second;

        if (K_SUCCESS != kd_mpi_vicap_dump_release(m_vicapDev, channel, &vf_info)) {
            std::cout << "Sensor: release dump failed" << std::endl;
            return -1;
        }
        m_dumpedVideoFrame[channel].first = false;
    }

    if (K_SUCCESS != kd_mpi_vicap_dump_frame(m_vicapDev, channel, VICAP_DUMP_YUV, &vf_info, static_cast<k_u32>(timeoutMs))) {
        std::cout << "Sensor: dump failed" << std::endl;
        return -1;
    }

    m_dumpedVideoFrame[channel] = std::make_pair(true, vf_info);

    memcpy(&info, &vf_info, sizeof(k_video_frame_info));

    return 0;
}
