#include "hik_camera/hik_camera.hpp"

namespace camera
{

CameraNode::CameraNode(SyncClient* client,
                        const std::string& node_name,
                        const rclcpp::NodeOptions& options)
    : Node(node_name, options), client_(client)
{
    RCLCPP_INFO(this -> get_logger(), "Creating");
    
    initializeParameters();
    initializeCamera();
    setupImagePublisher();
    startWorkerThread();
}

CameraNode::CameraNode(const rclcpp::NodeOptions& options)
    : Node("camera_node", options){
}

CameraNode::~CameraNode() {
    stopCamera();
}

void CameraNode::initializeParameters() {
    // 声明参数
    std::vector<std::pair<std::string, rclcpp::ParameterValue>> param_declarations = {
        {"width", rclcpp::ParameterValue(1280)},
        {"height", rclcpp::ParameterValue(1024)},
        {"FrameRateEnable", rclcpp::ParameterValue(false)},
        {"FrameRate", rclcpp::ParameterValue(10)},
        {"BurstFrameCount", rclcpp::ParameterValue(10)},
        {"ExposureTime", rclcpp::ParameterValue(25000)},
        {"GammaEnable", rclcpp::ParameterValue(true)},
        {"Gamma", rclcpp::ParameterValue(0.55)},
        {"GainAuto", rclcpp::ParameterValue(2)},
        {"SaturationEnable", rclcpp::ParameterValue(true)},
        {"Saturation", rclcpp::ParameterValue(128)},
        {"Offset_x", rclcpp::ParameterValue(0)},
        {"Offset_y", rclcpp::ParameterValue(0)},
        {"TriggerMode", rclcpp::ParameterValue(1)},
        {"TriggerSource", rclcpp::ParameterValue(2)},
        {"TriggerActivation", rclcpp::ParameterValue(2)},
        {"LineSelector", rclcpp::ParameterValue(2)}
    };
    
    for (const auto& param : param_declarations) {
        this->declare_parameter(param.first, param.second);
    }
    
    // 获取参数值
    this->get_parameter("width", width);
    this->get_parameter("height", height);
    this->get_parameter("FrameRateEnable", FrameRateEnable);
    this->get_parameter("FrameRate", FrameRate);
    this->get_parameter("BurstFrameCount", BurstFrameCount);
    this->get_parameter("ExposureTime", ExposureTime);
    this->get_parameter("GammaEnable", GammaEnable);
    this->get_parameter("Gamma", Gamma);
    this->get_parameter("GainAuto", GainAuto);
    this->get_parameter("SaturationEnable", SaturationEnable);
    this->get_parameter("Saturation", Saturation);
    this->get_parameter("Offset_x", Offset_x);
    this->get_parameter("Offset_y", Offset_y);
    this->get_parameter("TriggerMode", TriggerMode);
    this->get_parameter("TriggerSource", TriggerSource);
    this->get_parameter("TriggerActivation", TriggerActivation);
    this->get_parameter("LineSelector", LineSelector);
}

void CameraNode::initializeCamera() {
    handle = NULL;
    
    // 枚举设备
    if (!enumerateDevices()) {
        RCLCPP_FATAL(this->get_logger(), "Failed to enumerate devices");
        exit(-1);
    }
    
    // 创建设备句柄
    if (!createHandle()) {
        RCLCPP_FATAL(this->get_logger(), "Failed to create device handle");
        exit(-1);
    }
    
    // 打开设备
    if (!openDevice()) {
        RCLCPP_FATAL(this->get_logger(), "Failed to open device");
        exit(-1);
    }
    
    // 设置相机参数
    configureCamera();
    
    // 开始取流
    if (!startGrabbing()) {
        RCLCPP_FATAL(this->get_logger(), "Failed to start grabbing");
        exit(-1);
    }
}

bool CameraNode::enumerateDevices() {
    MV_CC_DEVICE_INFO_LIST stDeviceList;
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
    
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet) {
        RCLCPP_ERROR(this->get_logger(), "MV_CC_EnumDevices fail! nRet [%x]", nRet);
        return false;
    }
    
    if (stDeviceList.nDeviceNum == 0) {
        RCLCPP_ERROR(this->get_logger(), "No devices found!");
        return false;
    }
    
    // 打印设备信息
    for (unsigned int i = 0; i < stDeviceList.nDeviceNum; i++) {
        MV_CC_DEVICE_INFO *pDeviceInfo = stDeviceList.pDeviceInfo[i];
        if (pDeviceInfo) {
            RCLCPP_INFO(this->get_logger(), "[device %d]:", i);
            PrintDeviceInfo(pDeviceInfo);
        }
    }
    
