#include "MindVisionCamera.h"
#include <iostream>

MindVisionCamera::MindVisionCamera(int cam_id, const std::string& target_color) 
    : camera_handle(-1), is_opened(false), width(640), height(480), fps(30) {
    
    if (!initialize_camera(cam_id)) {
        throw std::runtime_error("Cannot initialize MindVision camera!");
    }

    is_opened = true;
    this->target_color = target_color;
    if (this->target_color != "red" && this->target_color != "blue") {
        throw std::invalid_argument("Only 'red' or 'blue' colors are supported");
    }

    set_cam_params();
}

bool MindVisionCamera::initialize_camera(int cam_id) {
    // 初始化SDK
    CameraSdkInit(1);
    
    int iCameraCounts = 10;
    tSdkCameraDevInfo tCameraEnumList[10];  // 声明为数组而不是指针
    
    // 枚举设备，并建立设备列表
    int iStatus = CameraEnumerateDevice(tCameraEnumList, &iCameraCounts);
    std::cout << "CameraEnumerateDevice returned: " << iStatus << std::endl;
    std::cout << "Found " << iCameraCounts << " cameras" << std::endl;

    if (iCameraCounts <= 0) {
        std::cerr << "No MindVision cameras found!" << std::endl;
        return false;
    }
    
    // 选择指定ID的相机
    if (cam_id >= iCameraCounts) {
        std::cerr << "Camera ID " << cam_id << " not found! Only " << iCameraCounts << " cameras detected." << std::endl;
        return false;
    }
    
    std::cout << "Initializing camera " << cam_id << ": " << tCameraEnumList[cam_id].acFriendlyName << std::endl;

    // 相机初始化
    iStatus = CameraInit(&tCameraEnumList[cam_id], -1, -1, &camera_handle);
    std::cout << "CameraInit returned: " << iStatus << std::endl;
    
    if (iStatus != CAMERA_STATUS_SUCCESS) {
        std::cerr << "Failed to initialize camera! Error code: " << iStatus << std::endl;
        return false;
    }
    
    // 获取相机能力
    tSdkCameraCapbility capability;
    iStatus = CameraGetCapability(camera_handle, &capability);
    if (iStatus != CAMERA_STATUS_SUCCESS) {
        std::cerr << "Failed to get camera capability! Error code: " << iStatus << std::endl;
        return false;
    }
    
    // 设置输出格式
    if (capability.sIspCapacity.bMonoSensor) {
        CameraSetIspOutFormat(camera_handle, CAMERA_MEDIA_TYPE_MONO8);
    } else {
        CameraSetIspOutFormat(camera_handle, CAMERA_MEDIA_TYPE_BGR8);
    }
    
    // 让SDK进入工作模式
    CameraPlay(camera_handle);
    
    // 获取并设置分辨率为 640x480
    tSdkImageResolution image_resolution;
    int status = CameraGetImageResolution(camera_handle, &image_resolution);
    std::cout << "Default resolution query returned: " << status << std::endl;

    // 设置分辨率为 640x480
    image_resolution.iIndex = 0xFF; // Use custom resolution (0xFF typically indicates custom resolution)
    image_resolution.iWidth = 640;
    image_resolution.iHeight = 480;
    image_resolution.iWidthFOV = image_resolution.iWidth;
    image_resolution.iHeightFOV = image_resolution.iHeight;
    image_resolution.iHOffsetFOV = 0;
    image_resolution.iVOffsetFOV = 0;

    status = CameraSetImageResolution(camera_handle, &image_resolution);
    if (status != CAMERA_STATUS_SUCCESS) {
        std::cout << "Failed to set resolution to 640x480, using default settings" << std::endl;
        // Try to get the current resolution after failed set
        CameraGetImageResolution(camera_handle, &image_resolution);
    } else {
        std::cout << "Successfully set camera resolution to: " << image_resolution.iWidth << "x" << image_resolution.iHeight << std::endl;
    }

    width = image_resolution.iWidth;
    height = image_resolution.iHeight;

    // 开启自动曝光
    CameraSetAeState(camera_handle, true);

    // 设置帧率
    CameraSetFrameSpeed(camera_handle, 10); // 设置为中等帧率

    std::cout << "Initialized camera with resolution: " << width << "x" << height << std::endl;
    
    return true;
}

