#include "fusion_types.h"
#include "threat_cache.h"
#include "fusion_engine.h"
#include "avoidance_controller.h"
#include "gimbal_controller.h"

// 直接引用热成像检测 C++ 类 (替代 Python 的 subprocess 方式)
#include "thermal_preprocessor.h"
#include "thermal_detector.h"
#include "thermal_types.h"

#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <chrono>
#include <cmath>
#include <cstring>
#include <cstdio>
#include <set>
#include <atomic>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <queue>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// ── ROS 集成 (可选) ──
// 编译时定义 USE_ROS 启用 ROS 节点模式
// 否则编译为独立测试工具
//
// Prometheus uav_control 框架集成:
//   本节点通过 prometheus_msgs 与 uav_control 通信:
//     订阅: /uav{N}/prometheus/state         (UAVState, 获取无人机状态/航向)
//     订阅: /uav{N}/prometheus/control_state  (UAVControlState, 检查控制权)
//     发布: /uav{N}/prometheus/command        (UAVCommand, 避障速度指令)
//     发布: /uav{N}/prometheus/text_info      (TextInfo, 地面站状态消息)
//   图像来源:
//     订阅: /uav{N}/gimbal/image_original     (sensor_msgs/Image, SpireCV 吊舱)
#ifdef USE_ROS
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/String.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32MultiArray.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <diagnostic_msgs/KeyValue.h>
#include <cv_bridge/cv_bridge.h>

// Prometheus 消息类型
#include <prometheus_msgs/UAVCommand.h>
#include <prometheus_msgs/UAVState.h>
#include <prometheus_msgs/UAVControlState.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/MultiDetectionInfo.h>
#include <prometheus_msgs/GimbalControl.h>
#include <prometheus_msgs/GimbalState.h>
#include <prometheus_msgs/TextInfo.h>
#endif

// ══════════════════════════════════════════════════════════════
//  热成像桥接 — 直接调用 C++ 检测器 (替代 subprocess)
// ══════════════════════════════════════════════════════════════

class ThermalBridge {
public:
    ThermalBridge(double max_rate = 2.0)
        : max_rate_(max_rate), processing_(false), fail_count_(0),
          enabled_(true), last_time_(0) {
        preprocessor_ = std::make_unique<ThermalPreprocessor>();
        detector_ = std::make_unique<ThermalDetector>();
    }

    bool enabled() const { return enabled_; }

    // 处理一帧图像, 同步调用
    ThermalCacheEntry process(const cv::Mat& image) {
        ThermalCacheEntry entry;
        if (!enabled_) return entry;

        auto now = std::chrono::system_clock::now();
        double ts = std::chrono::duration<double>(now.time_since_epoch()).count();

        if (processing_.exchange(true)) return entry;
        if (ts - last_time_ < 1.0 / std::max(max_rate_, 0.1)) {
            processing_ = false;
            return entry;
        }
        last_time_ = ts;

        try {
            auto preprocessed = preprocessor_->preprocess(image);
            auto assessment = detector_->detect(preprocessed);

            entry.ambient_temp = assessment.ambient_temp;
            entry.timestamp = ts;

            for (auto& d : assessment.detections) {
                ThermalCacheEntry::Detection det;
                det.type = ThreatTypeInfo::label(d.type);
                det.confidence = d.confidence;
                det.max_temp = d.max_temp;
                det.bbox_x = d.bbox.x;
                det.bbox_y = d.bbox.y;
                det.bbox_w = d.bbox.width;
                det.bbox_h = d.bbox.height;
                entry.detections.push_back(det);
            }

            fail_count_ = 0;
        } catch (const std::exception& e) {
            std::fprintf(stderr, "[ThermalBridge] 检测异常: %s\n", e.what());
            fail_count_++;
            if (fail_count_ >= 5) {
                std::fprintf(stderr, "[ThermalBridge] 连续 %d 次失败, 禁用\n", fail_count_);
                enabled_ = false;
            }
        }

        processing_ = false;
        return entry;
    }

    // 处理一帧图像, 异步回调
    using Callback = std::function<void(const ThermalCacheEntry&)>;
    void process_async(const cv::Mat& image, Callback callback) {
        if (!enabled_ || processing_.exchange(true)) return;

        std::thread([this, image, callback]() {
            auto result = process(image);
            processing_ = false;
            if (!result.detections.empty()) {
                callback(result);
            }
        }).detach();
    }

private:
    double max_rate_;
    std::atomic<bool> processing_;
    int fail_count_;
    bool enabled_;
    double last_time_;
    std::unique_ptr<ThermalPreprocessor> preprocessor_;
    std::unique_ptr<ThermalDetector> detector_;
};