    return true;
}

bool CameraNode::createHandle() {
    MV_CC_DEVICE_INFO_LIST stDeviceList;
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
    
    // 重新枚举设备获取设备列表
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet || stDeviceList.nDeviceNum == 0) {
        return false;
    }
    
    nRet = MV_CC_CreateHandle(&handle, stDeviceList.pDeviceInfo[0]);
    if (MV_OK != nRet) {
        RCLCPP_ERROR(this->get_logger(), "MV_CC_CreateHandle fail! nRet [%x]", nRet);
        return false;
    }
    
    return true;
}

bool CameraNode::openDevice() {
    nRet = MV_CC_OpenDevice(handle);
    if (MV_OK != nRet) {
        RCLCPP_ERROR(this->get_logger(), "MV_CC_OpenDevice fail! nRet [%x]", nRet);
        return false;
    }
    return true;
}

void CameraNode::configureCamera() {
    // 设置采集模式
    setEnumValue("AcquisitionMode", 2); // 连续采集模式
    
    // 设置基础参数
    RCLCPP_INFO(this->get_logger(), "Setting camera configuration from parameters");
    
    this->set(CAP_PROP_FRAMERATE_ENABLE, FrameRateEnable);
    if (FrameRateEnable) {
        this->set(CAP_PROP_FRAMERATE, FrameRate);
    }
    
    this->set(CAP_PROP_HEIGHT, height);
    this->set(CAP_PROP_WIDTH, width);
    this->set(CAP_PROP_OFFSETX, Offset_x);
    this->set(CAP_PROP_OFFSETY, Offset_y);
    this->set(CAP_PROP_EXPOSURE_TIME, ExposureTime);
    
    // 设置伽马
    this->set(CAP_PROP_GAMMA_ENABLE, GammaEnable);
    if (GammaEnable) {
        this->set(CAP_PROP_GAMMA, Gamma);
    }
    
    // 设置增益
    this->set(CAP_PROP_GAINAUTO, GainAuto);
    
    // 设置白平衡
    setEnumValue("BalanceWhiteAuto", 0);
    
    // 设置饱和度
    this->set(CAP_PROP_SATURATION_ENABLE, SaturationEnable);
    if (SaturationEnable) {
        this->set(CAP_PROP_SATURATION, Saturation);
    }
    
    // 设置触发模式
    configureTrigger();
    
    // 设置像素格式
    configurePixelFormat();
}

bool CameraNode::setEnumValue(const std::string& feature, unsigned int value) {
    nRet = MV_CC_SetEnumValue(handle, feature.c_str(), value);
    if (MV_OK == nRet) {
        RCLCPP_DEBUG(this->get_logger(), "Set %s OK! value=%u", feature.c_str(), value);
        return true;
    } else {
        RCLCPP_WARN(this->get_logger(), "Set %s Failed! nRet = [%x]", feature.c_str(), nRet);
        return false;
    }
}

void CameraNode::configureTrigger() {
    if (setEnumValue("TriggerMode", MV_TRIGGER_MODE_ON)) {
        setEnumValue("TriggerSource", 2);
        setEnumValue("TriggerActivation", 0);
    }
}

void CameraNode::configurePixelFormat() {
    // 设置像素格式为BayerRG8
    if (setEnumValue("PixelFormat", 0x01080009)) {
        RCLCPP_INFO(this->get_logger(), "PixelFormat set to BayerRG8");
    }
    
    // 验证像素格式
    MVCC_ENUMVALUE pixelFormat = {0};
    nRet = MV_CC_GetEnumValue(handle, "PixelFormat", &pixelFormat);
    if (MV_OK == nRet) {
        RCLCPP_DEBUG(this->get_logger(), "Current PixelFormat: %d", pixelFormat.nCurValue);
    }
}

bool CameraNode::startGrabbing() {
    nRet = MV_CC_StartGrabbing(handle);
    if (MV_OK != nRet) {
        RCLCPP_ERROR(this->get_logger(), "MV_CC_StartGrabbing fail! nRet [%x]", nRet);
        return false;
    }
    return true;
}

void CameraNode::setupImagePublisher() {
    this->pub_ = image_transport::create_camera_publisher(this, "~/image_raw");
    
    // 初始化相机信息管理器
    auto camera_calibration_file_param_ = this->declare_parameter(
        "camera_calibration_file", "file://config/camera_calibration.yaml");
    
    cinfo_manager_ = std::make_shared<camera_info_manager::CameraInfoManager>(this);
    if (!cinfo_manager_->loadCameraInfo(camera_calibration_file_param_)) {
        RCLCPP_WARN(this->get_logger(), "Failed to load camera calibration info");
    }
}