MindVisionCamera::~MindVisionCamera() {
    release();
}

void MindVisionCamera::set_camera_parameters() {
    // 先设置为手动曝光模式
    CameraSetAeState(camera_handle, false);
    
    // 根据颜色设置曝光参数
    if (target_color == "red") {
        CameraSetExposureTime(camera_handle, 10000); // 5ms曝光时间
    } else if (target_color == "blue") {
        CameraSetExposureTime(camera_handle, 4000); // 8ms曝光时间
    }
    
    // 设置白平衡
    CameraSetWbMode(camera_handle, false); // 关闭自动白平衡
    CameraSetOnceWB(camera_handle);        // 一次性白平衡
    
    // 设置其他参数
    CameraSetTriggerMode(camera_handle, 0); // 连续采集模式
}

void MindVisionCamera::set_cam_params() {
    set_camera_parameters();

    // Ensure resolution remains 640x480 after setting other parameters
    tSdkImageResolution image_resolution;
    image_resolution.iIndex = 0xFF; // Use custom resolution (0xFF typically indicates custom resolution)
    image_resolution.iWidth = 640;
    image_resolution.iHeight = 480;
    image_resolution.iWidthFOV = image_resolution.iWidth;
    image_resolution.iHeightFOV = image_resolution.iHeight;
    image_resolution.iHOffsetFOV = 0;
    image_resolution.iVOffsetFOV = 0;

    int status = CameraSetImageResolution(camera_handle, &image_resolution);
    if (status != CAMERA_STATUS_SUCCESS) {
        std::cout << "Failed to maintain resolution at 640x480 in set_cam_params" << std::endl;
    } else {
        std::cout << "Confirmed camera resolution: " << image_resolution.iWidth << "x" << image_resolution.iHeight << std::endl;
    }
}

bool MindVisionCamera::read_frame(cv::Mat& frame) {
    if (!is_opened) {
        return false;
    }

    tSdkFrameHead sFrameInfo;
    BYTE* pbyBuffer;
    
    // 获取一帧数据
    if (CameraGetImageBuffer(camera_handle, &sFrameInfo, &pbyBuffer, 1000) == CAMERA_STATUS_SUCCESS) {
        // 处理图像数据
        unsigned char* g_pRgbBuffer = new unsigned char[sFrameInfo.iWidth * sFrameInfo.iHeight * 3];
        CameraImageProcess(camera_handle, pbyBuffer, g_pRgbBuffer, &sFrameInfo);
        
        // 创建OpenCV Mat对象
        frame = cv::Mat(
            cv::Size(sFrameInfo.iWidth, sFrameInfo.iHeight),
            sFrameInfo.uiMediaType == CAMERA_MEDIA_TYPE_MONO8 ? CV_8UC1 : CV_8UC3,
            g_pRgbBuffer
        ).clone(); // clone()确保数据被复制，而不是共享指针
        
        // 释放原始缓冲区
        CameraReleaseImageBuffer(camera_handle, pbyBuffer);
        
        // 释放临时缓冲区
        delete[] g_pRgbBuffer;
        
        return true;
    }
    
    return false;
}

void MindVisionCamera::release() {
    if (is_opened && camera_handle >= 0) {
        // 停止采集
        CameraUnInit(camera_handle);
        camera_handle = -1;
        is_opened = false;
    }
}

bool MindVisionCamera::switch_color(const std::string& new_color) {
    std::string lower_color = new_color;
    // Convert to lowercase manually
    for (auto& c : lower_color) {
        c = std::tolower(c);
    }

    if ((lower_color == "red" || lower_color == "blue") && lower_color != target_color) {
        target_color = lower_color;
        set_cam_params();
        return true;
    }
    return false;
}