// ══════════════════════════════════════════════════════════════
//  工具函数
// ══════════════════════════════════════════════════════════════

static double get_time() {
    auto now = std::chrono::system_clock::now();
    return std::chrono::duration<double>(now.time_since_epoch()).count();
}

static double quaternion_to_yaw(double w, double x, double y, double z) {
    double siny_cosp = 2.0 * (w * z + x * y);
    double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
    double yaw = std::atan2(siny_cosp, cosy_cosp) * 180.0 / M_PI;
    return std::fmod(yaw + 360.0, 360.0);
}

// ══════════════════════════════════════════════════════════════
//  ROS 节点模式 — Prometheus uav_control 框架集成
// ══════════════════════════════════════════════════════════════

#ifdef USE_ROS

class ThreatFusionNode {
public:
    ThreatFusionNode()
        : cache_(0.5),
          bridge_(2.0),
          fusion_engine_(cache_, cfg_),
          avoidance_ctrl_(cfg_),
          gimbal_ctrl_(cfg_),
          uav_state_received_(false),
          command_id_(0)
    {
        load_params();
        setup_ros();
    }

    void run() {
        std::printf("[ThreatFusion] 节点已启动 (Prometheus uav_control 框架)\n");
        std::printf("[ThreatFusion] UAV%d | 命名空间: %s\n", cfg_.uav_id, cfg_.uav_namespace.c_str());
        std::printf("[ThreatFusion] 热成像:%s | 闪光:%s | 声源:%s\n",
                    cfg_.thermal_enabled ? "ON" : "OFF",
                    cfg_.flash_enabled ? "ON" : "OFF",
                    cfg_.acoustic_enabled ? "ON" : "OFF");

        // 连接吊舱
        if (gimbal_ctrl_.connect()) {
            std::printf("[ThreatFusion] AT10 吊舱已连接\n");
        }

        // 等待 uav_control 状态
        ROS_INFO("[ThreatFusion] 等待 /prometheus/state ...");
        ros::Time t0 = ros::Time::now();
        while (!uav_state_received_ && (ros::Time::now() - t0).toSec() < 10.0) {
            ros::spinOnce();
            ros::Duration(0.1).sleep();
        }
        if (!uav_state_received_) {
            ROS_WARN("[ThreatFusion] 未收到 UAVState, 继续运行 (可能 uav_control 未启动)");
        } else {
            ROS_INFO("[ThreatFusion] UAVState 已接收, 无人机已连接");
        }

        ros::spin();
    }

private:
    FusionConfig cfg_;
    ThreatCache cache_;
    ThermalBridge bridge_;
    FusionEngine fusion_engine_;
    AvoidanceController avoidance_ctrl_;
    GimbalController gimbal_ctrl_;

    // 无人机状态 (从 UAVState 获取)
    double drone_heading_ = 0;
    double drone_pos_x_ = 0, drone_pos_y_ = 0, drone_pos_z_ = 0;
    bool uav_state_received_ = false;
    bool uav_armed_ = false;
    uint8_t control_state_ = 0;  // UAVControlState.control_state

    FusionThreatLevel current_level_ = FusionThreatLevel::SAFE;
    std::vector<FusedThreat> current_threats_;
    bool avoidance_active_ = false;
    std::set<std::string> published_ids_;
    std::map<std::string, std::string> modality_status_;
    uint32_t command_id_ = 0;

    // ROS 接口
    // Prometheus 发布者
    ros::Publisher command_pub_;       // /uav{N}/prometheus/command
    ros::Publisher text_info_pub_;     // /uav{N}/prometheus/text_info
    ros::Publisher detection_pub_;     // /uav{N}/prometheus/detection
    // 自有话题
    ros::Publisher status_pub_;        // /uav{N}/threat_fusion/status
    ros::Publisher threat_pub_;        // /uav{N}/threat_fusion/fused_threats
    ros::Publisher avoid_pub_;         // /uav{N}/threat_fusion/avoidance_cmd
    ros::Publisher diag_pub_;          // /uav{N}/threat_fusion/diagnostics