void CameraNode::startWorkerThread() {
    std::unique_lock<std::shared_mutex> thread_lock(worker_thread_mutex);
    worker_thread = std::thread([this](){
        this->HKWorkThread(this->handle);
    });
}

void CameraNode::stopCamera() {
    // 等待工作线程结束
    if (worker_thread.joinable()) {
        worker_thread.join();
    }
    
    // 停止取流
    if (handle) {
        nRet = MV_CC_StopGrabbing(handle);
        if (MV_OK != nRet) {
            RCLCPP_ERROR(this->get_logger(), "MV_CC_StopGrabbing fail! nRet [%x]", nRet);
        } else {
            RCLCPP_INFO(this->get_logger(), "MV_CC_StopGrabbing succeed");
        }
        
        // 关闭设备
        nRet = MV_CC_CloseDevice(handle);
        if (MV_OK != nRet) {
            RCLCPP_ERROR(this->get_logger(), "MV_CC_CloseDevice fail! nRet [%x]", nRet);
        } else {
            RCLCPP_INFO(this->get_logger(), "MV_CC_CloseDevice succeed");
        }
        
        // 销毁句柄
        nRet = MV_CC_DestroyHandle(handle);
        if (MV_OK != nRet) {
            RCLCPP_ERROR(this->get_logger(), "MV_CC_DestroyHandle fail! nRet [%x]", nRet);
        } else {
            RCLCPP_INFO(this->get_logger(), "MV_CC_DestroyHandle succeed");
        }
        
        handle = NULL;
    }
}

bool CameraNode::set(CamerProperties type, float value) {
    bool success = false;
    
    switch (type) {
        case CAP_PROP_FRAMERATE_ENABLE:
            success = setBoolValue("AcquisitionFrameRateEnable", value);
            break;
        case CAP_PROP_FRAMERATE:
            success = setFloatValue("AcquisitionFrameRate", value);
            break;
        case CAP_PROP_BURSTFRAMECOUNT:
            success = setIntValue("AcquisitionBurstFrameCount", value);
            break;
        case CAP_PROP_HEIGHT:
            success = setIntValue("Height", value);
            break;
        case CAP_PROP_WIDTH:
            success = setIntValue("Width", value);
            break;
        case CAP_PROP_OFFSETX:
            success = setIntValue("OffsetX", value);
            break;
        case CAP_PROP_OFFSETY:
            success = setIntValue("OffsetY", value);
            break;
        case CAP_PROP_EXPOSURE_TIME:
            success = setFloatValue("ExposureTime", value);
            break;
        case CAP_PROP_GAMMA_ENABLE:
            success = setBoolValue("GammaEnable", value);
            break;
        case CAP_PROP_GAMMA:
            success = setFloatValue("Gamma", value);
            break;
        case CAP_PROP_GAINAUTO:
            success = setEnumValue("GainAuto", static_cast<unsigned int>(value));
            break;
        case CAP_PROP_SATURATION_ENABLE:
            success = setBoolValue("SaturationEnable", value);
            break;
        case CAP_PROP_SATURATION:
            success = setIntValue("Saturation", value);
            break;
        case CAP_PROP_TRIGGER_MODE:
            success = setEnumValue("TriggerMode", static_cast<unsigned int>(value));
            break;
        case CAP_PROP_TRIGGER_SOURCE:
            success = setEnumValue("TriggerSource", static_cast<unsigned int>(value));
            break;
        case CAP_PROP_TRIGGER_ACTIVATION:
            success = setEnumValue("TriggerActivation", static_cast<unsigned int>(value));
            break;
        case CAP_PROP_LINE_SELECTOR:
            success = setEnumValue("LineSelector", static_cast<unsigned int>(value));
            break;
        default:
            RCLCPP_WARN(this->get_logger(), "Unknown camera property: %d", type);
            return false;
    }
    
    return success;
}

bool CameraNode::setBoolValue(const std::string& feature, bool value) {
    nRet = MV_CC_SetBoolValue(handle, feature.c_str(), value);
    if (MV_OK == nRet) {
        RCLCPP_DEBUG(this->get_logger(), "Set %s OK! value=%s", 
                     feature.c_str(), value ? "true" : "false");
        return true;
    } else {
        RCLCPP_WARN(this->get_logger(), "Set %s Failed! nRet = [%x]", 
                    feature.c_str(), nRet);
        return false;
    }
}

