#include "mvs_camera.h"
#include "config.h"

#ifdef _MSC_VER
#pragma warning(disable: 4828)
#endif

#include <MvCameraControl.h>

#include <spdlog/spdlog.h>

#include <atomic>

bool check_mvs_api_call(int api_ret, unsigned int line_number,
                        const char *file_name, const char *api_call_str) {
    if (api_ret == MV_OK) [[likely]] return true;
    SPDLOG_ERROR("MVS api call {} failed at {}:{} with error 0x{:x}",
                 api_call_str, file_name, line_number, api_ret);
    RET_ERROR;
}

#define MVS_API_CHECK(api_call) \
    if (!check_mvs_api_call( \
        api_call, __LINE__, __FILE__, #api_call)) [[unlikely]] \
        return false

struct mvs_camera::impl {
    void *handle = nullptr;
    std::string_view cam_name;
    bool is_capturing = false;

    cv::cuda::GpuMat *inner_img = nullptr;
    std::atomic<cv::cuda::GpuMat *> next_img;

    static void on_error(unsigned int msg_type, void *user_data) {
        auto pimpl = (impl *) user_data;
        SPDLOG_ERROR("MVS camera {} exception 0x{:x}.", pimpl->cam_name, msg_type);
        if (msg_type == 0x8003) return; // stop capture
        assert(false);
    }

    static void on_image(unsigned char *data, MV_FRAME_OUT_INFO_EX *frame_info, void *user_data) {
        assert(frame_info->nFrameLen == raw_image_size);
        auto pimpl = (impl *) user_data;
        auto host_img = cv::Mat{image_height, image_width, CV_8UC1, data};

        // upload image to gpu
        if (pimpl->inner_img == nullptr) [[unlikely]] {
            pimpl->inner_img = new cv::cuda::GpuMat{};
        }
        pimpl->inner_img->upload(host_img);

        // commit new image
        pimpl->inner_img = pimpl->next_img.exchange(pimpl->inner_img);
        pimpl->next_img.notify_all();
    }
};

mvs_camera::mvs_camera()
        : pimpl(std::make_unique<impl>()) {}

mvs_camera::~mvs_camera() {
    close();
}

bool mvs_camera::open(std::string_view camera_name) {
    MV_CC_DEVICE_INFO_LIST dev_info_list;
    MVS_API_CHECK(MV_CC_EnumDevices(MV_USB_DEVICE, &dev_info_list));

    MV_CC_DEVICE_INFO *dev_info = nullptr;
    for (int i = 0; i < dev_info_list.nDeviceNum; ++i) {
        auto tmp_dev_info = dev_info_list.pDeviceInfo[i];
        auto tmp_dev_name = (char *) tmp_dev_info->SpecialInfo.stUsb3VInfo.chUserDefinedName;
        if (camera_name == tmp_dev_name) {
            dev_info = tmp_dev_info;
        }
    }
    if (dev_info == nullptr) {
        SPDLOG_ERROR("No camera named {}.", camera_name);
        RET_ERROR;
    }
    pimpl->cam_name = camera_name;

//    MVS_API_CHECK(MV_CC_IsDeviceAccessible(dev_info, MV_ACCESS_Control));
    MVS_API_CHECK(MV_CC_CreateHandle(&pimpl->handle, dev_info));
    MVS_API_CHECK(MV_CC_OpenDevice(pimpl->handle, MV_ACCESS_Control));

    // close and open again to fix some bug
    MVS_API_CHECK(MV_CC_CloseDevice(pimpl->handle));
    MVS_API_CHECK(MV_CC_OpenDevice(pimpl->handle, MV_ACCESS_Control));

    // register callbacks
    MVS_API_CHECK(MV_CC_RegisterExceptionCallBack(pimpl->handle, impl::on_error, pimpl.get()));
    MVS_API_CHECK(MV_CC_RegisterImageCallBackEx(pimpl->handle, impl::on_image, pimpl.get()));

    SPDLOG_INFO("Camera {} opened successfully.", pimpl->cam_name);
    return true;
}

void mvs_camera::close() {
    if (pimpl->handle == nullptr) return;
    stop_capture();
    MV_CC_CloseDevice(pimpl->handle);
    MV_CC_DestroyHandle(pimpl->handle);
    pimpl->handle = nullptr;
    SPDLOG_INFO("Camera {} closed.", pimpl->cam_name);
}

bool mvs_camera::start_capture(const capture_config *config) {
    assert(pimpl->handle != nullptr);

    // config camera
    MVS_API_CHECK(MV_CC_SetEnumValue(pimpl->handle, "PixelFormat", PixelType_Gvsp_BayerRG8));
    MVS_API_CHECK(MV_CC_SetEnumValue(pimpl->handle, "AcquisitionMode",
                                     MV_CAM_ACQUISITION_MODE::MV_ACQ_MODE_CONTINUOUS));
    MVS_API_CHECK(MV_CC_SetEnumValue(pimpl->handle, "TriggerMode", MV_TRIGGER_MODE_ON));
    MVS_API_CHECK(MV_CC_SetEnumValue(pimpl->handle, "TriggerSource",
                                     MV_CAM_TRIGGER_SOURCE::MV_TRIGGER_SOURCE_SOFTWARE));
    MVS_API_CHECK(MV_CC_SetFloatValue(pimpl->handle, "ExposureTime", config->exposure_time));
    MVS_API_CHECK(MV_CC_SetFloatValue(pimpl->handle, "Gain", config->analog_gain));

    MVS_API_CHECK(MV_CC_StartGrabbing(pimpl->handle));
    pimpl->is_capturing = true;
    SPDLOG_INFO("Camera {} is capturing.", pimpl->cam_name);
    return true;
}

void mvs_camera::stop_capture() {
    if (pimpl->handle == nullptr || !pimpl->is_capturing) return;
    MV_CC_StopGrabbing(pimpl->handle);
    pimpl->is_capturing = false;
    SPDLOG_INFO("Camera {} stopped capturing.", pimpl->cam_name);
}

bool mvs_camera::software_trigger() {
    assert(pimpl->handle != nullptr);
    MVS_API_CHECK(MV_CC_TriggerSoftwareExecute(pimpl->handle));
    return true;
}

void mvs_camera::retrieve_image(cv::cuda::GpuMat **image_ptr) {
    pimpl->next_img.wait(nullptr);
    *image_ptr = pimpl->next_img.exchange(*image_ptr);
}

bool mvs_camera::is_opened() const {
    return pimpl->handle != nullptr;
}

bool mvs_camera::is_capturing() const {
    return pimpl->is_capturing;
}