    // Prometheus 订阅者
    ros::Subscriber uav_state_sub_;    // /uav{N}/prometheus/state
    ros::Subscriber ctrl_state_sub_;   // /uav{N}/prometheus/control_state
    // 图像/检测订阅者
    ros::Subscriber thermal_sub_;
    ros::Subscriber flash_bool_sub_;
    ros::Subscriber flash_det_sub_;
    // 云台状态订阅
    ros::Subscriber gimbal_state_sub_; // /uav{N}/gimbal/state

    ros::Timer fusion_timer_;
    ros::Timer status_timer_;
    ros::Timer diag_timer_;

    void load_params() {
        auto& nh = *ros::NodeHandle("~");

        // Prometheus uav_control 参数
        cfg_.uav_id = nh.param("uav_id", 1);
        cfg_.uav_namespace = "/uav" + std::to_string(cfg_.uav_id);

        cfg_.thermal_enabled = nh.param("thermal_enabled", true);
        cfg_.flash_enabled   = nh.param("flash_enabled", true);
        cfg_.acoustic_enabled = nh.param("acoustic_enabled", true);
        cfg_.thermal_image_topic = nh.param("thermal_image_topic",
            cfg_.uav_namespace + "/gimbal/image_original");
        cfg_.fusion_time_window = nh.param("fusion_time_window", 0.5);
        cfg_.fusion_confidence_threshold = nh.param("fusion_confidence_threshold", 0.5);
        cfg_.acoustic_confidence_threshold = nh.param("acoustic_confidence_threshold", 0.6);
        cfg_.avoidance_enabled = nh.param("avoidance_enabled", true);
        cfg_.avoidance_speed = nh.param("avoidance_speed", 2.0);
        cfg_.avoidance_decel_distance = nh.param("avoidance_decel_distance", 10.0);
        cfg_.avoidance_cooldown = nh.param("avoidance_cooldown", 5.0);
        cfg_.avoidance_hold_time = nh.param("avoidance_hold_time", 2.0);
        cfg_.avoidance_smoothing = nh.param("avoidance_smoothing", 0.3);
        cfg_.thermal_hfov = nh.param("thermal_hfov", 32.0);

        // 安全限制 (对齐 uav_control_outdoor.yaml)
        cfg_.maximum_safe_vel_xy = nh.param("maximum_safe_vel_xy", 5.0);
        cfg_.maximum_safe_vel_z  = nh.param("maximum_safe_vel_z", 4.0);

        // P600 吊舱参数
        cfg_.gimbal_port = nh.param("gimbal_port", std::string("/dev/ttyUSB1"));
        cfg_.gimbal_kp = nh.param("gimbal_kp", 1.0);
        cfg_.tracking_kp_x = nh.param("tracking_kp_x", 0.3);
        cfg_.tracking_kp_z = nh.param("tracking_kp_z", 0.1);
        cfg_.tracking_max_velocity = nh.param("tracking_max_velocity", 2.5);
        cfg_.tracking_distance = nh.param("tracking_distance", 10.0);
        cfg_.gimbal_max_yaw_rate = nh.param("gimbal_max_yaw_rate", 40);

        modality_status_["thermal"]  = cfg_.thermal_enabled ? "OK" : "DISABLED";
        modality_status_["flash"]    = cfg_.flash_enabled ? "OK" : "DISABLED";
        modality_status_["acoustic"] = cfg_.acoustic_enabled ? "OK" : "DISABLED";
    }

