"""
mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机

【协议变更 v2 — 2026-07】
  航点存储格式使用经纬度 (lat, lon, alt)。导航/返航/悬停均发送
  VELOCITY SETPOINT：把目标经纬度换算成相对当前位置的 NED 方向，
  再按 max_speed 限速成速度命令（差分量，与参考原点无关）。

【坐标系说明 — 2026-08 修复】
  此前 POSITION LOCAL_OFFSET_NED 路径用 GPS 经纬度反算相对偏移，
  交给 PX4 在自身 LOCAL_NED 帧解析。GPS 与本地估计一旦不一致
  （跳变/漂移），目标跟着跳、飞控满速追（实测 HOLD_FINAL 7.77m/s）。
  故 HOLD_TASK / HOLD_FINAL 悬停改为速度闭环，从根上消除
  GPS↔本地帧混用。POSITION setpoint 仅保留给 TAKEOFF 预热/降级爬升
  （零水平偏移，且已加单帧钳制）。

【核心改动】
  1. self.drone → self.proxy（注入的 RouterProxy 实例）
  2. 所有状态读取：proxy.get_latest_state()["drone"][...]
  3. 所有指令发送：proxy.send_command() / send_waypoint() / send_setpoint()
  4. 新增 STATE 新鲜度校验（协议 §3.8.1），过期数据保守处理
  5. 新增 Failsafe 模式断流保护，退出 OFFBOARD 即熔断
  6. VISUAL_TRACKING 状态下完全让渡控制权，不发送任何 SETPOINT

【位置判定】
  航点存储为经纬高 (lat, lon, alt)，使用 Haversine 公式计算
  球面距离进行到达判定，不再依赖速度积分推算 NED 位置。

【控制权协调】
  MissionManager 通过自有 proxy 发送指令。连续 20Hz SETPOINT
  流由 UAVControlLoop 的独立线程负责。run_mission.py 负责协调：
  在 VISUAL_TRACKING 外停止 UAVControlLoop 线程，避免两套
  SETPOINT 流同时激活产生冲突。
"""

import math
import time
import logging
from enum import Enum

from utils.coord import haversine, dist_3d_latlon, latlon_alt_to_local_offset

logger = logging.getLogger(__name__)


class MissionState(Enum):
    """无人机任务状态枚举"""
    WAITING_WAYPOINTS = -1  # 等待地面站下发航点
    INIT = 0                # 初始化 / 等待连接
    ARMING = 1              # 解锁中
    TAKEOFF = 2             # 自动起飞中
    NAVIGATING = 3          # 航点巡航导航
    HOLD_TASK = 4           # 到达航点后的原地搜索 / 悬停
    VISUAL_TRACKING = 5     # 🎯 视觉追踪中（控制权让渡给 UAVControlLoop）
    RETURNING = 6           # 返航中
    LANDING = 7             # 自动降落中
    FINISHED = 8            # 任务结束 / 已上锁
    HOLD_FINAL = 9          # 结束巡点后原地悬停（return_to_home=False）
    PRE_LAND_HOVER = 10     # 🛬 降落前悬停（到达降落点后等待片刻再下降）


class FailsafeTriggered(Exception):
    """
    自定义安全异常：当安全员人工介入切出自控模式时抛出，
    用于暴力熔断主循环。
    """
    pass


# ================================================================
#  航点巡航管理器
# ================================================================

class MissionManager:
    """
    🛸 ZMQ 航点巡航状态机管理器（经纬度航点 + 相对位置 SETPOINT）

    职责：
      - 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 →
        航点巡航 → 视觉追踪 → 返航 → 降落）
      - 所有底层通信通过注入的 RouterProxy 完成
      - 在 VISUAL_TRACKING 状态下让渡控制权给 UAVControlLoop
      - Failsafe 熔断保护

    航点格式（经纬度协议 v2）：
        {
            "lat": 37.7749,        # 纬度（度）
            "lon": -122.4194,      # 经度（度）
            "alt": 1.5,            # 相对起飞点高度（米，正值向上）
            "yaw": 0.0,            # 偏航角（度）
            "command": "WAYPOINT", # TAKEOFF / LAND / WAYPOINT
            "hold_duration": 8.0,  # 悬停搜索等待时长（秒）
        }

    使用示例：
        proxy = RouterProxy()
        proxy.start()
        manager = MissionManager(proxy, waypoints=[...], target_altitude=1.5)

        while True:
            manager.update(target_detected=some_bool)
            time.sleep(0.05)
    """

    # ---- Failsafe：自主飞行的合法模式白名单 ----
    AUTONOMOUS_MODES = frozenset({
        "OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL",
    })

    # ---- 解锁/起飞阶段允许的过渡模式 ----
    TRANSITION_MODES = frozenset({
        "MANUAL", "ALTCTL", "POSCTL", "AUTO", "HOLD",
        "AUTO.READY", "AUTO.TAKEOFF", "AUTO.LOITER",
    })

    # ---- 起飞完成后可接受的悬停/保持模式 ----
    TAKEOFF_HOLD_MODES = frozenset({
        "HOLD", "AUTO", "AUTO.LOITER", "AUTO.TAKEOFF",
    })

    # ---- 目标丢失超时（秒） ----
    DEFAULT_TARGET_LOST_TIMEOUT = 3.0

    # ---- 各阶段超时 ----
    TAKEOFF_TIMEOUT = 15.0
    OFFBOARD_TIMEOUT = 5.0
    ARM_TIMEOUT = 5.0

    # ================================================================
    #  状态切换日志（property 拦截所有 self.state = X 赋值）
    # ================================================================

    @property
    def state(self):
        return self._state

    @state.setter
    def state(self, new_state):
        old_state = getattr(self, '_state', None)
        if old_state is not new_state:
            logger.info("🔄 [状态切换] %s → %s",
                         old_state.name if old_state is not None else "∅",
                         new_state.name)
        self._state = new_state

    # ================================================================

    def __init__(self, proxy, waypoints, target_altitude=1.5,
                 arrival_radius=0.3, hold_duration=5.0,
                 return_to_home=True, target_lost_timeout=None,
                 max_speed=1.2, pre_land_hover_duration=3.0,
                 landing_descent_speed=0.3):
        """
        Args:
            proxy:                   RouterProxy 实例（调用方负责 start/close）
            waypoints:               航点列表（经纬度格式 v2）
            target_altitude:         默认起飞高度（米，正值向上）
            arrival_radius:          航点到达判定半径（米）
            hold_duration:           巡点无目标的最大悬停搜索时间（秒）
            return_to_home:          航点巡完后是否返航，False 则终点悬停
            target_lost_timeout:     VISUAL_TRACKING 目标丢失超时（秒）
            max_speed:               航点巡航最大速度（m/s，默认 1.2）
            pre_land_hover_duration: 降落前悬停时长（秒，默认 3.0）
            landing_descent_speed:   降落下降速度（m/s，正值向下，默认 0.3）
        """
        # ---- 依赖注入 ----
        self.proxy = proxy

        # ---- 状态初始值（property 用 _state，须在任何 self.state = 前初始化） ----
        self._state = None

        # ---- 任务参数 ----
        # 航点列表（经纬度协议 v2：[{lat, lon, alt, yaw, command}, ...]）
        self.waypoints = list(waypoints) if waypoints else []
        # 根据是否有航点决定初始状态
        if self.waypoints:
            self.state = MissionState.INIT
            self._waypoints_ready = True
            logger.info("MissionManager 初始化完成，已有 %d 个航点", len(self.waypoints))
        else:
            self.state = MissionState.WAITING_WAYPOINTS
            self._waypoints_ready = False
            logger.info("MissionManager 初始化完成，等待 QGC 下发航点...")
        # v2: 目标高度正值向上（原 NED 负值协议已废除）
        self.target_altitude = abs(target_altitude)
        self.arrival_radius = arrival_radius
        self.hold_duration = hold_duration
        self.return_to_home = return_to_home
        self.target_lost_timeout = (
            target_lost_timeout if target_lost_timeout is not None
            else self.DEFAULT_TARGET_LOST_TIMEOUT
        )
        self.max_speed = max(max_speed, 0.1)   # 最大巡航速度（m/s），最低 0.1
        self.pre_land_hover_duration = max(pre_land_hover_duration, 0.0)  # 降落前悬停时长
        self.landing_descent_speed = max(0.1, min(landing_descent_speed, 1.0))  # 下降速度 [0.1, 1.0]

        # ---- 状态机 ----
        self.current_wp_index = 0

        # ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ----
        self.current_target = {
            "lat": 0.0, "lon": 0.0,
            "alt": self.target_altitude, "yaw": 0.0,
        }

        # ---- 计时器 ----
        self.hold_start_time = 0.0
        self.tracking_start_time = 0.0
        self._target_lost_start = None
        self._state_start_time = time.time()

        # ================================================================
        #  追踪完成信号（外部设置）
        # ================================================================
        self.tracking_completed = False
        self.tracking_max_duration = 10.0

        logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, "
                     "到达半径=%.1fm, 搜寻时长=%.1fs, 最大速度=%.1fm/s, "
                     "降落悬停=%.1fs, 下降速度=%.1fm/s, 返航=%s",
                     len(self.waypoints), self.target_altitude,
                     self.arrival_radius, self.hold_duration,
                     self.max_speed, self.pre_land_hover_duration,
                     self.landing_descent_speed, self.return_to_home)

        # ---- 日志节流控制 ----
        self._last_throttle_time = {}
        self._last_throttle_state = {}

    # ================================================================
    #  主入口
    # ================================================================

    def update(self, target_detected=False):
        """
        状态机高频核心业务逻辑，由外部主循环驱动（建议 ≥20Hz）

        Args:
            target_detected: 当前帧视觉检测是否有目标

        Raises:
            FailsafeTriggered: 检测到非法模式切换时抛出
        """
        # ---------------------------------------------------------------
        #  步骤 0：获取最新 STATE + 新鲜度校验 + Failsafe
        # ---------------------------------------------------------------
        state = self.proxy.get_latest_state()

        # ---- 新鲜度校验（协议 §3.8.1） ----
        if self.state not in (MissionState.INIT, MissionState.FINISHED):
            if state is not None:
                if not self.proxy.is_state_fresh(state):
                    # 过期警告已由 is_state_fresh 内部输出，此处不重复打印
                    return
            else:
                logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理")
                return

        # ---- 特殊状态，等待航点 ----
        if self.state == MissionState.WAITING_WAYPOINTS:
            if self._waypoints_ready:
                logger.info("✅ 航点已就绪，转入 INIT 状态")
                self.state = MissionState.INIT
                self._state_start_time = time.time()
            else:
                return

        # ---- Failsafe 越权检查 ----
        if self.state not in (MissionState.INIT, MissionState.FINISHED):
            if state is not None:
                mode = state["drone"].get("mode", "")
                # 飞控切到 AUTO.LAND 或 AUTO.RTL → 自动让渡控制权
                if mode in ("AUTO.LAND", "AUTO.RTL"):
                    # 去重：同一 mode 只在首次出现时打印，避免每帧刷屏
                    self._throttle_log_on_change(
                        "yield_mode", mode,
                        "⚠️ 飞控切换到 %s，Python 端主动让渡控制权", mode,
                        level=logging.WARNING)
                    # 根据模式决定切换到的状态
                    if mode == "AUTO.LAND":
                        self.state = MissionState.LANDING
                    elif mode == "AUTO.RTL":
                        self.state = MissionState.RETURNING
                    # 重置状态开始时间，让降落/返航处理从当前帧开始
                    self._state_start_time = time.time()
                    # 停止控制线程（通过 run_mission.py 的协调逻辑）
                    # 注意：state 变化后，下一帧 _coordinate_control 会自动停止线程
                    return
                if not self._is_mode_allowed_for_state(mode, self.state):
                    logger.warning(
                        "\n⚠️ [Failsafe] 飞控模式被切为: %s！"
                        "人工接管，熔断任务！", mode
                    )
                    self.state = MissionState.FINISHED
                    raise FailsafeTriggered(
                        f"安全员接管，当前模式: {mode}"
                    )

        # ---- 无 STATE 时仅 INIT 可容忍 ----
        if state is None:
            if self.state == MissionState.INIT:
                logger.info("[INIT] 等待 Router 状态推送...")
            return

        # ---------------------------------------------------------------
        #  状态机分支（位置估计已移除 v2 — 使用经纬度距离计算）
        # ---------------------------------------------------------------
        try:
            if self.state == MissionState.INIT:
                self._handle_init(state)
            elif self.state == MissionState.ARMING:
                self._handle_arming(state)
            elif self.state == MissionState.TAKEOFF:
                self._handle_takeoff(state)
            elif self.state == MissionState.NAVIGATING:
                self._handle_navigating(state)
            elif self.state == MissionState.HOLD_TASK:
                self._handle_hold_task(state, target_detected)
            elif self.state == MissionState.VISUAL_TRACKING:
                self._handle_visual_tracking(state, target_detected)
            elif self.state == MissionState.RETURNING:
                self._handle_returning(state)
            elif self.state == MissionState.LANDING:
                self._handle_landing(state)
            elif self.state == MissionState.HOLD_FINAL:
                self._handle_hold_final(state)
            elif self.state == MissionState.PRE_LAND_HOVER:
                self._handle_pre_land_hover(state)
        except FailsafeTriggered:
            raise
        except Exception as e:
            logger.error("状态机异常 (state=%s): %s", self.state.name, e,
                         exc_info=True)

    # ================================================================
    #  辅助方法
    # ================================================================

    def _is_autonomous_mode(self, mode):
        """判断是否属于自主飞行模式，兼容 PX4 AUTO 子模式。"""
        if mode in self.AUTONOMOUS_MODES:
            return True
        return isinstance(mode, str) and mode.startswith("AUTO.")

    def _is_transition_mode(self, mode):
        """解锁/起飞阶段允许的 PX4 过渡模式。"""
        if mode in self.TRANSITION_MODES:
            return True
        return isinstance(mode, str) and mode.startswith("AUTO.")

    def _is_mode_allowed_for_state(self, mode, state):
        """按任务阶段判断当前飞控模式是否允许。"""
        if state in (MissionState.ARMING, MissionState.TAKEOFF):
            return self._is_transition_mode(mode)
        return self._is_autonomous_mode(mode)

    def _is_takeoff_hold_mode(self, mode):
        """起飞高度到位后可接受的保持/悬停状态。"""
        return mode in self.TAKEOFF_HOLD_MODES

    def _get_current_alt(self, state):
        """从 STATE 获取当前相对高度（正值向上）"""
        try:
            return state["drone"].get("alt_rel", 0.0)
        except (KeyError, TypeError):
            return self.target_altitude

    # ---- 日志节流辅助 ----

    def _throttle_log(self, key, interval, msg, *args, level=logging.INFO):
        """节流日志：同一 key 在 interval 秒内只输出一次"""
        now = time.time()
        last = self._last_throttle_time.get(key, 0)
        if now - last >= interval:
            self._last_throttle_time[key] = now
            logger.log(level, msg, *args)

    def _throttle_log_on_change(self, key, current_value, msg, *args,
                                 level=logging.INFO):
        """状态变化日志：仅当 current_value 与上次记录不同时输出一次"""
        last = self._last_throttle_state.get(key)
        if last != current_value:
            self._last_throttle_state[key] = current_value
            logger.log(level, msg, *args)

    # ================================================================
    #  状态处理器
    # ================================================================

    # ---- 初始化 ----

    def _handle_init(self, state):
        """
        [INIT] 等待飞控连接，判断是否需要执行完整起飞序列

        三种分支：
          A) 已解锁 + OFFBOARD → 直接跳 NAVIGATING
          B) 未解锁 → 发 ARM 指令
          C) 已解锁但非 OFFBOARD → 进入 ARMING 等待后续 TAKEOFF 序列
        """
        try:
            drone = state["drone"]
            armed = drone.get("armed", False)
            mode = drone.get("mode", "")
            alt_rel = drone.get("alt_rel", 0.0)
            home = state.get("home", {})

            self._throttle_log("init_state", 5.0, "[INIT] mode=%s armed=%s alt_rel=%.1fm",
                               mode, armed, alt_rel)

            if not home.get("valid", False):
                self._throttle_log("home_valid_init", 5.0,
                                   "[INIT] home.valid=false，等待起飞点有效...")
                return

            # A) 已在 OFFBOARD + 已解锁 → 直接巡航
            if armed and mode == "OFFBOARD":
                logger.info("[INIT] 已在 OFFBOARD，直接进入航点巡航")
                self.state = MissionState.NAVIGATING
                self.current_wp_index = 0
                self._set_current_target_from_wp(0)
                self.proxy.reset_setpoint_log()
                return

            # B) 未解锁
            if not armed:
                logger.info("[INIT] 执行解锁...")
                ok, ack = self.proxy.send_command("ARM")
                if ok:
                    self.state = MissionState.ARMING
                    self._state_start_time = time.time()
                else:
                    logger.error("[INIT] ARM 失败: %s", ack)
                return

            # C) 已解锁但非 OFFBOARD
            if armed and mode != "OFFBOARD":
                logger.info("[INIT] 已解锁但 mode=%s，进入起飞序列", mode)
                self.state = MissionState.ARMING
                self._state_start_time = time.time()

        except (KeyError, TypeError) as e:
            logger.warning("[INIT] 解析异常: %s", e)

    # ---- 解锁 ----

    def _handle_arming(self, state):
        """
        [ARMING] 等待解锁确认，成功后进入 TAKEOFF
        """
        try:
            armed = state["drone"].get("armed", False)
            home = state.get("home", {})

            if not armed:
                elapsed = time.time() - self._state_start_time
                if elapsed > self.ARM_TIMEOUT:
                    logger.warning("[ARMING] 超时 (%.1fs)，重试 ARM", elapsed)
                    self.proxy.send_command("ARM")
                    self._state_start_time = time.time()
                else:
                    self._throttle_log("arming_wait", 3.0,
                                       "[ARMING] 等待解锁... (%.1fs)", elapsed)
                return

            if not home.get("valid", False):
                self._throttle_log("home_valid_arming", 5.0,
                                   "[ARMING] home.valid=false，等待起飞点有效...")
                return

            # ---- 解锁成功 → 起飞 ----
            logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", self.target_altitude)

            ok, ack = self.proxy.send_waypoint(
                lat=state["home"]["lat"],
                lon=state["home"]["lon"],
                alt=self.target_altitude,
                alt_frame="RELATIVE",
                speed=3.0, action="TAKEOFF",
                yaw=state["drone"]["yaw"],
                acceptance_radius=0.5,
                is_last=True
            )
            if ok:
                self.state = MissionState.TAKEOFF
                self._state_start_time = time.time()
            else:
                logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack)
                drone = state.get("drone", {})
                ok2, _ = self._send_relative_position_setpoint(
                    state,
                    drone.get("lat", 0.0), drone.get("lon", 0.0),
                    self.target_altitude, yaw=0.0,
                )
                if ok2:
                    self.state = MissionState.TAKEOFF
                    self._state_start_time = time.time()
                else:
                    logger.error("[TAKEOFF] 降级爬升也失败")

        except (KeyError, TypeError) as e:
            logger.warning("[ARMING] 异常: %s", e)

    # ---- 起飞 ----

    def _handle_takeoff(self, state):
        """
        [TAKEOFF] 等待 WAYPOINT(TAKEOFF) 完成，然后预热并切换 OFFBOARD

        流程（协议 §3.3.1）：
          ① 等待高度到达目标 90% 且 mode == "HOLD"
          ② 发送预热 SETPOINT（相对当前位置悬停）
          ③ time.sleep(0.2) 等待流建立
          ④ 发送 COMMAND("OFFBOARD")
          ⑤ 轮询等待 mode == "OFFBOARD"
          ⑥ 跳转 NAVIGATING
        """
        try:
            drone = state["drone"]
            home = state.get("home", {})
            alt_rel = drone.get("alt_rel", 0.0)
            mode = drone.get("mode", "")

            # ---- ① 等待 TAKEOFF 完成 ----
            reached_alt = alt_rel >= self.target_altitude * 0.9
            in_hold = self._is_takeoff_hold_mode(mode)

            if not (reached_alt and in_hold):
                self._throttle_log("takeoff_wait", 3.0,
                                   "[起飞中] alt=%.1f/%.1f mode=%s 耗时=%.1fs",
                                   alt_rel, self.target_altitude, mode,
                                   time.time() - self._state_start_time)
                return

            # ---- ② 高度到位 + HOLD 模式 → 预热 + 切 OFFBOARD ----
            logger.info("[TAKEOFF] 高度 %.1fm，模式 %s，准备切换 OFFBOARD...",
                        alt_rel, mode)

            # 发送预热 SETPOINT（相对当前位置悬停）
            self._send_relative_position_setpoint(
                state,
                drone.get("lat", 0.0), drone.get("lon", 0.0),
                alt_rel, yaw=0.0,
            )
            time.sleep(0.2)

            # 发送 OFFBOARD 指令
            ok, ack = self.proxy.send_command("OFFBOARD")
            if not ok:
                logger.error("[TAKEOFF] OFFBOARD 指令失败: %s", ack)
                if time.time() - self._state_start_time > self.TAKEOFF_TIMEOUT:
                    logger.warning("[TAKEOFF] 超时，强行进入巡航")
                    self.state = MissionState.NAVIGATING
                    self.current_wp_index = 0
                    self._set_current_target_from_wp(0)
                    self.proxy.reset_setpoint_log()
                return

            # 等待模式切换
            for _ in range(int(self.OFFBOARD_TIMEOUT / 0.1)):
                st = self.proxy.get_latest_state()
                if st and st["drone"]["mode"] == "OFFBOARD":
                    logger.info("[TAKEOFF] 已进入 OFFBOARD")
                    self.state = MissionState.NAVIGATING
                    self.current_wp_index = 0
                    self._set_current_target_from_wp(0)
                    self.proxy.reset_setpoint_log()
                    return
                time.sleep(0.1)

            logger.warning("[TAKEOFF] OFFBOARD 切换超时，强行进入巡航")
            self.state = MissionState.NAVIGATING
            self.current_wp_index = 0
            self._set_current_target_from_wp(0)
            self.proxy.reset_setpoint_log()

        except (KeyError, TypeError) as e:
            logger.warning("[TAKEOFF] 异常: %s", e)

    # ---- 航点导航 ----

    def _handle_navigating(self, state):
        """
        [NAVIGATING] 每帧发送 VELOCITY SETPOINT 前往当前航点

        速度受 max_speed 限制，含自动减速区。
        到达判定使用 Haversine 距离。
          - 普通航点到达 → HOLD_TASK
          - 降落航点到达 → PRE_LAND_HOVER（先悬停再降落）
        """
        if not self.waypoints or self.current_wp_index >= len(self.waypoints):
            self._finish_navigation(state)
            return

        wp = self.waypoints[self.current_wp_index]
        cmd = wp.get("command", "WAYPOINT")

        # ---- 起飞航点 直接跳过 ----
        if cmd == "TAKEOFF":
            self.current_wp_index += 1
            return

        # ---- 普通航点 / 降落航点 — 统一导航到目标坐标 ----
        wp_lat = wp["lat"]
        wp_lon = wp["lon"]
        wp_alt = wp.get("alt", self.target_altitude)
        wp_yaw = wp.get("yaw", 0.0)

        # 发送 VELOCITY SETPOINT，速度受 max_speed 限制
        self._send_velocity_to_target(state, wp_lat, wp_lon, wp_alt)

        # ---- 共享目标 ----
        self.current_target = {"lat": wp_lat, "lon": wp_lon,
                               "alt": wp_alt, "yaw": wp_yaw}

        # ---- 到达判定（Haversine 球面距离） ----
        drone = state["drone"]
        drone_lat = drone.get("lat", 0.0)
        drone_lon = drone.get("lon", 0.0)
        current_alt = self._get_current_alt(state)
        dist = dist_3d_latlon(drone_lat, drone_lon, current_alt,
                              wp_lat, wp_lon, wp_alt)

        if dist < self.arrival_radius:
            if cmd == "LAND":
                logger.info("\n🛬 [到达降落点] 悬停 %.1fs 后降落...",
                            self.pre_land_hover_duration)
                self.state = MissionState.PRE_LAND_HOVER
                self._state_start_time = time.time()
            else:
                logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm",
                            self.current_wp_index + 1, len(self.waypoints), dist)
                self.state = MissionState.HOLD_TASK
                self.hold_start_time = time.time()
        else:
            self._throttle_log("navigating", 5.0,
                               "[巡航中] 航点 [%d/%d] 距离=%.2fm 速度≤%.1fm/s",
                               self.current_wp_index + 1,
                               len(self.waypoints), dist,
                               self.max_speed)

    def _finish_navigation(self, state):
        """所有航点飞完后的跳转"""
        home = state.get("home", {}) if state else {}
        home_lat = home.get("lat", 0.0)
        home_lon = home.get("lon", 0.0)

        if self.return_to_home:
            logger.info("\n[巡航完成] 返回起始点 (lat=%.6f lon=%.6f)...",
                        home_lat, home_lon)
            self.state = MissionState.RETURNING
            self.current_target = {
                "lat": home_lat, "lon": home_lon,
                "alt": self.target_altitude, "yaw": 0,
            }
        else:
            logger.info("\n[巡航完成] 末端悬停...")
            self.state = MissionState.HOLD_FINAL
        self._state_start_time = time.time()

    # ---- 悬停搜索 ----

    def _handle_hold_task(self, state, target_detected):
        """
        [HOLD_TASK] 抵达航点后的悬停搜索

        策略：
          - 检测到目标 → VISUAL_TRACKING
          - 超时无目标 → 下一个航点
          - 持续发送 POSITION SETPOINT 悬停
        """
        # ---- 发现目标 ----
        if target_detected:
            logger.info("\n🎯 [目标锁定] 切入视觉追踪状态！")
            self.state = MissionState.VISUAL_TRACKING
            self.tracking_start_time = time.time()
            self._target_lost_start = None
            self.tracking_completed = False
            return

        # ---- 保持当前位置悬停（速度闭环，坐标系一致） ----
        # 说明：悬停不用 POSITION LOCAL_OFFSET_NED。位置模式把"由 GPS 经纬度
        # 反算的相对偏移"交给 PX4 在自身 LOCAL_NED 帧里解析，GPS 与本地估计
        # 一旦不一致（跳变/漂移），目标跟着跳、飞控满速追（曾实测 7.77m/s）。
        # 速度模式是差分量，与参考原点无关，命令值即速度上界，天然安全。
        wp = self.waypoints[self.current_wp_index]
        self._send_velocity_to_target(
            state, wp["lat"], wp["lon"], wp.get("alt", self.target_altitude),
        )
        self.current_target = {
            "lat": wp["lat"], "lon": wp["lon"],
            "alt": wp.get("alt", self.target_altitude),
            "yaw": wp.get("yaw", 0),
        }

        # ---- 超时判定 ----
        elapsed = time.time() - self.hold_start_time
        if elapsed >= self.hold_duration:
            logger.info("\n[未发现目标] 搜寻超时 (%.1fs)，前往下一航点",
                        self.hold_duration)
            self.current_wp_index += 1
            self.state = MissionState.NAVIGATING
            self.proxy.reset_setpoint_log()
        else:
            self._throttle_log("hold_task", 3.0,
                               "[检索中] 航点 %d 剩余 %.1fs",
                               self.current_wp_index + 1,
                               self.hold_duration - elapsed)

    # ---- 视觉追踪 ----

    def _handle_visual_tracking(self, state, target_detected):
        """
        [VISUAL_TRACKING] 🎯 完全让渡控制权给 UAVControlLoop

        MissionManager 在此状态下 **不发送任何 SETPOINT**。
        UAVControlLoop (20Hz 独立线程) 通过其自有 proxy 发送
        VELOCITY SETPOINT 进行视觉闭环追踪。

        退出条件：
          1. self.tracking_completed == True（外部模块设置）
          2. 目标丢失超过 target_lost_timeout 秒
          3. 追踪总时长超过 tracking_max_duration（兜底）
        """
        tracking_elapsed = time.time() - self.tracking_start_time

        # ---- 兜底超时 ----
        if tracking_elapsed > self.tracking_max_duration:
            logger.info("\n⏰ [追踪超时] %.1fs ≥ %.1fs",
                        tracking_elapsed, self.tracking_max_duration)
            self._exit_tracking()
            return

        # ---- 外部完成信号 ----
        if self.tracking_completed:
            logger.info("\n✅ [追踪完成] 外部模块报告完成，返回巡航")
            self.tracking_completed = False
            self._exit_tracking()
            return

        # ---- 目标丢失超时 ----
        if not target_detected:
            if self._target_lost_start is None:
                self._target_lost_start = time.time()
                lost_elapsed = 0.0
            else:
                lost_elapsed = time.time() - self._target_lost_start
                if lost_elapsed >= self.target_lost_timeout:
                    logger.info("\n⏰ [丢失超时] %.1fs，放弃追踪",
                                lost_elapsed)
                    self._exit_tracking()
                    return
            self._throttle_log("track_lost", 2.0,
                               "[追踪中] 丢失 %.1fs/%.1fs",
                               lost_elapsed if self._target_lost_start
                               else 0.0,
                               self.target_lost_timeout)
        else:
            self._throttle_log_on_change("track_lost_clear",
                                         self._target_lost_start is not None,
                                         "[追踪中] 目标恢复")
            self._target_lost_start = None

        self._throttle_log("track_heartbeat", 5.0,
                           "[追踪中] 进行中... %.1fs", tracking_elapsed)

    def _exit_tracking(self):
        """退出 VISUAL_TRACKING 的统一出口"""
        self.current_wp_index += 1
        self.state = MissionState.NAVIGATING
        self._target_lost_start = None
        self.proxy.reset_setpoint_log()
        logger.info("[状态切换] VISUAL_TRACKING → NAVIGATING，"
                    "前往航点 %d", self.current_wp_index + 1)

    # ---- 返航 ----

    def _handle_returning(self, state):
        """
        [RETURNING] 每帧发送 VELOCITY SETPOINT 返回 HOME 点

        速度受 max_speed 限制，到达判定使用 Haversine 球面距离。
        """
        home = state.get("home", {})
        drone = state.get("drone", {})
        home_lat = home.get("lat", 0.0)
        home_lon = home.get("lon", 0.0)

        # 发送 VELOCITY SETPOINT，速度受 max_speed 限制
        self._send_velocity_to_target(
            state, home_lat, home_lon, self.target_altitude,
        )
        self.current_target = {
            "lat": home_lat, "lon": home_lon,
            "alt": self.target_altitude, "yaw": 0,
        }

        # ---- Haversine 距离到达判定 ----
        current_alt = self._get_current_alt(state)
        drone_lat = drone.get("lat", 0.0)
        drone_lon = drone.get("lon", 0.0)
        dist = dist_3d_latlon(drone_lat, drone_lon, current_alt,
                              home_lat, home_lon, self.target_altitude)

        self._throttle_log("returning", 3.0,
                           "[返航中] 距离=%.2fm", dist)

        if dist < self.arrival_radius:
            logger.info("\n[到达] 已返回起始点，发送 LAND 指令...")
            ok, ack = self.proxy.send_command("LAND")
            if ok:
                self.state = MissionState.LANDING
            else:
                logger.warning("[返航] LAND 失败: %s", ack)

    # ---- 降落前悬停 ----

    def _handle_pre_land_hover(self, state):
        """
        [PRE_LAND_HOVER] 到达降落点后的悬停等待

        在降落点上方悬停 pre_land_hover_duration 秒后，
        发送 LAND 指令进入 LANDING 状态（由 PX4 接管后续自动降落）。
        """
        elapsed = time.time() - self._state_start_time

        # 保持位置悬停（目标点速度控制 → 接近时自动归零）
        wp = self.waypoints[self.current_wp_index]
        self._send_velocity_to_target(
            state,
            wp["lat"], wp["lon"],
            wp.get("alt", self.target_altitude),
        )

        if elapsed >= self.pre_land_hover_duration:
            logger.info("\n🛬 [悬停完成] 执行降落指令")
            ok, ack = self.proxy.send_command("LAND")
            if ok:
                self.state = MissionState.LANDING
            else:
                logger.warning("[降落] LAND 指令失败: %s，仍进入降落状态", ack)
                self.state = MissionState.LANDING
        else:
            self._throttle_log("pre_land", 1.0,
                               "[降落悬停] 剩余 %.1fs",
                               self.pre_land_hover_duration - elapsed)

    # ---- 降落 ----
    def _handle_landing(self, state):
        """[LANDING] 监控 disarm 状态判断着陆"""
        try:
            armed = state["drone"].get("armed", False)
            alt_rel = state["drone"].get("alt_rel", 0.0)
            self._throttle_log("landing", 3.0,
                               "[降落中] 高度=%.1fm 已解锁=%s",
                               alt_rel, not armed)

            if not armed:
                logger.info("\n[完结] 已着陆上锁，任务结束")
                self.state = MissionState.FINISHED
        except (KeyError, TypeError) as e:
            logger.warning("[LANDING] 异常: %s", e)

    # ---- 末端悬停 ----

    def _handle_hold_final(self, state):
        """
        [HOLD_FINAL] 末端悬停，保持最终航点位置

        使用速度闭环（_send_velocity_to_target）而非 POSITION LOCAL_OFFSET_NED，
        避免 GPS 偏移与 PX4 本地坐标系不一致导致飞控满速追目标（曾实测 7.77m/s）。
        """
        if self.waypoints:
            final_wp = self.waypoints[-1]
        else:
            final_wp = {"lat": 0.0, "lon": 0.0,
                        "alt": self.target_altitude, "yaw": 0}

        self._send_velocity_to_target(
            state,
            final_wp["lat"], final_wp["lon"],
            final_wp.get("alt", self.target_altitude),
        )
        self.current_target = {
            "lat": final_wp["lat"], "lon": final_wp["lon"],
            "alt": final_wp.get("alt", self.target_altitude),
            "yaw": final_wp.get("yaw", 0),
        }
        self._throttle_log("hold_final", 10.0,
                           "[挂起] 保持 (%.6f, %.6f, %.1f) 等待接管",
                           final_wp["lat"], final_wp["lon"],
                           final_wp.get("alt", self.target_altitude))

    # ================================================================
    #  辅助
    # ================================================================

    def _send_relative_position_setpoint(self, state, target_lat, target_lon,
                                         target_alt, yaw=0.0, max_speed=None):
        """
        按相对当前位置的 LOCAL_OFFSET_NED 发送 POSITION SETPOINT。

        统一限速：位置设定点按单帧最大推进距离 max_step 钳制，
        即大偏移被拆成多帧逐步逼近，从源头避免飞控满速冲撞。
        （此前 HOLD_FINAL 阶段曾因偏移异常出现 7.77m/s 失控）

        max_step = max(max_speed * 0.5, 0.3)。正常悬停（偏移 < 0.3m）
        不受影响；异常大偏移被钳制的同时打印 WARNING 便于诊断。

        Args:
            max_speed: 限速（m/s），默认 self.max_speed
        """
        if max_speed is None:
            max_speed = self.max_speed

        drone = state.get("drone", {}) if state else {}
        current_lat = drone.get("lat", 0.0)
        current_lon = drone.get("lon", 0.0)
        current_alt = self._get_current_alt(state)

        x, y, z = latlon_alt_to_local_offset(
            target_lat, target_lon, target_alt,
            current_lat, current_lon, current_alt,
        )

        # ---- 统一限速：单帧最大目标偏移（约半个限速秒距离） ----
        max_step = max(max_speed * 0.5, 0.3)
        dist_h = math.sqrt(x * x + y * y)
        if dist_h > max_step * 1.5:
            logger.warning(
                "[限速] POSITION 偏移过大 %.1fm，钳制为 %.1fm（max_speed=%.1f）",
                dist_h, max_step, max_speed)
        if dist_h > max_step:
            x = x / dist_h * max_step
            y = y / dist_h * max_step
        # 垂直方向同样钳制（z>0 表示目标在下）
        z = max(min(z, max_step), -max_step)

        return self.proxy.send_setpoint(
            x=x, y=y, z=z, yaw=yaw,
            frame="LOCAL_OFFSET_NED",
            control_mode="POSITION",
        )

    def _send_velocity_to_target(self, state, target_lat, target_lon,
                                  target_alt, max_speed=None):
        """
        按相对目标的 NED 方向发送 VELOCITY SETPOINT，速度受 max_speed 限制。

        当距离目标较近时自动减速（线性减速区 = 2 × max_speed 米），
        到达 arrival_radius 范围后由调用方（arrival 判定）负责切换状态。

        Args:
            state:      最新飞机 STATE
            target_lat: 目标纬度（度）
            target_lon: 目标经度（度）
            target_alt: 目标相对高度（米，正值向上）
            max_speed:  最大速度（m/s），默认 self.max_speed

        Returns:
            (成功: bool, ACK 字典: dict)
        """
        if max_speed is None:
            max_speed = self.max_speed

        drone = state.get("drone", {}) if state else {}
        current_lat = drone.get("lat", 0.0)
        current_lon = drone.get("lon", 0.0)
        current_alt = self._get_current_alt(state)

        # 计算 NED 偏移 (x=北, y=东, z=下)
        x, y, z = latlon_alt_to_local_offset(
            target_lat, target_lon, target_alt,
            current_lat, current_lon, current_alt,
        )

        dist_h = math.sqrt(x * x + y * y)

        # ---- 水平速度：方向归一化 × 限速（含减速区） ----
        DECEL_FACTOR = 2.0  # 减速区 = max_speed × 2 米
        if dist_h < 0.15:
            vx, vy = 0.0, 0.0
        else:
            desired_speed = max_speed
            decel_dist = max_speed * DECEL_FACTOR
            if dist_h < decel_dist:
                # 线性减速：距离越近速度越慢
                desired_speed = max_speed * (dist_h / decel_dist)
                desired_speed = max(desired_speed, 0.15)  # 最低速度
            vx = (x / dist_h) * desired_speed
            vy = (y / dist_h) * desired_speed

        # ---- 垂直速度：简单 P 控制 ----
        # z > 0 表示目标在下 → 需要正 vz（NED 向下为正）
        vz = z * 2.0   # 增益 2.0
        vz = max(min(vz, max_speed), -max_speed)

        return self.proxy.send_setpoint(
            vx=vx, vy=vy, vz=vz,
            yaw_rate=0.0,
            control_mode="VELOCITY",
        )

    def _set_current_target_from_wp(self, index):
        """从航点列表更新 current_target"""
        if self.waypoints and index < len(self.waypoints):
            wp = self.waypoints[index]
            self.current_target = {
                "lat": wp["lat"], "lon": wp["lon"],
                "alt": wp.get("alt", self.target_altitude),
                "yaw": wp.get("yaw", 0),
            }

    def set_waypoints(self, waypoints):
        """由外部调用，动态注入/更新航点列表（经纬度格式 v2）"""
        self.waypoints = list(waypoints)
        self.current_wp_index = 0
        self._waypoints_ready = True
        logger.info("MissionManager 航点已更新，共 %d 个", len(waypoints))

    # ================================================================
    #  🚨 紧急停靠（外部调用）
    # ================================================================

    def emergency_stop(self):
        """外部触发的紧急降落入口（可在任何状态下调用）"""
        logger.warning("🚨 [emergency_stop] 紧急降落触发！")

        ok, ack = self.proxy.send_command("LAND")
        if ok:
            logger.info("✅ LAND 指令已发送")
        else:
            logger.error(f"❌ LAND 指令发送失败: {ack}")

        self.state = MissionState.LANDING
        self._state_start_time = time.time()