#include "gimbal_controller.h"
#include <cstdio>
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

GimbalController::GimbalController(const FusionConfig& cfg)
    : cfg_(cfg) {}

GimbalController::~GimbalController() {
    disconnect();
}

bool GimbalController::connect() {
#ifdef USE_SPIRECV
    if (gimbal_) return true;

    // P600手册: AT10 吊舱通过串口控制, 默认 /dev/ttyUSB1
    gimbal_ = new sv::Gimbal(
        sv::GimbalType::GIMBAL_TYPE_AT10,
        sv::GimbalSerial(cfg_.gimbal_port)
    );

    if (!gimbal_->open()) {
        std::fprintf(stderr, "[GimbalController] 无法打开吊舱: %s\n", cfg_.gimbal_port.c_str());
        delete gimbal_;
        gimbal_ = nullptr;
        state_.connected = false;
        return false;
    }

    state_.connected = true;
    std::printf("[GimbalController] AT10 吊舱已连接: %s\n", cfg_.gimbal_port.c_str());

    // 默认切到红外白热模式
    setFrameType(GimbalImageType::IR_WHITEHOT);
    return true;
#else
    std::printf("[GimbalController] SpireCV 未启用, 吊舱模拟模式\n");
    state_.connected = true;
    return true;
#endif
}

void GimbalController::disconnect() {
#ifdef USE_SPIRECV
    if (gimbal_) {
        gimbal_->close();
        delete gimbal_;
        gimbal_ = nullptr;
    }
#endif
    state_.connected = false;
}

void GimbalController::setAngleEuler(double roll, double pitch, double yaw) {
    state_.roll  = roll;
    state_.pitch = pitch;
    state_.yaw   = yaw;

#ifdef USE_SPIRECV
    if (gimbal_) {
        // P600手册: setAngleEuler(roll, pitch, yaw)
        // pitch 下俯=正, yaw 左转=正
        gimbal_->setAngleEuler(roll, pitch, yaw);
    }
#else
    std::printf("[Gimbal] setAngleEuler(%.1f, %.1f, %.1f)\n", roll, pitch, yaw);
#endif
}

void GimbalController::setAngleRateEuler(double roll, double pitch_rate, double yaw_rate) {
#ifdef USE_SPIRECV
    if (gimbal_) {
        gimbal_->setAngleRateEuler(roll, pitch_rate, yaw_rate);
    }
#else
    std::printf("[Gimbal] setAngleRateEuler(%.1f, %.1f, %.1f)\n", roll, pitch_rate, yaw_rate);
#endif
}

void GimbalController::setFrameType(GimbalImageType type) {
    state_.image_type = type;

#ifdef USE_SPIRECV
    if (gimbal_) {
        // SpireCV 图像类型映射 (P600手册 p46-48)
        sv::GimbalImageType sv_type;
        switch (type) {
            case GimbalImageType::EO:             sv_type = sv::GimbalImageType::GIMBAL_EO; break;
            case GimbalImageType::IR_WHITEHOT:    sv_type = sv::GimbalImageType::GIMBAL_IRW; break;
            case GimbalImageType::IR_PSEUDOCOLOR: sv_type = sv::GimbalImageType::GIMBAL_IRC; break;
            case GimbalImageType::EO_IR_WHITEHOT: sv_type = sv::GimbalImageType::GIMBAL_EO_IRW; break;
            case GimbalImageType::EO_IR_COLOR:    sv_type = sv::GimbalImageType::GIMBAL_EO_IRC; break;
            default: sv_type = sv::GimbalImageType::GIMBAL_IRW; break;
        }
        gimbal_->setFrameType(sv_type);
    }
#else
    std::printf("[Gimbal] setFrameType(%s)\n", gimbal_image_type_str(type));
#endif
}

void GimbalController::trackTarget(double target_offset_x, double target_offset_y) {
    // P600手册追踪参数: kp_gimbal=1.0
    double kp = cfg_.gimbal_kp;

    // 将归一化偏移 [-1, 1] 转为角速率
    double max_yaw_rate = cfg_.gimbal_max_yaw_rate;
    double yaw_rate  = -target_offset_x * kp * max_yaw_rate;
    double pitch_rate = target_offset_y * kp * max_yaw_rate;

    // 限制角速率
    yaw_rate  = std::max(-(double)max_yaw_rate, std::min((double)max_yaw_rate, yaw_rate));
    pitch_rate = std::max(-(double)max_yaw_rate, std::min((double)max_yaw_rate, pitch_rate));

    setAngleRateEuler(0, pitch_rate, yaw_rate);
}

void GimbalController::setDownwardAngle(double angle_deg) {
    // P600手册: downward_locked_angle() → setAngleEuler(0, angle, 0)
    setAngleEuler(0, angle_deg, 0);
    std::printf("[Gimbal] 俯仰锁定: %.1f 度\n", angle_deg);
}

void GimbalController::takePhoto() {
#ifdef USE_SPIRECV
    if (gimbal_) {
        gimbal_->takePhoto();
    }
#else
    std::printf("[Gimbal] takePhoto()\n");
#endif
}

void GimbalController::setZoom(double factor) {
#ifdef USE_SPIRECV
    if (gimbal_) {
        gimbal_->setZoom(factor);
    }
#else
    std::printf("[Gimbal] setZoom(%.1f)\n", factor);
#endif
}