    void setup_ros() {
        ros::NodeHandle nh;

        // ── Prometheus 发布者 ──
        // 发布避障速度指令到 uav_control
        command_pub_ = nh.advertise<prometheus_msgs::UAVCommand>(
            cfg_.uav_namespace + "/prometheus/command", 10);
        // 发布地面站文本消息
        text_info_pub_ = nh.advertise<prometheus_msgs::TextInfo>(
            cfg_.uav_namespace + "/prometheus/text_info", 5);
        // 发布检测结果 (供其他模块使用, 如 yolov5_tracking)
        detection_pub_ = nh.advertise<prometheus_msgs::MultiDetectionInfo>(
            cfg_.uav_namespace + "/prometheus/detection", 5);

        // ── 自有话题发布者 ──
        status_pub_ = nh.advertise<std_msgs::String>(
            cfg_.uav_namespace + "/threat_fusion/status", 5);
        threat_pub_ = nh.advertise<std_msgs::String>(
            cfg_.uav_namespace + "/threat_fusion/fused_threats", 10);
        avoid_pub_  = nh.advertise<prometheus_msgs::UAVCommand>(
            cfg_.uav_namespace + "/threat_fusion/avoidance_cmd", 5);
        diag_pub_   = nh.advertise<diagnostic_msgs::DiagnosticArray>(
            cfg_.uav_namespace + "/threat_fusion/diagnostics", 5);

        // ── Prometheus 订阅者 ──
        // 订阅无人机状态 (替代 MAVROS pose 订阅)
        uav_state_sub_ = nh.subscribe(
            cfg_.uav_namespace + "/prometheus/state", 5,
            &ThreatFusionNode::uav_state_cb, this);
        // 订阅控制状态 (检查是否处于 COMMAND_CONTROL 模式)
        ctrl_state_sub_ = nh.subscribe(
            cfg_.uav_namespace + "/prometheus/control_state", 5,
            &ThreatFusionNode::ctrl_state_cb, this);
        // 订阅云台状态
        gimbal_state_sub_ = nh.subscribe(
            cfg_.uav_namespace + "/gimbal/state", 5,
            &ThreatFusionNode::gimbal_state_cb, this);

        // ── 图像/检测订阅者 ──
        if (cfg_.thermal_enabled) {
            thermal_sub_ = nh.subscribe(cfg_.thermal_image_topic, 1,
                &ThreatFusionNode::thermal_image_cb, this);
        }
        if (cfg_.flash_enabled) {
            flash_bool_sub_ = nh.subscribe("/flash_detector/flash_detected", 5,
                &ThreatFusionNode::flash_bool_cb, this);
            flash_det_sub_ = nh.subscribe("/flash_detector/detection", 5,
                &ThreatFusionNode::flash_detection_cb, this);
        }

        // ── 定时器 ──
        fusion_timer_ = nh.createTimer(ros::Duration(0.1),
            &ThreatFusionNode::fusion_timer_cb, this);
        status_timer_ = nh.createTimer(ros::Duration(0.5),
            &ThreatFusionNode::status_timer_cb, this);
        diag_timer_ = nh.createTimer(ros::Duration(1.0),
            &ThreatFusionNode::diag_timer_cb, this);
    }

    // ── Prometheus 回调 ──

    // 从 UAVState 获取无人机状态 (替代 MAVROS pose)
    // UAVState.attitude[2] = yaw (rad, ENU 坐标系)
    void uav_state_cb(const prometheus_msgs::UAVState::ConstPtr& msg) {
        uav_state_received_ = true;
        uav_armed_ = msg->armed;

        // 位置 (ENU)
        drone_pos_x_ = msg->position[0];
        drone_pos_y_ = msg->position[1];
        drone_pos_z_ = msg->position[2];

        // 航向: UAVState.attitude[2] 为 yaw (rad), ENU 坐标系
        // Prometheus 约定: yaw=0 指向东, 逆时针为正
        // 我们需要: 0=北, 顺时针为正
        double yaw_rad = msg->attitude[2];
        drone_heading_ = std::fmod((90.0 - yaw_rad * 180.0 / M_PI) + 360.0, 360.0);
    }

    void ctrl_state_cb(const prometheus_msgs::UAVControlState::ConstPtr& msg) {
        control_state_ = msg->control_state;
    }

    void gimbal_state_cb(const prometheus_msgs::GimbalState::ConstPtr& msg) {
        // 可用于同步云台角度状态
    }

    // ── 图像/检测回调 ──

    void thermal_image_cb(const sensor_msgs::Image::ConstPtr& msg) {
        try {
            auto cv_img = cv_bridge::toCvShare(msg, "bgr8");
            auto entry = bridge_.process(cv_img->image);
            if (!entry.detections.empty()) {
                entry.timestamp = get_time();
                cache_.add_thermal(entry);

                // 发布检测结果为 Prometheus DetectionInfo (供追踪模块使用)
                publish_detections(entry, msg->header);

                // 用第一个人员检测的偏移量控制云台追踪
                for (auto& d : entry.detections) {
                    if (d.type == "Person" && d.bbox_w > 0) {
                        double img_cx = msg->width / 2.0;
                        double img_cy = msg->height / 2.0;
                        double det_cx = d.bbox_x + d.bbox_w / 2.0;
                        double det_cy = d.bbox_y + d.bbox_h / 2.0;
                        double offset_x = (det_cx - img_cx) / img_cx;
                        double offset_y = (det_cy - img_cy) / img_cy;
                        gimbal_ctrl_.trackTarget(offset_x, offset_y);
                        break;
                    }
                }
            }
        } catch (const cv_bridge::Exception& e) {
            ROS_WARN("[ThreatFusion] cv_bridge 异常: %s", e.what());
        }
    }

    void flash_bool_cb(const std_msgs::Bool::ConstPtr& msg) {}

    void flash_detection_cb(const std_msgs::Float32MultiArray::ConstPtr& msg) {
        auto& data = msg->data;
        if (data.size() >= 5) {
            FlashDetection f;
            f.cx = data[0]; f.cy = data[1]; f.area = data[2];
            f.sight_x = data[3]; f.sight_y = data[4];
            f.timestamp = get_time();
            cache_.add_flash(f);
        }
    }

    // ── 定时器 ──

    void fusion_timer_cb(const ros::TimerEvent&) {
        double now = get_time();
        auto threats = fusion_engine_.fuse(now, drone_heading_);
        current_threats_ = threats;

        current_level_ = FusionThreatLevel::SAFE;
        for (auto& t : threats) {
            if (t.level > current_level_) current_level_ = t.level;
        }

        // 发布新威胁
        for (auto& t : threats) {
            if (published_ids_.find(t.threat_id) == published_ids_.end()) {
                publish_fused_threat(t);
                published_ids_.insert(t.threat_id);
            }
        }
        if (published_ids_.size() > 100) {
            std::set<std::string> keep(published_ids_.rbegin(), std::next(published_ids_.rbegin(), 50));
            published_ids_ = keep;
        }

        // 避障: 通过 Prometheus UAVCommand 发送速度指令
        AvoidanceCmd cmd;
        avoidance_active_ = avoidance_ctrl_.compute(threats, now, cmd);
        if (avoidance_active_ && uav_armed_) {
            // 限速 (与 uav_control 安全限制对齐)
            cmd.vx = std::max(-cfg_.maximum_safe_vel_xy, std::min(cfg_.maximum_safe_vel_xy, cmd.vx));
            cmd.vy = std::max(-cfg_.maximum_safe_vel_xy, std::min(cfg_.maximum_safe_vel_xy, cmd.vy));
            cmd.vz = std::max(-cfg_.maximum_safe_vel_z, std::min(cfg_.maximum_safe_vel_z, cmd.vz));

            // 地理围栏检查
            double new_x = drone_pos_x_ + cmd.vx * 0.1;
            double new_y = drone_pos_y_ + cmd.vy * 0.1;
            double new_z = drone_pos_z_ + cmd.vz * 0.1;
            if (new_x < cfg_.geo_fence_x_min || new_x > cfg_.geo_fence_x_max ||
                new_y < cfg_.geo_fence_y_min || new_y > cfg_.geo_fence_y_max ||
                new_z < cfg_.geo_fence_z_min || new_z > cfg_.geo_fence_z_max) {
                ROS_WARN("[ThreatFusion] 避障指令超出地理围栏, 已拦截");
                avoidance_active_ = false;
            } else {
                // 发布到 Prometheus 命令话题
                auto uav_cmd = make_avoidance_command(cmd);
                command_pub_.publish(uav_cmd);
                avoid_pub_.publish(uav_cmd);
            }
        }
    }

    void status_timer_cb(const ros::TimerEvent&) {
        // 发布自有状态 (JSON)
        char buf[512];
        std::snprintf(buf, sizeof(buf),
            "{\"threat_level\":\"%s\",\"threat_count\":%d,\"avoidance_active\":%s,"
            "\"drone_heading\":%.1f,\"uav_armed\":%s,\"control_state\":%d,"
            "\"modalities\":{\"thermal\":\"%s\",\"flash\":\"%s\",\"acoustic\":\"%s\"},"
            "\"timestamp\":%.3f}",
            FusionThreatLevelInfo::label(current_level_),
            (int)current_threats_.size(),
            avoidance_active_ ? "true" : "false",
            drone_heading_,
            uav_armed_ ? "true" : "false",
            (int)control_state_,
            modality_status_["thermal"].c_str(),
            modality_status_["flash"].c_str(),
            modality_status_["acoustic"].c_str(),
            get_time());

        std_msgs::String msg;
        msg.data = buf;
        status_pub_.publish(msg);

        // 同时发布 Prometheus TextInfo (地面站可见)
        if (current_level_ >= FusionThreatLevel::WARNING) {
            prometheus_msgs::TextInfo info;
            info.header.stamp = ros::Time::now();
            if (current_level_ >= FusionThreatLevel::CRITICAL)
                info.MessageType = prometheus_msgs::TextInfo::FATAL;
            else if (current_level_ >= FusionThreatLevel::DANGER)
                info.MessageType = prometheus_msgs::TextInfo::ERROR;
            else
                info.MessageType = prometheus_msgs::TextInfo::WARN;

            char info_buf[256];
            std::snprintf(info_buf, sizeof(info_buf),
                "ThreatFusion: %s | %d threats | avoidance=%s",
                FusionThreatLevelInfo::label(current_level_),
                (int)current_threats_.size(),
                avoidance_active_ ? "ON" : "OFF");
            info.Message = info_buf;
            text_info_pub_.publish(info);
        }
    }

    void diag_timer_cb(const ros::TimerEvent&) {
        diagnostic_msgs::DiagnosticArray diag;
        diag.header.stamp = ros::Time::now();
        diagnostic_msgs::DiagnosticStatus ds;
        ds.name = cfg_.uav_namespace + "/threat_fusion";
        ds.level = (current_level_ >= FusionThreatLevel::DANGER)
                   ? diagnostic_msgs::DiagnosticStatus::WARN
                   : diagnostic_msgs::DiagnosticStatus::OK;

        char buf[128];
        std::snprintf(buf, sizeof(buf), "Level: %s, Threats: %d",
                      FusionThreatLevelInfo::label(current_level_),
                      (int)current_threats_.size());
        ds.message = buf;

        diagnostic_msgs::KeyValue kv;
        kv.key = "thermal"; kv.value = modality_status_["thermal"]; ds.values.push_back(kv);
        kv.key = "flash";   kv.value = modality_status_["flash"];   ds.values.push_back(kv);
        kv.key = "acoustic"; kv.value = modality_status_["acoustic"]; ds.values.push_back(kv);
        kv.key = "avoidance_active"; kv.value = avoidance_active_ ? "true" : "false"; ds.values.push_back(kv);
        kv.key = "uav_state_received"; kv.value = uav_state_received_ ? "true" : "false"; ds.values.push_back(kv);
        diag.status.push_back(ds);
        diag_pub_.publish(diag);
    }

    // ── 辅助 ──

    // 构建 Prometheus 避障命令 (ENU 速度模式)
    prometheus_msgs::UAVCommand make_avoidance_command(const AvoidanceCmd& cmd) {
        prometheus_msgs::UAVCommand ucmd;
        ucmd.header.stamp = ros::Time::now();
        ucmd.Agent_CMD = prometheus_msgs::UAVCommand::Move;
        ucmd.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL;  // ENU 速度控制
        ucmd.velocity_ref[0] = cmd.vx;  // East
        ucmd.velocity_ref[1] = cmd.vy;  // North
        ucmd.velocity_ref[2] = cmd.vz;  // Up
        ucmd.yaw_ref = drone_heading_ * M_PI / 180.0;  // 保持当前航向
        ucmd.Command_ID = ++command_id_;
        return ucmd;
    }

    // 发布热成像检测结果为 Prometheus MultiDetectionInfo
    void publish_detections(const ThermalCacheEntry& entry,
                            const std_msgs::Header& img_header) {
        prometheus_msgs::MultiDetectionInfo multi;
        multi.header = img_header;
        multi.num_objs = (int32_t)entry.detections.size();
        multi.detect_or_track = 0;  // 0=detection

        for (auto& d : entry.detections) {
            prometheus_msgs::DetectionInfo det;
            det.header = img_header;
            det.object_name = d.type;
            det.detected = true;
            det.frame = 0;  // 本地坐标系
            det.pixel_position[0] = d.bbox_x + d.bbox_w / 2;
            det.pixel_position[1] = d.bbox_y + d.bbox_h / 2;
            // 类别映射
            if (d.type == "Person") det.category = 1;
            else if (d.type == "Hotspot") det.category = 2;
            else if (d.type.find("Barrel") != std::string::npos) det.category = 3;
            else if (d.type.find("Weapon") != std::string::npos || d.type.find("Gun") != std::string::npos)
                det.category = 4;
            else det.category = 99;

            multi.detection_infos.push_back(det);
        }
        detection_pub_.publish(multi);
    }

    void publish_fused_threat(const FusedThreat& t) {
        char buf[512];
        std::string sources_str = "[";
        for (size_t i = 0; i < t.sources.size(); i++) {
            if (i > 0) sources_str += ",";
            sources_str += "\"" + t.sources[i] + "\"";
        }
        sources_str += "]";

        std::snprintf(buf, sizeof(buf),
            "{\"id\":\"%s\",\"level\":\"%s\",\"confidence\":%.3f,"
            "\"bearing\":%.1f,\"elevation\":%.1f,\"distance\":%.1f,"
            "\"sources\":%s,\"description\":\"%s\",\"timestamp\":%.3f}",
            t.threat_id.c_str(),
            FusionThreatLevelInfo::label(t.level),
            t.confidence,
            t.bearing, t.elevation, t.distance,
            sources_str.c_str(),
            t.description.c_str(),
            t.timestamp);

        std_msgs::String msg;
        msg.data = buf;
        threat_pub_.publish(msg);
    }
};

#endif // USE_ROS

// ══════════════════════════════════════════════════════════════
//  独立测试模式 (无 ROS)
// ══════════════════════════════════════════════════════════════

#ifndef USE_ROS

static void print_usage(const char* prog) {
    std::cout << "多模态威胁融合 — 独立测试模式\n"
              << "用法:\n"
              << "  " << prog << " <image> [options]\n"
              << "  " << prog << " <directory> [options]\n\n"
              << "选项:\n"
              << "  --no-show       不显示结果\n"
              << "  --json          JSON 输出\n"
              << "  --simulate      模拟融合流程 (用热成像结果模拟多模态)\n"
              << "  --gimbal-test   测试云台控制 (需要 SpireCV)\n"
              << "  --help          显示帮助\n";
}

static std::string get_output_path(const std::string& input_path) {
    auto pos = input_path.find_last_of("/\\");
    std::string dir = (pos != std::string::npos) ? input_path.substr(0, pos + 1) : "";
    std::string fname = (pos != std::string::npos) ? input_path.substr(pos + 1) : input_path;
    return dir + "fusion_" + fname;
}

static bool has_image_ext(const std::string& path) {
    std::string ext;
    auto pos = path.rfind('.');
    if (pos != std::string::npos) ext = path.substr(pos);
    return ext == ".jpg" || ext == ".jpeg" || ext == ".png" || ext == ".bmp" || ext == ".tif";
}

// 模拟融合: 用热成像检测结果模拟多模态融合
static void simulate_fusion(const ThermalCacheEntry& thermal_entry,
                            const FusionConfig& cfg,
                            double heading,
                            int image_width)
{
    ThreatCache cache(cfg.fusion_time_window);
    cache.add_thermal(thermal_entry);

    FusionEngine engine(cache, cfg);
    auto threats = engine.fuse(get_time(), heading, image_width);

    std::cout << "\n── 融合结果 ──\n";
    std::cout << "威胁数量: " << threats.size() << "\n";
    for (auto& t : threats) {
        std::cout << "  [" << FusionThreatLevelInfo::label(t.level) << "] "
                  << t.threat_id << " conf=" << (int)(t.confidence * 100) << "% "
                  << "bearing=" << (int)t.bearing << "° "
                  << "dist=" << t.distance << "m "
                  << "sources=";
        for (auto& s : t.sources) std::cout << s << ",";
        std::cout << " " << t.description << "\n";
    }

    // 避障
    AvoidanceController avoid(cfg);
    AvoidanceCmd cmd;
    if (avoid.compute(threats, get_time(), cmd)) {
        std::cout << "  避障指令: vx=" << cmd.vx << " vy=" << cmd.vy << " vz=" << cmd.vz << "\n";
    } else {
        std::cout << "  无需避障\n";
    }
}