bool CameraNode::setIntValue(const std::string& feature, int value) {
    nRet = MV_CC_SetIntValue(handle, feature.c_str(), value);
    if (MV_OK == nRet) {
        RCLCPP_DEBUG(this->get_logger(), "Set %s OK! value=%d", feature.c_str(), value);
        return true;
    } else {
        RCLCPP_WARN(this->get_logger(), "Set %s Failed! nRet = [%x]", feature.c_str(), nRet);
        return false;
    }
}

bool CameraNode::setFloatValue(const std::string& feature, float value) {
    nRet = MV_CC_SetFloatValue(handle, feature.c_str(), value);
    if (MV_OK == nRet) {
        RCLCPP_DEBUG(this->get_logger(), "Set %s OK! value=%f", feature.c_str(), value);
        return true;
    } else {
        RCLCPP_WARN(this->get_logger(), "Set %s Failed! nRet = [%x]", feature.c_str(), nRet);
        return false;
    }
}

bool CameraNode::reset() {
    bool success = true;
    
    success = this->set(CAP_PROP_FRAMERATE_ENABLE, FrameRateEnable) && success;
    if (FrameRateEnable) {
        success = this->set(CAP_PROP_FRAMERATE, FrameRate) && success;
    }
    
    success = this->set(CAP_PROP_HEIGHT, height) && success;
    success = this->set(CAP_PROP_WIDTH, width) && success;
    success = this->set(CAP_PROP_OFFSETX, Offset_x) && success;
    success = this->set(CAP_PROP_OFFSETY, Offset_y) && success;
    success = this->set(CAP_PROP_EXPOSURE_TIME, ExposureTime) && success;
    success = this->set(CAP_PROP_GAMMA_ENABLE, GammaEnable) && success;
    
    if (GammaEnable) {
        success = this->set(CAP_PROP_GAMMA, Gamma) && success;
    }
    
    success = this->set(CAP_PROP_GAINAUTO, GainAuto) && success;
    success = this->set(CAP_PROP_SATURATION_ENABLE, SaturationEnable) && success;
    
    if (SaturationEnable) {
        success = this->set(CAP_PROP_SATURATION, Saturation) && success;
    }
    
    success = this->set(CAP_PROP_TRIGGER_MODE, TriggerMode) && success;
    success = this->set(CAP_PROP_TRIGGER_SOURCE, TriggerSource) && success;
    success = this->set(CAP_PROP_TRIGGER_ACTIVATION, TriggerActivation) && success;
    success = this->set(CAP_PROP_LINE_SELECTOR, LineSelector) && success;
    
    return success;
}

bool CameraNode::PrintDeviceInfo(MV_CC_DEVICE_INFO *pstMVDevInfo) {
    if (NULL == pstMVDevInfo){
        RCLCPP_ERROR(this->get_logger(), "The Pointer of pstMVDevInfoList is NULL!");
        return false;
    }
    
    if (pstMVDevInfo->nTLayerType == MV_GIGE_DEVICE){
        RCLCPP_INFO(this->get_logger(), "Device Type: GigE");
        RCLCPP_INFO(this->get_logger(), "Current IP: %x", pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp);
        RCLCPP_INFO(this->get_logger(), "User Defined Name: %s", 
                   pstMVDevInfo->SpecialInfo.stGigEInfo.chUserDefinedName);
    }
    else if (pstMVDevInfo->nTLayerType == MV_USB_DEVICE) {
        RCLCPP_INFO(this->get_logger(), "Device Type: USB3.0");
        RCLCPP_INFO(this->get_logger(), "User Defined Name: %s", 
                   pstMVDevInfo->SpecialInfo.stUsb3VInfo.chUserDefinedName);
    }
    else {
        RCLCPP_WARN(this->get_logger(), "Unsupported device type");
        return false;
    }
    
    return true;
}

