#pragma once

#include "fusion_types.h"
#include <string>
#include <functional>

// ── 吊舱控制器 (SpireCV AT10 Pro) ──
// 参考: P600双光吊舱使用说明 §5 二次开发
//
// SpireCV API:
//   gimbal.setAngleEuler(roll, pitch, yaw)        — pitch下俯=正, yaw左=正
//   gimbal.setAngleRateEuler(roll, pitch_rate, yaw_rate) — 角速率控制
//   gimbal.setFrameType(GimbalImageType)           — 切换图像类型
//   gimbal.takePhoto()
//   gimbal.setZoom(double)
//
// ROS 话题 (SpireCV at10_gimbal_server):
//   订阅: /uav1/gimbal/image_original  (原始图像)
//   订阅: /uav1/spirecv/image_det       (检测结果叠加图)
//   服务: /uav1/gimbal/set_frame_type    (切换可见光/红外)
//   服务: /uav1/gimbal_downward_lock     (俯仰锁定角度)
//
// 追踪参数 (P600手册 p29):
//   kp_x=0.3, kp_z=0.1, kp_gimbal=1.0
//   max_velocity=2.5m/s, tracking_distance=10.0m

// 当 USE_SPIRECV 宏定义时启用真实 SpireCV 集成,
// 否则提供空实现用于测试

#ifdef USE_SPIRECV
#include <spirecv/gimbal/Gimbal.hpp>
#endif

class GimbalController {
public:
    struct State {
        double roll  = 0;   // 度
        double pitch = 0;   // 度 (下俯为正)
        double yaw   = 0;   // 度 (左转为正)
        GimbalImageType image_type = GimbalImageType::IR_WHITEHOT;
        bool connected = false;
    };

    explicit GimbalController(const FusionConfig& cfg);
    ~GimbalController();

    // 连接/断开吊舱
    bool connect();
    void disconnect();
    bool is_connected() const { return state_.connected; }

    // 角度控制 — 绝对角度
    // pitch: 下俯为正 (P600手册约定)
    // yaw:   左转为正
    void setAngleEuler(double roll, double pitch, double yaw);

    // 角速率控制 (度/秒)
    // 用于追踪目标时平滑旋转
    void setAngleRateEuler(double roll, double pitch_rate, double yaw_rate);

    // 切换图像类型
    // GIMBAL_EO=可见光, GIMBAL_IRW=红外白热, GIMBAL_IRC=红外伪彩
    void setFrameType(GimbalImageType type);

    // 追踪目标: 根据目标在图像中的偏移控制云台旋转
    // target_offset_x, target_offset_y: 目标相对于图像中心的归一化偏移 [-1, 1]
    void trackTarget(double target_offset_x, double target_offset_y);

    // 俯仰锁定: 俯视指定角度 (用于固定监控)
    // P600手册: downward_locked_angle() → setAngleEuler(0, angle, 0)
    void setDownwardAngle(double angle_deg);

    // 拍照
    void takePhoto();

    // 变焦
    void setZoom(double factor);

    // 获取当前状态
    const State& state() const { return state_; }

private:
    FusionConfig cfg_;
    State state_;

#ifdef USE_SPIRECV
    sv::Gimbal* gimbal_ = nullptr;
#endif
};
