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

Camera::Camera(int cam_id, const std::string& target_color) {
    // 先设置为 MJPEG 模式再打开（某些驱动要求）
    cap.open(cam_id, cv::CAP_V4L2);
    if (!cap.isOpened()) {
        throw std::runtime_error("Cannot open camera!");
    }

    // 必须先设 FOURCC，再设分辨率和帧率！顺序很重要
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_FPS, 100)

    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(); // 再次应用参数（包括曝光等）
}

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

void Camera::set_cam_params() {
    // 先设 FOURCC（顺序很重要！）
    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));
    
    const int width = 640;
    const int height = 480;
    const int fps = 60; // 从 80 降到 60

    cap.set(cv::CAP_PROP_FRAME_WIDTH, width);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, height);
    cap.set(cv::CAP_PROP_FPS, fps);

    cap.set(cv::CAP_PROP_AUTOFOCUS, 1);
    cap.set(cv::CAP_PROP_AUTO_WB, 1);
    cap.set(cv::CAP_PROP_AUTO_EXPOSURE, 0); // 手动曝光

    if (target_color == "red") {
        cap.set(cv::CAP_PROP_EXPOSURE, -50); // 可微调
        //cap.set(cv::CAP_PROP_CONTRAST,50);
    } else if (target_color == "blue") {
        cap.set(cv::CAP_PROP_EXPOSURE, -8);
    }
    cap.set(cv::CAP_PROP_WB_TEMPERATURE, 3200);

    // 验证
    double w = cap.get(cv::CAP_PROP_FRAME_WIDTH);
    double h = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
    double f = cap.get(cv::CAP_PROP_FPS);
    double fourcc = cap.get(cv::CAP_PROP_FOURCC);
    char str[5] = {0};
    memcpy(str, &fourcc, 4);
    std::cout << "Actual: " << w << "x" << h << " @ " << f << "fps, FOURCC='" << str << "'" << std::endl;
}
bool Camera::read_frame(cv::Mat& frame) {
    if (!is_opened) {
        return false;
    }
    return cap.read(frame);
}

void Camera::release() {
    if (is_opened) {
        cap.release();
        is_opened = false;
    }
}

bool Camera::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;
}