int main(int argc, char* argv[]) {
    if (argc < 2) { print_usage(argv[0]); return 1; }

    std::string source;
    bool show = true, json_out = false, simulate = false, gimbal_test = false;

    for (int i = 1; i < argc; i++) {
        if (std::strcmp(argv[i], "--no-show") == 0)     show = false;
        else if (std::strcmp(argv[i], "--json") == 0)    json_out = true;
        else if (std::strcmp(argv[i], "--simulate") == 0) simulate = true;
        else if (std::strcmp(argv[i], "--gimbal-test") == 0) gimbal_test = true;
        else if (std::strcmp(argv[i], "--help") == 0)    { print_usage(argv[0]); return 0; }
        else if (source.empty())                          source = argv[i];
    }

    if (!gimbal_test && source.empty()) { print_usage(argv[0]); return 1; }

    // 云台测试
    if (gimbal_test) {
        FusionConfig cfg;
        GimbalController gimbal(cfg);
        std::cout << "连接 AT10 吊舱...\n";
        if (!gimbal.connect()) {
            std::cerr << "连接失败\n";
            return 1;
        }
        std::cout << "已连接, 测试控制...\n";

        // 切换到红外白热
        gimbal.setFrameType(GimbalImageType::IR_WHITEHOT);
        // 俯视 45 度
        gimbal.setDownwardAngle(45.0);
        // 追踪偏移
        gimbal.trackTarget(0.3, -0.2);
        // 拍照
        gimbal.takePhoto();

        std::cout << "测试完成\n";
        gimbal.disconnect();
        return 0;
    }

    // 图像处理
    cv::Mat img = cv::imread(source, cv::IMREAD_COLOR);
    if (img.empty()) img = cv::imread(source, cv::IMREAD_GRAYSCALE);
    if (img.empty()) {
        std::cerr << "无法读取: " << source << "\n";
        return 1;
    }

    // 热成像检测 (直接 C++ 调用, 无 subprocess)
    ThermalBridge bridge;

    auto t0 = std::chrono::high_resolution_clock::now();
    auto entry = bridge.process(img);
    auto t1 = std::chrono::high_resolution_clock::now();
    double ms = std::chrono::duration<double, std::milli>(t1 - t0).count();

    // 输出
    if (json_out) {
        std::cout << "{\n";
        std::cout << "  \"detection_count\": " << entry.detections.size() << ",\n";
        std::cout << "  \"ambient_temp\": " << entry.ambient_temp << ",\n";
        std::cout << "  \"time_ms\": " << ms << ",\n";
        std::cout << "  \"detections\": [\n";
        for (size_t i = 0; i < entry.detections.size(); i++) {
            auto& d = entry.detections[i];
            std::cout << "    {\"type\":\"" << d.type
                      << "\", \"confidence\":" << d.confidence
                      << ", \"max_temp\":" << d.max_temp
                      << ", \"bbox\":{\"x\":" << d.bbox_x
                      << ",\"y\":" << d.bbox_y
                      << ",\"w\":" << d.bbox_w
                      << ",\"h\":" << d.bbox_h << "}}";
            if (i + 1 < entry.detections.size()) std::cout << ",";
            std::cout << "\n";
        }
        std::cout << "  ]\n}\n";
    } else {
        std::cout << "========================================\n";
        std::cout << " Image: " << source << "\n";
        std::cout << " Time: " << ms << " ms\n";
        std::cout << " Ambient: " << entry.ambient_temp << " C\n";
        std::cout << " Detections: " << entry.detections.size() << "\n";
        for (auto& d : entry.detections) {
            std::cout << "    [" << d.type << "] conf:" << (int)(d.confidence * 100)
                      << "% temp:" << d.max_temp << "C area:" << d.bbox_w * d.bbox_h << "\n";
        }
        std::cout << "========================================\n";
    }

    // 模拟融合
    if (simulate && !entry.detections.empty()) {
        simulate_fusion(entry, FusionConfig{}, 0.0, img.cols);
    }

    // 可视化 (复用 thermal_detector 的绘制)
    ThermalPreprocessor preprocessor;
    ThermalDetector detector;
    auto preprocessed = preprocessor.preprocess(img);
    auto assessment = detector.detect(preprocessed);
    cv::Mat vis = ThermalDetector::draw_detections(img, assessment);

    // 在可视化图上叠加融合信息
    if (simulate && !entry.detections.empty()) {
        ThreatCache cache_vis;
        cache_vis.add_thermal(entry);
        FusionEngine engine_vis(cache_vis, FusionConfig{});
        auto threats = engine_vis.fuse(get_time(), 0.0, img.cols);

        int y_off = 30;
        for (auto& t : threats) {
            std::string text = std::string("[") + FusionThreatLevelInfo::label(t.level)
                             + "] " + t.description;
            cv::putText(vis, text, {10, y_off}, cv::FONT_HERSHEY_SIMPLEX, 0.5,
                        {0, 0, 255}, 1);
            y_off += 20;
        }
    }

    std::string out_path = get_output_path(source);
    cv::imwrite(out_path, vis);
    if (!json_out) std::cout << " Saved: " << out_path << "\n";

    if (show) {
        cv::imshow("Fusion Detection", vis);
        cv::waitKey(0);
        cv::destroyAllWindows();
    }

    return 0;
}

#else // USE_ROS

int main(int argc, char** argv) {
    ros::init(argc, argv, "threat_fusion_node");
    ThreatFusionNode node;
    node.run();
    return 0;
}

#endif // USE_ROS
