#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
多模态威胁融合节点 — 热成像 + 闪光 + 声源 → 避障

融合三条感知管道的输出，生成综合威胁评估和避障速度指令，
通过 MAVROS 发送给 PX4 执行。

架构:
  热成像 exe (subprocess) ──┐
  闪光检测 ROS Node  ───────┼──► ThreatFusionNode ──► MAVROS ──► PX4
  声源检测 ROS Node   ───────┘        │
                                     ▼
                              地面站 (rosbridge → 浏览器)

用法:
  rosrun threat_fusion threat_fusion_node.py
  rosrun threat_fusion threat_fusion_node.py _thermal_exe_path:=/path/to/thermal_detector
"""

from __future__ import print_function

import json
import math
import os
import subprocess
import sys
import tempfile
import threading
import time
from collections import deque
from concurrent.futures import ThreadPoolExecutor
from dataclasses import dataclass, field
from enum import IntEnum
from typing import Any, Dict, List, Optional, Tuple

import numpy as np

import rospy
import std_msgs.msg
from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3
from sensor_msgs.msg import Image
from std_msgs.msg import Bool, Float32MultiArray, String
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue

try:
    from cv_bridge import CvBridge
    HAS_CV_BRIDGE = True
except ImportError:
    HAS_CV_BRIDGE = False

try:
    import cv2
    HAS_CV2 = True
except ImportError:
    HAS_CV2 = False

# acoustic_analyzer 的自定义消息
try:
    from acoustic_analyzer.msg import AcousticThreat, AcousticThreatArray
    HAS_ACOUSTIC_MSG = True
except ImportError:
    HAS_ACOUSTIC_MSG = False


# ══════════════════════════════════════════════════════════════
#  数据结构
# ══════════════════════════════════════════════════════════════

class ThreatLevel(IntEnum):
    SAFE = 0
    CAUTION = 1
    WARNING = 2
    DANGER = 3
    CRITICAL = 4

    @property
    def label(self):
        return ["Safe", "Caution", "Warning", "Danger", "Critical"][self.value]


@dataclass
class FusedThreat:
    threat_id: str = ""
    level: ThreatLevel = ThreatLevel.SAFE
    confidence: float = 0.0
    bearing: float = 0.0       # 方位角 (度), 0=北, 顺时针
    elevation: float = 0.0     # 仰角 (度)
    distance: float = -1.0     # 距离 (米), -1=未知
    sources: list = field(default_factory=list)
    description: str = ""
    timestamp: float = 0.0
    details: dict = field(default_factory=dict)


# ══════════════════════════════════════════════════════════════
#  时间窗口缓存
# ══════════════════════════════════════════════════════════════

class ThreatCache:
    def __init__(self, time_window=0.5):
        self.time_window = time_window
        self._thermal = deque(maxlen=20)
        self._flash = deque(maxlen=5)
        self._acoustic = deque(maxlen=20)
        self._lock = threading.Lock()

    def add_thermal(self, detections, timestamp):
        with self._lock:
            self._thermal.append({"data": detections, "time": timestamp})

    def add_flash(self, cx, cy, area, sight_x, sight_y, timestamp):
        with self._lock:
            self._flash.append({
                "cx": cx, "cy": cy, "area": area,
                "sight_x": sight_x, "sight_y": sight_y,
                "time": timestamp,
            })

    def add_acoustic(self, threats, timestamp):
        with self._lock:
            for t in threats:
                self._acoustic.append({"data": t, "time": timestamp})

    def get_thermal(self, now):
        cutoff = now - self.time_window
        with self._lock:
            return [e for e in self._thermal if e["time"] >= cutoff]

    def get_flash(self, now):
        cutoff = now - self.time_window
        with self._lock:
            items = [e for e in self._flash if e["time"] >= cutoff]
            return items[-1] if items else None

    def get_acoustic(self, now):
        cutoff = now - self.time_window
        with self._lock:
            return [e for e in self._acoustic if e["time"] >= cutoff]

    def clear(self):
        with self._lock:
            self._thermal.clear()
            self._flash.clear()
            self._acoustic.clear()


# ══════════════════════════════════════════════════════════════
#  热成像 exe 子进程封装
# ══════════════════════════════════════════════════════════════

class ThermalBridge:
    def __init__(self, exe_path, timeout=5.0, max_rate=2.0):
        self.exe_path = exe_path
        self.timeout = timeout
        self.max_rate = max_rate
        self._processing = False
        self._last_time = 0.0
        self._fail_count = 0
        self._enabled = os.path.isfile(exe_path) if exe_path else False
        self._bridge = CvBridge() if HAS_CV_BRIDGE else None
        self._executor = ThreadPoolExecutor(max_workers=1)
        self._temp_files = []

        if not self._enabled:
            if exe_path:
                rospy.logwarn("[ThermalBridge] exe 不存在: %s, 热成像模态已禁用", exe_path)
            else:
                rospy.logwarn("[ThermalBridge] 未配置 thermal_exe_path, 热成像模态已禁用")

    @property
    def enabled(self):
        return self._enabled

    def process_image(self, image_msg, callback):
        if not self._enabled or not HAS_CV2 or not HAS_CV_BRIDGE:
            return
        now = time.time()
        if self._processing:
            return
        if now - self._last_time < 1.0 / max(self.max_rate, 0.1):
            return
        self._processing = True
        self._last_time = now
        self._executor.submit(self._process, image_msg, callback)

    def _process(self, image_msg, callback):
        tmp_path = None
        try:
            tmp_path = self._save_temp_image(image_msg)
            stdout = self._invoke_exe(tmp_path)
            if stdout:
                result = self._parse_json(stdout)
                if result is not None:
                    self._fail_count = 0
                    callback(result)
                    return
            self._fail_count += 1
            if self._fail_count >= 5:
                rospy.logerr("[ThermalBridge] 连续 %d 次失败, 禁用热成像模态", self._fail_count)
                self._enabled = False
        except Exception as e:
            rospy.logwarn("[ThermalBridge] 处理异常: %s", str(e))
            self._fail_count += 1
        finally:
            self._processing = False
            if tmp_path and os.path.exists(tmp_path):
                try:
                    os.unlink(tmp_path)
                except OSError:
                    pass

    def _save_temp_image(self, image_msg):
        cv_image = self._bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8")
        fd, path = tempfile.mkstemp(suffix=".jpg", prefix="thermal_fusion_")
        os.close(fd)
        cv2.imwrite(path, cv_image)
        return path

    def _invoke_exe(self, image_path):
        env = os.environ.copy()
        # 常见 OpenCV DLL 路径
        for p in [
            os.path.expanduser("~/.conda/envs/opencv-build/Library/bin"),
            os.path.expanduser("~/miniconda3/envs/opencv-build/Library/bin"),
        ]:
            if os.path.isdir(p):
                env["PATH"] = p + os.pathsep + env.get("PATH", "")
        try:
            result = subprocess.run(
                [self.exe_path, image_path, "--json", "--no-show"],
                capture_output=True, text=True, timeout=self.timeout,
                encoding="utf-8", errors="replace", env=env,
            )
            if result.returncode == 0:
                return result.stdout
            else:
                rospy.logwarn("[ThermalBridge] exe 返回 %d: %s", result.returncode, result.stderr[:200])
        except subprocess.TimeoutExpired:
            rospy.logwarn("[ThermalBridge] exe 超时 (%.1fs)", self.timeout)
        except FileNotFoundError:
            rospy.logerr("[ThermalBridge] exe 未找到: %s", self.exe_path)
        return None

    def _parse_json(self, stdout):
        start = stdout.find("{")
        if start < 0:
            return None
        try:
            text = stdout[start:]
            decoder = json.JSONDecoder()
            obj, _ = decoder.raw_decode(text)
            # 移除 file 字段 (中文路径可能含非法转义)
            obj.pop("file", None)
            return obj
        except json.JSONDecodeError:
            rospy.logwarn("[ThermalBridge] JSON 解析失败: %.100s", stdout[:100])
            return None

    def shutdown(self):
        self._executor.shutdown(wait=False)


# ══════════════════════════════════════════════════════════════
#  融合引擎
# ══════════════════════════════════════════════════════════════

class FusionEngine:
    # 权重配置
    WEIGHTS = {"thermal": 0.35, "flash": 0.30, "acoustic": 0.35}
    CROSS_BOOST_2 = 0.25
    CROSS_BOOST_3 = 0.15

    # 热成像类型 → 单独威胁等级
    THERMAL_SOLO_LEVEL = {
        "Person": ThreatLevel.WARNING,
        "Barrel Overheat": ThreatLevel.DANGER,
        "Cold Weapon": ThreatLevel.CAUTION,
        "Flash Residue": ThreatLevel.CAUTION,
        "Hotspot": ThreatLevel.CAUTION,
    }

    # 声源类型 → 单独威胁等级
    ACOUSTIC_SOLO_LEVEL = {
        "gunshot": ThreatLevel.WARNING,
        "artillery": ThreatLevel.DANGER,
        "explosion": ThreatLevel.DANGER,
        "ambient": ThreatLevel.SAFE,
    }

    def __init__(self, cache, params):
        self.cache = cache
        self.params = params
        self._threat_counter = 0

    def fuse(self, now, drone_heading, image_width=640):
        thermal_list = self.cache.get_thermal(now) if self.params.get("thermal_enabled") else []
        flash_item = self.cache.get_flash(now) if self.params.get("flash_enabled") else None
        acoustic_list = self.cache.get_acoustic(now) if self.params.get("acoustic_enabled") else []

        threats = []

        # 将各模态检测转为统一格式: (source, azimuth, confidence, level, details)
        unified = []

        for entry in thermal_list:
            for det in entry["data"].get("detections", []):
                azimuth = self._bbox_to_azimuth(det.get("bbox", {}), image_width, drone_heading)
                conf = det.get("confidence", 0.5)
                det_type = det.get("type", "Hotspot")
                level = self.THERMAL_SOLO_LEVEL.get(det_type, ThreatLevel.CAUTION)
                unified.append(("thermal", azimuth, conf, level, det))

        if flash_item:
            azimuth = self._sight_to_azimuth(flash_item["sight_x"], drone_heading)
            area = flash_item.get("area", 0)
            conf = min(1.0, area / 400.0) if area > 0 else 0.3
            level = ThreatLevel.WARNING if area > 200 else ThreatLevel.CAUTION
            unified.append(("flash", azimuth, conf, level, flash_item))

        for entry in acoustic_list:
            t = entry["data"]
            azimuth = t.get("azimuth", 0.0)
            conf = t.get("confidence", 0.0)
            sound_type = t.get("sound_type", "ambient")
            level = self.ACOUSTIC_SOLO_LEVEL.get(sound_type, ThreatLevel.SAFE)
            if conf >= self.params.get("acoustic_confidence_threshold", 0.6):
                unified.append(("acoustic", azimuth, conf, level, t))

        if not unified:
            return threats

        # 空间关联分组
        groups = self._spatial_group(unified)

        for group in groups:
            threat = self._build_threat(group, now)
            if threat and threat.confidence >= self.params.get("fusion_confidence_threshold", 0.5):
                threats.append(threat)

        # 保守策略: 单模态高置信度告警
        for source, azimuth, conf, level, details in unified:
            if conf >= 0.8 and level >= ThreatLevel.WARNING:
                already = any(
                    abs(_angle_diff(t.bearing, azimuth)) < 20
                    for t in threats
                )
                if not already:
                    self._threat_counter += 1
                    threats.append(FusedThreat(
                        threat_id="FT-{:04d}".format(self._threat_counter),
                        level=level,
                        confidence=conf,
                        bearing=azimuth,
                        elevation=0.0,
                        distance=-1.0,
                        sources=[source],
                        description="{} high-confidence alert".format(source),
                        timestamp=now,
                        details={source: details},
                    ))

        return threats

    def _bbox_to_azimuth(self, bbox, image_width, drone_heading):
        cx = bbox.get("x", 0) + bbox.get("w", 0) / 2.0
        hfov = self.params.get("thermal_hfov", 32.0)
        norm = (cx / max(image_width, 1) - 0.5) * hfov
        return (drone_heading + norm) % 360

    def _sight_to_azimuth(self, sight_x, drone_heading):
        return (drone_heading + math.degrees(sight_x)) % 360

    def _spatial_group(self, unified, threshold=15.0):
        if not unified:
            return []
        groups = []
        assigned = [False] * len(unified)
        for i in range(len(unified)):
            if assigned[i]:
                continue
            group = [unified[i]]
            assigned[i] = True
            for j in range(i + 1, len(unified)):
                if assigned[j]:
                    continue
                if unified[i][0] == unified[j][0]:
                    continue  # 同一模态不合并
                diff = abs(_angle_diff(unified[i][1], unified[j][1]))
                if diff < threshold:
                    group.append(unified[j])
                    assigned[j] = True
            groups.append(group)
        # 未分配的也各自成组
        for i in range(len(unified)):
            if not assigned[i]:
                groups.append([unified[i]])
        return groups

    def _build_threat(self, group, now):
        if not group:
            return None
        self._threat_counter += 1
        sources = list(set(g[0] for g in group))

        # 加权置信度
        total_w = 0.0
        total_wc = 0.0
        for src, az, conf, level, det in group:
            w = self.WEIGHTS.get(src, 0.3)
            total_w += w
            total_wc += w * conf
        fused_conf = total_wc / max(total_w, 0.01)

        # 跨模态奖励
        if len(sources) >= 2:
            fused_conf = min(1.0, fused_conf + self.CROSS_BOOST_2)
        if len(sources) >= 3:
            fused_conf = min(1.0, fused_conf + self.CROSS_BOOST_3)

        # 方位角: 加权圆均值
        bearings_weights = [(g[1], self.WEIGHTS.get(g[0], 0.3) * max(g[2], 0.1)) for g in group]
        fused_bearing = _circular_weighted_mean(bearings_weights)

        # 距离: 取已知距离中最小的
        distances = [g[4].get("distance", -1) for g in group if isinstance(g[4], dict)]
        fused_distance = min((d for d in distances if d > 0), default=-1.0)

        # 仰角: 取声源的 elevation (如果有)
        elevations = [g[4].get("elevation", 0.0) for g in group
                      if g[0] == "acoustic" and isinstance(g[4], dict)]
        fused_elevation = elevations[0] if elevations else 0.0

        # 威胁等级: 基于组合规则
        fused_level = self._escalate(group)

        # 描述
        descriptions = []
        for src, az, conf, level, det in group:
            if src == "thermal" and isinstance(det, dict):
                descriptions.append("{}({:.0f}%)".format(det.get("type", "?"), conf * 100))
            elif src == "flash":
                descriptions.append("Flash({:.0f}px)".format(det.get("area", 0)))
            elif src == "acoustic" and isinstance(det, dict):
                descriptions.append("{}({:.0f}%)".format(det.get("sound_type", "?"), conf * 100))
        desc = " + ".join(descriptions) if descriptions else "Unknown"

        return FusedThreat(
            threat_id="FT-{:04d}".format(self._threat_counter),
            level=fused_level,
            confidence=fused_conf,
            bearing=fused_bearing,
            elevation=fused_elevation,
            distance=fused_distance,
            sources=sources,
            description=desc,
            timestamp=now,
            details={g[0]: g[4] for g in group if isinstance(g[4], dict)},
        )

    def _escalate(self, group):
        sources_map = {}
        for src, az, conf, level, det in group:
            sources_map.setdefault(src, []).append((det, conf, level))

        has_thermal_type = set()
        has_acoustic_type = set()
        has_flash = "flash" in sources_map
        max_level = ThreatLevel.SAFE

        for det, conf, level in sources_map.get("thermal", []):
            if isinstance(det, dict):
                has_thermal_type.add(det.get("type", ""))
            max_level = max(max_level, level)

        for det, conf, level in sources_map.get("acoustic", []):
            if isinstance(det, dict):
                has_acoustic_type.add(det.get("sound_type", ""))
            max_level = max(max_level, level)

        if has_flash:
            max_level = max(max_level, ThreatLevel.CAUTION)

        # 跨模态组合升级 (沿用 thermal_types.h Assessment::level() 逻辑)
        has_person = "Person" in has_thermal_type
        has_barrel = "Barrel Overheat" in has_thermal_type
        has_weapon = "Cold Weapon" in has_thermal_type
        has_gunshot = "gunshot" in has_acoustic_type
        has_explosion = any(t in has_acoustic_type for t in ("artillery", "explosion"))

        if has_person and has_barrel:
            return ThreatLevel.CRITICAL
        if has_person and has_weapon:
            return ThreatLevel.CRITICAL
        if has_person and has_gunshot:
            return ThreatLevel.CRITICAL
        if has_barrel and has_gunshot:
            return ThreatLevel.CRITICAL
        if has_weapon and has_gunshot:
            return ThreatLevel.CRITICAL
        if has_person and has_flash:
            return ThreatLevel.DANGER
        if has_gunshot and has_flash:
            return ThreatLevel.DANGER
        if has_explosion:
            return ThreatLevel.DANGER
        if has_person:
            return ThreatLevel.WARNING
        if has_barrel:
            return ThreatLevel.DANGER
        if has_weapon:
            return ThreatLevel.CAUTION
        if has_gunshot:
            return ThreatLevel.WARNING

        return max_level


# ══════════════════════════════════════════════════════════════
#  避障控制器
# ══════════════════════════════════════════════════════════════

class AvoidanceController:
    def __init__(self, params):
        self.params = params
        self._smooth_vx = 0.0
        self._smooth_vy = 0.0
        self._smooth_vz = 0.0
        self._last_avoidance_time = 0.0
        self._active = False

    @property
    def active(self):
        return self._active

    def compute(self, threats, now, drone_heading=0.0):
        if not self.params.get("avoidance_enabled", True):
            return None

        dangerous = [t for t in threats if t.level >= ThreatLevel.WARNING]
        if not dangerous:
            # 衰减
            if self._active:
                self._smooth_vx *= 0.8
                self._smooth_vy *= 0.8
                self._smooth_vz *= 0.8
                if abs(self._smooth_vx) < 0.01 and abs(self._smooth_vy) < 0.01:
                    self._active = False
                    return None
                return self._make_twist(self._smooth_vx, self._smooth_vy, self._smooth_vz, now)
            return None

        # 冷却检查
        cooldown = self.params.get("avoidance_cooldown", 5.0)
        if self._active and (now - self._last_avoidance_time) < cooldown:
            # 冷却中，衰减
            hold_time = self.params.get("avoidance_hold_time", 2.0)
            elapsed = now - self._last_avoidance_time
            if elapsed < hold_time:
                return self._make_twist(self._smooth_vx, self._smooth_vy, self._smooth_vz, now)
            decay = max(0, 1.0 - (elapsed - hold_time) / (cooldown - hold_time))
            return self._make_twist(
                self._smooth_vx * decay,
                self._smooth_vy * decay,
                self._smooth_vz * decay,
                now,
            )

        # 计算斥力向量
        speed = self.params.get("avoidance_speed", 2.0)
        decel_dist = self.params.get("avoidance_decel_distance", 10.0)
        vectors = []
        for t in dangerous:
            vx, vy, vz = self._repulsion(t, speed, decel_dist)
            vectors.append((vx, vy, vz, t))

        # 合成
        cx, cy, cz = self._combine(vectors)

        # 平滑
        alpha = self.params.get("avoidance_smoothing", 0.3)
        self._smooth_vx = alpha * cx + (1 - alpha) * self._smooth_vx
        self._smooth_vy = alpha * cy + (1 - alpha) * self._smooth_vy
        self._smooth_vz = alpha * cz + (1 - alpha) * self._smooth_vz

        self._active = True
        self._last_avoidance_time = now
        return self._make_twist(self._smooth_vx, self._smooth_vy, self._smooth_vz, now)

    def _repulsion(self, threat, speed, decel_dist):
        # 方位角 → ENU 反方向
        az_rad = math.radians(threat.bearing)
        avoid_x = -math.sin(az_rad)  # ENU: x=East
        avoid_y = -math.cos(az_rad)  # ENU: y=North

        # 距离缩放
        d = threat.distance
        if d > 0 and d < decel_dist:
            scale = ((decel_dist - d) / decel_dist) ** 2
        elif d > 0:
            scale = 0.0  # 远处不避障
        else:
            scale = 1.0  # 未知距离，全力避障

        mag = math.sqrt(avoid_x ** 2 + avoid_y ** 2)
        if mag > 0:
            avoid_x = avoid_x / mag * speed * scale
            avoid_y = avoid_y / mag * speed * scale

        # 垂直避障
        vz = 0.0
        if self.params.get("avoidance_vertical_enabled", True) and abs(threat.elevation) > 10:
            vz = -math.sin(math.radians(threat.elevation)) * speed * 0.5 * scale

        return avoid_x, avoid_y, vz

    def _combine(self, vectors):
        cx, cy, cz = 0.0, 0.0, 0.0
        speed = self.params.get("avoidance_speed", 2.0)
        for vx, vy, vz, threat in vectors:
            w = 1.0 / max(threat.distance, 1.0) if threat.distance > 0 else 2.0
            w *= threat.confidence
            cx += vx * w
            cy += vy * w
            cz += vz * w
        mag = math.sqrt(cx ** 2 + cy ** 2 + cz ** 2)
        if mag > speed:
            f = speed / mag
            cx *= f
            cy *= f
            cz *= f
        return cx, cy, cz

    def _make_twist(self, vx, vy, vz, now):
        t = TwistStamped()
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "map"
        t.twist.linear = Vector3(vx, vy, vz)
        t.twist.angular = Vector3(0, 0, 0)
        return t

    def reset(self):
        self._smooth_vx = 0.0
        self._smooth_vy = 0.0
        self._smooth_vz = 0.0
        self._active = False


# ══════════════════════════════════════════════════════════════
#  工具函数
# ══════════════════════════════════════════════════════════════

def _angle_diff(a, b):
    d = (a - b) % 360
    if d > 180:
        d -= 360
    return d


def _circular_weighted_mean(azimuths_weights):
    sin_sum = sum(w * math.sin(math.radians(a)) for a, w in azimuths_weights)
    cos_sum = sum(w * math.cos(math.radians(a)) for a, w in azimuths_weights)
    return math.degrees(math.atan2(sin_sum, cos_sum)) % 360


# ══════════════════════════════════════════════════════════════
#  主 ROS 节点
# ══════════════════════════════════════════════════════════════

class ThreatFusionNode:
    def __init__(self):
        rospy.init_node("threat_fusion_node", anonymous=False)

        # 参数
        self.params = self._load_params()

        # 状态
        self._drone_heading = 0.0
        self._current_level = ThreatLevel.SAFE
        self._current_threats = []
        self._avoidance_active = False
        self._modality_status = {"thermal": "DISABLED", "flash": "DISABLED", "acoustic": "DISABLED"}
        self._published_ids = set()

        # 核心组件
        self.cache = ThreatCache(self.params.get("fusion_time_window", 0.5))
        self.thermal_bridge = ThermalBridge(
            self.params.get("thermal_exe_path", ""),
            max_rate=2.0,
        )
        self._modality_status["thermal"] = "OK" if self.thermal_bridge.enabled else "DISABLED"

        self.fusion_engine = FusionEngine(self.cache, self.params)
        self.avoidance_ctrl = AvoidanceController(self.params)

        # 订阅者
        self._setup_subscribers()
        # 发布者
        self._setup_publishers()
        # 服务
        self._setup_services()
        # 定时器
        self._setup_timers()

        rospy.loginfo("[ThreatFusion] 节点已启动")
        rospy.loginfo("[ThreatFusion] 热成像: %s | 闪光: %s | 声源: %s",
                       self._modality_status["thermal"],
                       self._modality_status["flash"],
                       self._modality_status["acoustic"])

    def _load_params(self):
        p = {}
        p["thermal_exe_path"] = rospy.get_param("~thermal_exe_path", "")
        p["thermal_image_topic"] = rospy.get_param("~thermal_image_topic", "/camera/thermal/image_raw")
        p["thermal_enabled"] = rospy.get_param("~thermal_enabled", True)
        p["flash_enabled"] = rospy.get_param("~flash_enabled", True)
        p["acoustic_enabled"] = rospy.get_param("~acoustic_enabled", True)
        p["acoustic_confidence_threshold"] = rospy.get_param("~acoustic_confidence_threshold", 0.6)
        p["fusion_time_window"] = rospy.get_param("~fusion_time_window", 0.5)
        p["fusion_confidence_threshold"] = rospy.get_param("~fusion_confidence_threshold", 0.5)
        p["avoidance_enabled"] = rospy.get_param("~avoidance_enabled", True)
        p["avoidance_speed"] = rospy.get_param("~avoidance_speed", 2.0)
        p["avoidance_decel_distance"] = rospy.get_param("~avoidance_decel_distance", 10.0)
        p["avoidance_cooldown"] = rospy.get_param("~avoidance_cooldown", 5.0)
        p["avoidance_hold_time"] = rospy.get_param("~avoidance_hold_time", 2.0)
        p["avoidance_smoothing"] = rospy.get_param("~avoidance_smoothing", 0.3)
        p["avoidance_vertical_enabled"] = rospy.get_param("~avoidance_vertical_enabled", True)
        p["uav_namespace"] = rospy.get_param("~uav_namespace", "/uav1")
        p["thermal_hfov"] = rospy.get_param("~thermal_hfov", 32.0)
        return p

    # ── 订阅 ──

    def _setup_subscribers(self):
        ns = self.params["uav_namespace"]

        # 热成像图像
        if self.thermal_bridge.enabled:
            rospy.Subscriber(
                self.params["thermal_image_topic"], Image,
                self._thermal_image_cb, queue_size=1,
            )

        # 闪光检测
        if self.params["flash_enabled"]:
            rospy.Subscriber("/flash_detector/flash_detected", Bool,
                             self._flash_detected_cb, queue_size=5)
            rospy.Subscriber("/flash_detector/detection", Float32MultiArray,
                             self._flash_detection_cb, queue_size=5)
            self._modality_status["flash"] = "OK"

        # 声源检测
        if self.params["acoustic_enabled"]:
            if HAS_ACOUSTIC_MSG:
                rospy.Subscriber("/acoustic/threats", AcousticThreatArray,
                                 self._acoustic_threats_cb, queue_size=5)
                self._modality_status["acoustic"] = "OK"
            else:
                rospy.logwarn("[ThreatFusion] acoustic_analyzer 消息包不可用, 声源模态禁用")
                self.params["acoustic_enabled"] = False

        # 无人机位姿 (获取航向角)
        rospy.Subscriber(ns + "/mavros/local_position/pose", PoseStamped,
                         self._pose_cb, queue_size=5)

    def _thermal_image_cb(self, msg):
        self.thermal_bridge.process_image(msg, self._on_thermal_result)

    def _on_thermal_result(self, result):
        now = time.time()
        self.cache.add_thermal(result, now)

    def _flash_detected_cb(self, msg):
        pass  # 状态由 detection 回调处理

    def _flash_detection_cb(self, msg):
        data = msg.data
        if len(data) >= 5:
            now = time.time()
            self.cache.add_flash(data[0], data[1], data[2], data[3], data[4], now)

    def _acoustic_threats_cb(self, msg):
        now = time.time()
        threats = []
        for t in msg.threats:
            threats.append({
                "threat_id": t.threat_id,
                "sound_type": t.sound_type,
                "confidence": t.confidence,
                "azimuth": t.azimuth,
                "elevation": t.elevation,
                "distance": t.distance,
                "distance_confidence": t.distance_confidence,
            })
        self.cache.add_acoustic(threats, now)

    def _pose_cb(self, msg):
        q = msg.pose.orientation
        # 四元数 → 偏航角 (ENU)
        siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        self._drone_heading = math.degrees(math.atan2(siny_cosp, cosy_cosp)) % 360

    # ── 发布 ──

    def _setup_publishers(self):
        self._status_pub = rospy.Publisher("/threat_fusion/status", String, queue_size=5)
        self._threat_pub = rospy.Publisher("/threat_fusion/fused_threats", String, queue_size=10)
        self._avoid_pub = rospy.Publisher("/threat_fusion/avoidance_cmd", TwistStamped, queue_size=5)
        self._diag_pub = rospy.Publisher("/threat_fusion/diagnostics", DiagnosticArray, queue_size=5)

        ns = self.params["uav_namespace"]
        self._mavros_vel_pub = rospy.Publisher(
            ns + "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=5,
        )

    # ── 服务 ──

    def _setup_services(self):
        rospy.Service("/threat_fusion/reset", std_msgs.srv.Trigger, self._reset_cb)

    def _reset_cb(self, req):
        self.cache.clear()
        self.avoidance_ctrl.reset()
        self._published_ids.clear()
        self._current_level = ThreatLevel.SAFE
        self._current_threats = []
        return std_msgs.srv.TriggerResponse(success=True, message="Fusion state reset")

    # ── 定时器 ──

    def _setup_timers(self):
        # 10Hz 融合
        rospy.Timer(rospy.Duration(0.1), self._fusion_timer_cb)
        # 2Hz 状态
        rospy.Timer(rospy.Duration(0.5), self._status_timer_cb)
        # 1Hz 诊断
        rospy.Timer(rospy.Duration(1.0), self._diag_timer_cb)

    def _fusion_timer_cb(self, event):
        now = time.time()
        threats = self.fusion_engine.fuse(now, self._drone_heading)
        self._current_threats = threats
        self._current_level = max((t.level for t in threats), default=ThreatLevel.SAFE)

        # 发布新威胁事件
        for t in threats:
            if t.threat_id not in self._published_ids:
                self._publish_fused_threat(t)
                self._published_ids.add(t.threat_id)

        # 清理旧 ID (保留最近 100 个)
        if len(self._published_ids) > 100:
            self._published_ids = set(list(self._published_ids)[-50:])

        # 避障
        avoidance_cmd = self.avoidance_ctrl.compute(threats, now, self._drone_heading)
        self._avoidance_active = avoidance_cmd is not None
        if avoidance_cmd:
            self._avoid_pub.publish(avoidance_cmd)
            self._mavros_vel_pub.publish(avoidance_cmd)

    def _status_timer_cb(self, event):
        status = {
            "threat_level": self._current_level.label,
            "threat_count": len(self._current_threats),
            "avoidance_active": self._avoidance_active,
            "drone_heading": round(self._drone_heading, 1),
            "modalities": dict(self._modality_status),
            "timestamp": time.time(),
        }
        self._status_pub.publish(String(json.dumps(status, ensure_ascii=False)))

    def _diag_timer_cb(self, event):
        diag = DiagnosticArray()
        diag.header.stamp = rospy.Time.now()
        ds = DiagnosticStatus()
        ds.name = "threat_fusion"
        ds.level = DiagnosticStatus.OK if self._current_level < ThreatLevel.DANGER else DiagnosticStatus.WARN
        ds.message = "Level: {}, Threats: {}".format(self._current_level.label, len(self._current_threats))
        ds.values = [
            KeyValue("thermal", self._modality_status["thermal"]),
            KeyValue("flash", self._modality_status["flash"]),
            KeyValue("acoustic", self._modality_status["acoustic"]),
            KeyValue("avoidance_active", str(self._avoidance_active)),
        ]
        diag.status.append(ds)
        self._diag_pub.publish(diag)

    # ── 发布辅助 ──

    def _publish_fused_threat(self, threat):
        data = {
            "id": threat.threat_id,
            "level": threat.level.label,
            "confidence": round(threat.confidence, 3),
            "bearing": round(threat.bearing, 1),
            "elevation": round(threat.elevation, 1),
            "distance": round(threat.distance, 1),
            "sources": threat.sources,
            "description": threat.description,
            "timestamp": threat.timestamp,
        }
        self._threat_pub.publish(String(json.dumps(data, ensure_ascii=False)))

    def run(self):
        rospy.spin()
        self.thermal_bridge.shutdown()


# ══════════════════════════════════════════════════════════════
#  入口
# ══════════════════════════════════════════════════════════════

if __name__ == "__main__":
    try:
        node = ThreatFusionNode()
        node.run()
    except rospy.ROSInterruptException:
        pass