void* CameraNode::HKWorkThread(void* p_handle) {
    RCLCPP_INFO(get_logger(), "Starting HKWorkThread");
    
    int nRet;
    cv::Mat frame;    
    size_t frame_id = 0;
    auto camera_info_msg = std::make_shared<sensor_msgs::msg::CameraInfo>(
            cinfo_manager_ -> getCameraInfo());
    
    // 分配图像缓冲区
    unsigned char *m_pBufForDriver = (unsigned char *)malloc(sizeof(unsigned char) * MAX_IMAGE_DATA_SIZE);
    unsigned char *m_pBufForSaveImage = (unsigned char *)malloc(MAX_IMAGE_DATA_SIZE);
    
    if (!m_pBufForDriver || !m_pBufForSaveImage) {
        RCLCPP_FATAL(get_logger(), "Failed to allocate memory for image buffers");
        free(m_pBufForDriver);
        free(m_pBufForSaveImage);
        return nullptr;
    }
    
    MV_FRAME_OUT_INFO_EX stImageInfo = {0};
    MV_CC_PIXEL_CONVERT_PARAM stConvertParam = {0};
    
    RCLCPP_INFO(get_logger(), "Start publishing image messages");
    int image_empty_count = 0;
    const int MAX_EMPTY_COUNT = 100;
    
    while (rclcpp::ok()) {
        auto msg = std::make_unique<sensor_msgs::msg::Image>();

        // 获取一帧图像
        nRet = MV_CC_GetOneFrameTimeout(p_handle, m_pBufForDriver, MAX_IMAGE_DATA_SIZE, &stImageInfo, 15);
        if (nRet != MV_OK) {
            if (++image_empty_count > MAX_EMPTY_COUNT) {
                RCLCPP_ERROR(get_logger(), "Failed to read frames %d times consecutively", MAX_EMPTY_COUNT);
                break;
            }
            continue;
        }
        
        image_empty_count = 0;
        
        // 转换图像格式
        stConvertParam.nWidth = stImageInfo.nWidth;
        stConvertParam.nHeight = stImageInfo.nHeight;
        stConvertParam.pSrcData = m_pBufForDriver;
        stConvertParam.nSrcDataLen = stImageInfo.nFrameLen;
        stConvertParam.enDstPixelType = PixelType_Gvsp_BGR8_Packed;
        stConvertParam.pDstBuffer = m_pBufForSaveImage;
        stConvertParam.nDstBufferSize = MAX_IMAGE_DATA_SIZE;
        stConvertParam.enSrcPixelType = stImageInfo.enPixelType;
        
        nRet = MV_CC_ConvertPixelType(p_handle, &stConvertParam);
        if (nRet != MV_OK) {
            RCLCPP_WARN(get_logger(), "Failed to convert pixel format, nRet [%x]", nRet);
            continue;
        }
        
        // 创建OpenCV图像并发布
        frame = cv::Mat(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC3, m_pBufForSaveImage);
        convert_frame_to_message(frame, frame_id, *msg, *camera_info_msg);
        pub_.publish(std::move(msg), camera_info_msg);
        ++frame_id;
    }
    
    // 清理资源
    free(m_pBufForDriver);
    free(m_pBufForSaveImage);
    
    RCLCPP_INFO(get_logger(), "HKWorkThread finished");
    return nullptr;
}

std::string CameraNode::mat_type2encoding(int mat_type) {
    switch (mat_type) {
        case CV_8UC1:
            return "mono8";
        case CV_8UC3:
            return "bgr8";
        case CV_16SC1:
            return "mono16";
        case CV_8UC4:
            return "rgba8";
        default:
            throw std::runtime_error("Unsupported encoding type");
    }
}

void CameraNode::convert_frame_to_message(const cv::Mat & frame, size_t frame_id,
    sensor_msgs::msg::Image & msg, 
    sensor_msgs::msg::CameraInfo & camera_info_msg) {
    
    // 复制图像数据到ROS消息
    msg.height = frame.rows;
    msg.width = frame.cols;
    msg.encoding = mat_type2encoding(frame.type());
    msg.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame.step);
    
    size_t size = frame.step * frame.rows;
    msg.data.resize(size);
    memcpy(&msg.data[0], frame.data, size);
    
    // 获取时间戳
    rclcpp::Time timestamp;
    uint64_t timestamp_us;
    uint64_t now_timestamp_us = 
        std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::system_clock::now().time_since_epoch()).count();
    
    if (client_ && client_->requestCameraTimestamp(now_timestamp_us, timestamp_us)) {
        timestamp = rclcpp::Time(timestamp_us);
        RCLCPP_INFO(this->get_logger(), "Camera timestamp: %ld us", timestamp_us);
    } else {
        timestamp = this->now();
        RCLCPP_WARN(this->get_logger(), "Failed to get camera timestamp, using system time");
    }
    
    // 设置消息头
    msg.header.frame_id = std::to_string(frame_id);
    msg.header.stamp = timestamp;
    camera_info_msg.header.frame_id = std::to_string(frame_id);
    camera_info_msg.header.stamp = timestamp;
}

} // namespace camera

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode)