AirTrapMine/src/GUI/mission_orchestrator.py

#!/usr/bin/env python3
"""
MissionOrchestrator —— 多機偵測後的動態包圍任務編排器。

設計重點
========
1. 偵察階段仍可使用既有 A/B 群組：A 搜尋、B 待機。
2. 偵測並由操作員確認後，A/B 角色立即解除，所有友機視為同一個集合。
3. 不再使用 leader-follow / PURSUE 隊形；確認後直接針對敵機未來位置的包圍 slot 收斂。
   A/B 高度以敵機即時相對高度為基準，不再固定以 30m 為中心。
4. 每台友機使用 C2 平滑且持續前進的 quintic Cartesian「微幅發散 → 收斂」虛擬軌跡，避免繞大圈。
5. 對所有 slot assignment（4 機時最多 4! = 24 種）評分：
   - 路徑長度
   - 路線交叉
   - 預測最小間距
   - 頻繁交換 slot 的成本
6. 同層路徑交叉維持硬限制；間距不足時在持續前進中疊加分離修正。
   A/B 跨層若水平距離過近且尚未建立垂直間隔，先原地垂直分層，再水平移動。
7. 包圍完成條件改為「每台友機均到達自己的 slot」，不是形心距敵機。
8. 敵機定位遺失超時時進 TARGET_LOST，將友機 hold 在當下位置，避免繼續追過期目標。

相容性
======
- 保留既有 MissionExecutor 介面：start(plan) / update_plan(plan) / stop()
- 保留 EngagementPanel 的主要 signal/API。
- __init__ 保留舊版 r_engage / r_engage_exit / lon_spacing / lat_offset 參數，
  讓既有 GUI 呼叫端不必立刻全部修改；新策略不再依賴 leader-follow 的 spacing。
"""

import math
import time
from enum import Enum
from itertools import permutations, product

from PyQt6.QtWidgets import (
    QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame,
    QPushButton, QDoubleSpinBox
)
from PyQt6.QtCore import Qt, QObject, pyqtSignal


# ---------------------------------------------------------------------------
# 基礎工具

EARTH_R = 6371000.0


def _log(level, message):
    print(f"[{level}] {message}", flush=True)


def _haversine(lat1, lon1, lat2, lon2):
    """兩經緯度間水平地面距離 (m)。"""
    p1 = math.radians(lat1)
    p2 = math.radians(lat2)
    dphi = math.radians(lat2 - lat1)
    dlam = math.radians(lon2 - lon1)
    a = (math.sin(dphi / 2.0) ** 2
         + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2)
    return 2.0 * EARTH_R * math.asin(min(1.0, math.sqrt(a)))


def _ll_to_m(lat, lon, ref_lat, ref_lon):
    """經緯度 -> 以 ref 為原點的局部平面公尺座標 (x=東, y=北)。"""
    x = math.radians(lon - ref_lon) * EARTH_R * math.cos(math.radians(ref_lat))
    y = math.radians(lat - ref_lat) * EARTH_R
    return x, y


def _m_to_ll(x, y, ref_lat, ref_lon):
    """局部平面公尺座標 -> 經緯度。"""
    lat = ref_lat + math.degrees(y / EARTH_R)
    cos_lat = max(1e-9, math.cos(math.radians(ref_lat)))
    lon = ref_lon + math.degrees(x / (EARTH_R * cos_lat))
    return lat, lon


def _norm2(x, y):
    return math.hypot(x, y)


def _normalize(x, y, fallback=(1.0, 0.0)):
    n = math.hypot(x, y)
    if n < 1e-9:
        return fallback
    return x / n, y / n


def _bezier_point(p0, p1, p2, p3, t):
    """2D cubic Bezier。pX=(x,y)。"""
    u = 1.0 - t
    b0 = u ** 3
    b1 = 3.0 * u * u * t
    b2 = 3.0 * u * t * t
    b3 = t ** 3
    return (
        b0 * p0[0] + b1 * p1[0] + b2 * p2[0] + b3 * p3[0],
        b0 * p0[1] + b1 * p1[1] + b2 * p2[1] + b3 * p3[1],
    )


def _smoothstep5(t):
    """Quintic smoothstep: 0/1 端點的一、二階導數皆為 0。"""
    t = min(1.0, max(0.0, float(t)))
    return t * t * t * (t * (t * 6.0 - 15.0) + 10.0)


def _wrap_pi(a):
    """角度包到 [-pi, pi)。"""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def _polyline_length(points):
    total = 0.0
    for a, b in zip(points, points[1:]):
        total += math.hypot(b[0] - a[0], b[1] - a[1])
    return total


def _orientation(a, b, c):
    return ((b[0] - a[0]) * (c[1] - a[1])
            - (b[1] - a[1]) * (c[0] - a[0]))


def _segments_intersect(a, b, c, d, eps=1e-9):
    """一般位置線段相交測試；對任務規劃足夠。"""
    o1 = _orientation(a, b, c)
    o2 = _orientation(a, b, d)
    o3 = _orientation(c, d, a)
    o4 = _orientation(c, d, b)

    # 只把真正穿越當成 crossing；共線/端點接觸交給 minimum separation 處理。
    return ((o1 > eps and o2 < -eps) or (o1 < -eps and o2 > eps)) and \
           ((o3 > eps and o4 < -eps) or (o3 < -eps and o4 > eps))


# ---------------------------------------------------------------------------
# 預設參數

DEFAULT_R_DETECT = 40.0
DEFAULT_DETECT_DWELL = 1.0

# 以下兩個保留作為舊呼叫端相容參數；新策略不用「距離到某半徑才切追擊/包圍」。
DEFAULT_R_ENGAGE = 25.0
DEFAULT_R_ENGAGE_EXIT = 35.0

# 新策略建議值
DEFAULT_CIRCLE_RADIUS = 10.0          # 最終包圍半徑 (m)；近場會以敵機當下位置為圓心
DEFAULT_FORMATION_ALT = 30.0          # 敵機高度缺失時的安全備援值 (m，相對 home)
DEFAULT_ALT_LAYER_GAP = 10.0          # A/B 兩層的垂直間隔 (m)
DEFAULT_MIN_ALT_FLOOR = 10.0          # 最低安全相對高度 (m)
DEFAULT_CROSS_LAYER_HOLD_DISTANCE = 12.0  # A/B 水平距離小於此值時先確認垂直分層 (m)
DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP = 7.0  # 跨層近距離時要求的最小實際垂直間隔 (m)
DEFAULT_CROSS_LAYER_ALT_TOLERANCE = 1.0   # 垂直分層完成容許誤差 (m)

DEFAULT_SPREAD_DISTANCE = 3.0         # 小幅外擴上限 (m)：避免繞大圈，只保留必要的發散
DEFAULT_APPROACH_MARGIN = 4.0         # 保留相容參數；新 Cartesian 平滑路徑不再強制繞到圈外 staging
DEFAULT_LOOKAHEAD_T = 0.35            # 遠距離時的 Bezier 前視比例
DEFAULT_FINAL_DIRECT_DISTANCE = 4.0    # 只在最後 4m 直接送 slot；避免中途提早進入減速
DEFAULT_LEAD_FADE_START = 30.0         # 平均距敵 <= 此距離開始逐步減少敵機前置預測
DEFAULT_LEAD_FADE_END = 18.0           # 平均距敵 <= 此距離完全取消前置，圓心貼住敵機當下位置
DEFAULT_CENTER_SMOOTH_ALPHA = 0.25    # 規劃圓心低通係數，降低 GPS/敵速造成的路徑抖動
DEFAULT_LOOKAHEAD_DISTANCE = 14.0      # 保持較遠 carrot point，避免反覆追短 waypoint 而減速
DEFAULT_PATH_SAMPLES = 41             # 路徑/交叉預測取樣數；提高以降低漏判
DEFAULT_SAFE_SEPARATION = 8.0         # 硬安全間距 (m)
DEFAULT_FORMATION_SPACING = 12.0      # 移動中希望逐步拉到的隊形間距；比硬安全距離更早開始展開
DEFAULT_INITIAL_SEP_TOLERANCE = 0.5   # 起始若小於 8m，允許最多只再縮短這麼多 (m)
DEFAULT_EMERGENCY_SEPARATION = 3.0   # 無論如何不可低於的水平緊急下限 (m)
DEFAULT_RECOVERY_SPREAD_STEP = 2.0    # 舊版保底：無 assignment 時的小幅外擴 (m)
DEFAULT_SEPARATION_CORRECTION_GAIN = 1.25  # 更積極的移動中隊形修正
DEFAULT_SEPARATION_CORRECTION_MAX = 5.0    # 最大側向修正；仍保證主要前進分量
DEFAULT_DIRECTION_SMOOTH_ALPHA = 0.35      # 每 tick 接受新方向的比例；越小越平滑
DEFAULT_MAX_LATERAL_RATIO = 0.45           # 側向/前進最大比例，避免修隊形時原地橫移
DEFAULT_LAYER_LANE_SPACING = 4.0       # 同層同心圓車道的半徑間隔 (m)
DEFAULT_LAYER_AVOID_DISTANCE = 14.0    # 同層距離小於此值開始啟用同心圓避讓
DEFAULT_LAYER_LANE_FADE_DISTANCE = 12.0 # 距最終 slot 小於此值時逐步收回共同包圍半徑
DEFAULT_HEADING_SMOOTH_ALPHA = 0.25      # 敵機 heading 低通，避免 X 陣形因航向抖動快速旋轉
DEFAULT_ENCIRCLE_TOLERANCE = 3.0      # 每台距自己 slot <= 此值才算完成包圍
DEFAULT_REASSIGN_IMPROVEMENT = 0.20   # 安全時至少改善 20% 才允許換 slot
DEFAULT_REASSIGN_COOLDOWN = 2.0       # slot 交換最短間隔 (s)；安全時原則上不換

DEFAULT_REPLAN_MOVE_M = 1.0           # 敵機移動 >= 1m 觸發更新
DEFAULT_REPLAN_CAP_SEC = 0.4           # 最慢 0.4 秒更新一次瞬時目標
DEFAULT_LEAD_TIME_CAP = 4.0
DEFAULT_FRIENDLY_SPEED = 3.0
DEFAULT_ENEMY_VEL_WINDOW = 1.0
DEFAULT_TARGET_LOST_TIMEOUT = 5.0

# assignment cost
DEFAULT_CROSSING_PENALTY = 20000.0
DEFAULT_SEPARATION_PENALTY = 30000.0
DEFAULT_SLOT_SWITCH_PENALTY = 8.0

# 舊版相容參數：保留但不再當 leader-follow spacing 使用
DEFAULT_LON_SPACING = 8.0
DEFAULT_LAT_OFFSET = 5.0

_NONE_LABEL = "（未選擇）"


class MissionPhase(Enum):
    IDLE = "idle"
    WATCH = "watch"
    DETECTED = "detected"
    CONVERGE = "converge"          # 動態分配 slot + 發散/收斂曲線接近
    ENCIRCLE = "encircle"          # 所有友機已進入自己的 slot
    TARGET_LOST = "target_lost"    # 目標定位逾時，友機 hold


# ---------------------------------------------------------------------------
# Mission Orchestrator

class MissionOrchestrator(QObject):
    """
    任務狀態機。

    callback：
      make_executor()  -> MissionExecutor
      gps_provider()   -> {drone_id: {'lat','lon','alt'}}
      stop_groups_fn() -> 操作員確認重編隊時，停止既有 A/B 群組任務

    新策略：
      WATCH -> DETECTED -> CONVERGE -> ENCIRCLE

    進入 CONVERGE 後保留 A/B 分層：A/B 各自固定在敵機高度上下層；
    同層必要時使用同心圓半徑車道避讓。跨層近距離時先垂直、後水平。
    """

    phase_changed = pyqtSignal(str)
    detection_prompt = pyqtSignal(str)
    status_message = pyqtSignal(str)

    def __init__(
        self,
        make_executor,
        gps_provider,
        stop_groups_fn=None,
        r_detect=DEFAULT_R_DETECT,
        detect_dwell_sec=DEFAULT_DETECT_DWELL,
        r_engage=DEFAULT_R_ENGAGE,
        r_engage_exit=DEFAULT_R_ENGAGE_EXIT,
        replan_move_m=DEFAULT_REPLAN_MOVE_M,
        replan_cap_sec=DEFAULT_REPLAN_CAP_SEC,
        circle_radius=DEFAULT_CIRCLE_RADIUS,
        lon_spacing=DEFAULT_LON_SPACING,
        lat_offset=DEFAULT_LAT_OFFSET,
        formation_alt=DEFAULT_FORMATION_ALT,
        alt_layer_gap=DEFAULT_ALT_LAYER_GAP,
        min_alt_floor=DEFAULT_MIN_ALT_FLOOR,
        lead_time_cap_sec=DEFAULT_LEAD_TIME_CAP,
        friendly_speed=DEFAULT_FRIENDLY_SPEED,
        enemy_vel_window_sec=DEFAULT_ENEMY_VEL_WINDOW,
        spread_distance=DEFAULT_SPREAD_DISTANCE,
        approach_margin=DEFAULT_APPROACH_MARGIN,
        lookahead_t=DEFAULT_LOOKAHEAD_T,
        final_direct_distance=DEFAULT_FINAL_DIRECT_DISTANCE,
        lead_fade_start=DEFAULT_LEAD_FADE_START,
        lead_fade_end=DEFAULT_LEAD_FADE_END,
        center_smooth_alpha=DEFAULT_CENTER_SMOOTH_ALPHA,
        lookahead_distance=DEFAULT_LOOKAHEAD_DISTANCE,
        path_samples=DEFAULT_PATH_SAMPLES,
        safe_separation=DEFAULT_SAFE_SEPARATION,
        formation_spacing=DEFAULT_FORMATION_SPACING,
        initial_sep_tolerance=DEFAULT_INITIAL_SEP_TOLERANCE,
        emergency_separation=DEFAULT_EMERGENCY_SEPARATION,
        recovery_spread_step=DEFAULT_RECOVERY_SPREAD_STEP,
        separation_correction_gain=DEFAULT_SEPARATION_CORRECTION_GAIN,
        separation_correction_max=DEFAULT_SEPARATION_CORRECTION_MAX,
        direction_smooth_alpha=DEFAULT_DIRECTION_SMOOTH_ALPHA,
        max_lateral_ratio=DEFAULT_MAX_LATERAL_RATIO,
        layer_lane_spacing=DEFAULT_LAYER_LANE_SPACING,
        layer_avoid_distance=DEFAULT_LAYER_AVOID_DISTANCE,
        layer_lane_fade_distance=DEFAULT_LAYER_LANE_FADE_DISTANCE,
        heading_smooth_alpha=DEFAULT_HEADING_SMOOTH_ALPHA,
        encircle_tolerance=DEFAULT_ENCIRCLE_TOLERANCE,
        reassign_improvement=DEFAULT_REASSIGN_IMPROVEMENT,
        reassign_cooldown_sec=DEFAULT_REASSIGN_COOLDOWN,
        target_lost_timeout_sec=DEFAULT_TARGET_LOST_TIMEOUT,
        crossing_penalty=DEFAULT_CROSSING_PENALTY,
        separation_penalty=DEFAULT_SEPARATION_PENALTY,
        slot_switch_penalty=DEFAULT_SLOT_SWITCH_PENALTY,
        parent=None,
        cross_layer_hold_distance=DEFAULT_CROSS_LAYER_HOLD_DISTANCE,
        cross_layer_min_vertical_gap=DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP,
        cross_layer_alt_tolerance=DEFAULT_CROSS_LAYER_ALT_TOLERANCE,
    ):
        super().__init__(parent)

        self._make_executor = make_executor
        self._gps_provider = gps_provider
        self._stop_groups_fn = stop_groups_fn

        self.r_detect = float(r_detect)
        self.detect_dwell_sec = float(detect_dwell_sec)

        # legacy/compatibility
        self.r_engage = float(r_engage)
        self.r_engage_exit = float(r_engage_exit)
        self.lon_spacing = float(lon_spacing)
        self.lat_offset = float(lat_offset)

        self.replan_move_m = float(replan_move_m)
        self.replan_cap_sec = float(replan_cap_sec)
        self.circle_radius = float(circle_radius)
        self.formation_alt = float(formation_alt)
        self.alt_layer_gap = float(alt_layer_gap)
        self.min_alt_floor = float(min_alt_floor)
        self.cross_layer_hold_distance = max(
            0.0, float(cross_layer_hold_distance)
        )
        self.cross_layer_min_vertical_gap = max(
            0.0, float(cross_layer_min_vertical_gap)
        )
        self.cross_layer_alt_tolerance = max(
            0.0, float(cross_layer_alt_tolerance)
        )
        self.lead_time_cap_sec = float(lead_time_cap_sec)
        self.friendly_speed = float(friendly_speed)
        self.enemy_vel_window_sec = float(enemy_vel_window_sec)

        self.spread_distance = float(spread_distance)
        self.approach_margin = float(approach_margin)
        self.lookahead_t = min(0.95, max(0.05, float(lookahead_t)))
        self.final_direct_distance = max(0.0, float(final_direct_distance))
        self.lead_fade_start = max(0.0, float(lead_fade_start))
        self.lead_fade_end = max(0.0, float(lead_fade_end))
        self.center_smooth_alpha = min(1.0, max(0.02, float(center_smooth_alpha)))
        self.lookahead_distance = max(1.0, float(lookahead_distance))
        if self.lead_fade_end > self.lead_fade_start:
            self.lead_fade_end, self.lead_fade_start = self.lead_fade_start, self.lead_fade_end
        self.path_samples = max(7, int(path_samples))
        self.safe_separation = float(safe_separation)
        self.formation_spacing = max(self.safe_separation, float(formation_spacing))
        self.initial_sep_tolerance = max(0.0, float(initial_sep_tolerance))
        self.emergency_separation = max(0.5, float(emergency_separation))
        self.recovery_spread_step = max(0.5, float(recovery_spread_step))
        self.separation_correction_gain = max(0.0, float(separation_correction_gain))
        self.separation_correction_max = max(0.0, float(separation_correction_max))
        self.direction_smooth_alpha = min(1.0, max(0.05, float(direction_smooth_alpha)))
        self.max_lateral_ratio = min(1.0, max(0.05, float(max_lateral_ratio)))
        self.layer_lane_spacing = max(0.5, float(layer_lane_spacing))
        self.layer_avoid_distance = max(self.safe_separation, float(layer_avoid_distance))
        self.layer_lane_fade_distance = max(1.0, float(layer_lane_fade_distance))
        self.heading_smooth_alpha = min(1.0, max(0.02, float(heading_smooth_alpha)))
        self.encircle_tolerance = float(encircle_tolerance)
        self.reassign_improvement = min(0.9, max(0.0, float(reassign_improvement)))
        self.reassign_cooldown_sec = float(reassign_cooldown_sec)
        self.target_lost_timeout_sec = float(target_lost_timeout_sec)
        self.crossing_penalty = float(crossing_penalty)
        self.separation_penalty = float(separation_penalty)
        self.slot_switch_penalty = float(slot_switch_penalty)

        self.phase = MissionPhase.IDLE
        self.a_drone_ids = []
        self.b_drone_ids = []
        self.enemy_id = None

        self._detect_since = 0.0

        # 確認後的角色無關友機集合
        self._friendly_ids = []
        self._merged_ex = None

        # assignment: {drone_id: slot_index}
        self._assignment = {}
        self._last_assignment_change_at = 0.0
        self._last_assignment_had_conflict = False

        # 敵機運動估計
        self._enemy_hist = []
        self._enemy_vel = (0.0, 0.0)   # east, north m/s
        self._enemy_alt = None          # 敵機即時相對高度；缺失時才用 formation_alt
        self._enemy_heading_deg = None  # 0°=North, clockwise；X 陣形以此為基準
        self._last_enemy_seen_at = 0.0

        # replan / command state
        self._last_replan_at = 0.0
        self._last_plan_enemy = None
        self._last_predicted_center = None
        self._last_slots = None
        self._smoothed_center = None
        # 每架持續前進方向；避免每次 replan 重新生成 carrot 時航向跳動/煞停
        self._motion_dirs = {}
        self._slot_groups = {}  # {slot_index: "A"/"B"}
        self._vertical_hold_ids = set()

    # ------------------------------------------------------------------ 設定

    def configure(self, a_drone_ids, b_drone_ids, enemy_id):
        """
        WATCH/DETECTED 階段可更新 A/B 與敵機。
        一旦進入 CONVERGE/ENCIRCLE，角色集合鎖定，避免 GUI 下拉變動破壞任務。
        """
        if self.phase in (
            MissionPhase.CONVERGE,
            MissionPhase.ENCIRCLE,
            MissionPhase.TARGET_LOST,
        ):
            return False

        self.a_drone_ids = list(a_drone_ids)
        self.b_drone_ids = list(b_drone_ids)
        new_enemy = enemy_id or None
        if new_enemy != self.enemy_id:
            self._detect_since = 0.0
            self._enemy_hist = []
            self._enemy_vel = (0.0, 0.0)
            self._enemy_alt = None
            self._enemy_heading_deg = None
        self.enemy_id = new_enemy
        return True

    def _configured(self):
        friendly = set(self.a_drone_ids) | set(self.b_drone_ids)
        return (
            bool(self.enemy_id)
            and bool(self.a_drone_ids)
            and self.enemy_id not in friendly
            and not (set(self.a_drone_ids) & set(self.b_drone_ids))
        )

    # ------------------------------------------------------------------ 操作員確認 / 停止

    def confirm_detection(self):
        """
        DETECTED -> CONVERGE。

        1. 停止既有 A/B 群組任務。
        2. A/B 身分解除，合併為單一 friendly 集合。
        3. 計算預測敵機中心與圓形 slots。
        4. 做全域 slot assignment。
        5. 產生 Bezier lookahead point，啟動合併 executor。
        """
        if self.phase != MissionPhase.DETECTED:
            self.status_message.emit("目前不在待確認狀態")
            return

        gps = self._gps_provider()
        epos = gps.get(self.enemy_id)
        now = time.monotonic()
        if not epos or not self._position_is_fresh(epos, now):
            self.status_message.emit("敵機定位遺失，無法開始包圍")
            return
        if self._position_alt(epos) is None:
            self.status_message.emit("敵機相對 Home 高度尚未收到，無法開始包圍")
            return

        friendly_ids = self._unique_keep_order(self.a_drone_ids + self.b_drone_ids)
        if not friendly_ids:
            self.status_message.emit("沒有可用友機")
            return
        if self._positions_for(friendly_ids, gps) is None:
            self.status_message.emit("有友機尚未定位，無法開始包圍")
            return

        if self._stop_groups_fn:
            try:
                self._stop_groups_fn()
            except Exception as e:
                _log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")

        self._friendly_ids = friendly_ids
        self._assignment = {}
        self._last_assignment_change_at = 0.0
        self._last_assignment_had_conflict = False
        self._enemy_hist = []
        self._enemy_vel = (0.0, 0.0)
        self._enemy_alt = None
        self._enemy_heading_deg = None
        self._smoothed_center = None
        self._motion_dirs = {}
        self._slot_groups = {}  # {slot_index: "A"/"B"}
        self._vertical_hold_ids = set()
        self._last_enemy_seen_at = self._position_received_at(epos, now)
        self._update_enemy_velocity(epos)

        center = self._planning_center(gps, epos, reset=True)
        slots = self._circle_slots_for_center(center, len(self._friendly_ids))
        assignment, info = self._choose_assignment(gps, center, slots, force=True)

        self._merged_ex = self._make_executor()
        if not assignment:
            # 找不到完全滿足間距的方案時，不再先停/預分散。
            # 改採 0-crossing 的最低風險 provisional assignment，
            # 一邊朝 slot 前進、一邊疊加 separation correction 拉開隊形。
            assignment, info = self._choose_progress_assignment(gps, center, slots)
            if not assignment:
                self.status_message.emit("目前找不到 0-crossing 路徑，暫不開始包圍")
                self._merged_ex = None
                return
            self._assignment = assignment
            self._last_assignment_change_at = time.monotonic()
            self._last_assignment_had_conflict = True
            targets = self._moving_converge_targets(gps, center, slots, assignment)
            if targets is None:
                self.status_message.emit("無法建立移動中隊形調整目標")
                self._merged_ex = None
                return
            self._merged_ex.start(self._targets_to_plan(targets))
            self.status_message.emit(
                "起始間距不足：保持前進，同時在移動中逐步拉開隊形"
            )
        else:
            self._assignment = assignment
            self._last_assignment_change_at = time.monotonic()
            self._last_assignment_had_conflict = info['conflict']
            targets = self._moving_converge_targets(gps, center, slots, assignment)
            self._merged_ex.start(self._targets_to_plan(targets))

        self._last_predicted_center = center
        self._last_slots = slots
        self._mark_replanned(epos)
        self._set_phase(MissionPhase.CONVERGE)

        if assignment:
            self.status_message.emit("開始動態包圍：slot 鎖定，移動中同步展開隊形")

    def stop(self):
        """停止任務並回 IDLE。"""
        if self._merged_ex is not None:
            try:
                self._merged_ex.stop()
            except Exception:
                pass
        self._merged_ex = None
        self._friendly_ids = []
        self._assignment = {}
        self._detect_since = 0.0
        self._enemy_hist = []
        self._enemy_vel = (0.0, 0.0)
        self._enemy_alt = None
        self._enemy_heading_deg = None
        self._last_enemy_seen_at = 0.0
        self._last_predicted_center = None
        self._last_slots = None
        self._smoothed_center = None
        self._motion_dirs = {}
        self._slot_groups = {}  # {slot_index: "A"/"B"}
        self._vertical_hold_ids = set()
        self._set_phase(MissionPhase.IDLE)
        self.status_message.emit("任務已停止")

    # ------------------------------------------------------------------ tick

    def tick(self):
        """由 GUI 週期呼叫，建議 5~10 Hz。"""
        if self.phase == MissionPhase.IDLE and self._configured():
            self._set_phase(MissionPhase.WATCH)
        elif self.phase == MissionPhase.WATCH and not self._configured():
            self._detect_since = 0.0
            self._set_phase(MissionPhase.IDLE)

        if self.phase == MissionPhase.WATCH:
            self._tick_watch()
        elif self.phase == MissionPhase.CONVERGE:
            self._tick_converge()
        elif self.phase == MissionPhase.ENCIRCLE:
            self._tick_encircle()
        elif self.phase == MissionPhase.TARGET_LOST:
            self._tick_target_lost()

    def _tick_watch(self):
        gps = self._gps_provider()
        epos = gps.get(self.enemy_id)
        now = time.monotonic()
        if not epos or not self._position_is_fresh(epos, now):
            self._detect_since = 0.0
            return

        nearest_id = None
        nearest_d = None
        for did in self.a_drone_ids:
            pos = gps.get(did)
            if not pos or not self._position_is_fresh(pos, now):
                continue
            d = _haversine(epos['lat'], epos['lon'], pos['lat'], pos['lon'])
            if nearest_d is None or d < nearest_d:
                nearest_id, nearest_d = did, d

        if nearest_d is not None and nearest_d <= self.r_detect:
            if self._detect_since == 0.0:
                self._detect_since = now
            elif now - self._detect_since >= self.detect_dwell_sec:
                self._set_phase(MissionPhase.DETECTED)
                self.detection_prompt.emit(
                    f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認動態包圍"
                )
        else:
            self._detect_since = 0.0

    def _tick_converge(self):
        gps = self._gps_provider()
        epos = gps.get(self.enemy_id)
        now = time.monotonic()

        if (not epos or not self._position_is_fresh(epos, now)
                or self._position_alt(epos) is None):
            self._handle_enemy_missing(gps, now)
            return

        self._last_enemy_seen_at = self._position_received_at(epos, now)
        self._update_enemy_velocity(epos)

        if self._positions_for(self._friendly_ids, gps) is None:
            return

        if self._all_in_slots(gps, epos):
            self._set_phase(MissionPhase.ENCIRCLE)
            self.status_message.emit("所有友機已進入各自包圍 slot")
            # 進 ENCIRCLE 當下仍送一次精確 slot
            self._update_encircle_command(gps, epos, force=True)
            return

        if self._should_replan(now, epos):
            self._update_converge_command(gps, epos)

    def _tick_encircle(self):
        gps = self._gps_provider()
        epos = gps.get(self.enemy_id)
        now = time.monotonic()

        if (not epos or not self._position_is_fresh(epos, now)
                or self._position_alt(epos) is None):
            self._handle_enemy_missing(gps, now)
            return

        self._last_enemy_seen_at = self._position_received_at(epos, now)
        self._update_enemy_velocity(epos)

        if self._positions_for(self._friendly_ids, gps) is None:
            return

        # 敵機持續移動，圓心跟著預測位置移動。
        # 若任何友機明顯脫離自己的 slot，回 CONVERGE 重新做曲線收斂。
        if not self._all_in_slots(gps, epos, tolerance=self.encircle_tolerance * 1.8):
            self._set_phase(MissionPhase.CONVERGE)
            self.status_message.emit("包圍幾何失配，重新收斂")
            self._update_converge_command(gps, epos, force_assignment=False)
            return

        if self._should_replan(now, epos):
            self._update_encircle_command(gps, epos)

    def _tick_target_lost(self):
        gps = self._gps_provider()
        epos = gps.get(self.enemy_id)
        now = time.monotonic()
        if (not epos or not self._position_is_fresh(epos, now)
                or self._position_alt(epos) is None):
            return

        # 目標重新出現：不沿用過期軌跡，重新估速並回 CONVERGE。
        self._enemy_hist = []
        self._enemy_vel = (0.0, 0.0)
        self._last_enemy_seen_at = self._position_received_at(epos, now)
        self._update_enemy_velocity(epos)
        self._set_phase(MissionPhase.CONVERGE)
        self.status_message.emit("敵機定位恢復，重新建立包圍軌跡")
        self._update_converge_command(gps, epos, force_assignment=True)

    # ------------------------------------------------------------------ 敵機遺失 / hold

    def _handle_enemy_missing(self, gps, now):
        if self._last_enemy_seen_at <= 0.0:
            self._last_enemy_seen_at = now
            return
        if now - self._last_enemy_seen_at < self.target_lost_timeout_sec:
            return

        self._hold_current_positions(gps)
        self._set_phase(MissionPhase.TARGET_LOST)
        self.status_message.emit("敵機定位逾時：友機保持目前位置")

    def _hold_current_positions(self, gps):
        if self._merged_ex is None:
            return
        targets = {}
        now = time.monotonic()
        for did in self._friendly_ids:
            p = gps.get(did)
            if not p or not self._position_is_fresh(p, now):
                continue
            current_alt = self._position_alt(p)
            if current_alt is None:
                continue
            targets[did] = (
                p['lat'],
                p['lon'],
                max(self.min_alt_floor, current_alt),
            )
        if len(targets) == len(self._friendly_ids):
            self._merged_ex.update_plan(self._targets_to_plan(targets))

    # ------------------------------------------------------------------ 敵機速度 / 預測中心

    def _set_enemy_heading(self, heading_deg):
        """以最短角度差低通更新敵機 heading，正確處理 359°/0° 邊界。"""
        heading_deg = float(heading_deg) % 360.0
        if self._enemy_heading_deg is None:
            self._enemy_heading_deg = heading_deg
            return
        err = ((heading_deg - self._enemy_heading_deg + 180.0) % 360.0) - 180.0
        self._enemy_heading_deg = (
            self._enemy_heading_deg + self.heading_smooth_alpha * err
        ) % 360.0

    def _update_enemy_velocity(self, epos):
        """更新敵機高度/heading，並以 GPS 差分估水平速度 (east, north) m/s。"""
        try:
            enemy_alt = float(epos.get('alt'))
        except (TypeError, ValueError):
            enemy_alt = None
        if enemy_alt is not None and math.isfinite(enemy_alt):
            self._enemy_alt = enemy_alt

        # heading convention：0°=North、90°=East、順時針增加。
        # 優先讀 heading/hdg/course；yaw 若像 radians 就自動轉 degree。
        raw_heading = None
        for key in ('heading', 'hdg', 'course'):
            try:
                v = epos.get(key)
                if v is not None and math.isfinite(float(v)):
                    v = float(v)
                    if key == 'hdg' and abs(v) > 360.0:
                        v *= 0.01
                    raw_heading = v % 360.0
                    break
            except (TypeError, ValueError):
                pass
        if raw_heading is None:
            try:
                v = epos.get('yaw')
                if v is not None and math.isfinite(float(v)):
                    v = float(v)
                    raw_heading = (math.degrees(v) if abs(v) <= 2.0 * math.pi + 0.2 else v) % 360.0
            except (TypeError, ValueError):
                pass

        if raw_heading is not None:
            self._set_enemy_heading(raw_heading)

        now = time.monotonic()
        self._enemy_hist.append((now, epos['lat'], epos['lon']))

        while (
            len(self._enemy_hist) >= 2
            and now - self._enemy_hist[0][0] > self.enemy_vel_window_sec
        ):
            self._enemy_hist.pop(0)

        if len(self._enemy_hist) >= 2:
            t0, la0, lo0 = self._enemy_hist[0]
            t1, la1, lo1 = self._enemy_hist[-1]
            dt = t1 - t0
            if dt > 1e-3:
                dx, dy = _ll_to_m(la1, lo1, la0, lo0)
                self._enemy_vel = (dx / dt, dy / dt)

        # 若 provider 沒有 heading，以每次更新的水平航跡方向作持續備援；
        # 不能只在第一次設定，否則敵機轉彎後 X 陣形不會跟著旋轉。
        if raw_heading is None:
            evx, evy = self._enemy_vel
            if math.hypot(evx, evy) > 0.3:
                course_deg = math.degrees(math.atan2(evx, evy)) % 360.0
                self._set_enemy_heading(course_deg)

    def enemy_speed(self):
        return math.hypot(*self._enemy_vel)

    def _predict_enemy_center(self, gps, epos):
        """
        預測「包圍圓未來中心」，而不是給 leader 一個追擊點。
        lead time 取友機到敵機平均距離 / friendly_speed，並封頂。
        """
        gaps = []
        for did in self._friendly_ids:
            p = gps.get(did)
            if p:
                gaps.append(_haversine(p['lat'], p['lon'], epos['lat'], epos['lon']))
        avg_gap = sum(gaps) / len(gaps) if gaps else 0.0
        raw_lead_t = min(
            self.lead_time_cap_sec,
            avg_gap / max(0.1, self.friendly_speed),
        )

        # 遠距離保留前置攔截；接近包圍圈時逐步把前置量淡出。
        # 這樣最終包圍圓會貼著敵機當下位置，不會因敵機速度 × lead time
        # 讓視覺上/實際上看起來像半徑被拉到 20~30 m。
        if self.lead_fade_start <= self.lead_fade_end + 1e-6:
            lead_scale = 0.0 if avg_gap <= self.lead_fade_end else 1.0
        elif avg_gap >= self.lead_fade_start:
            lead_scale = 1.0
        elif avg_gap <= self.lead_fade_end:
            lead_scale = 0.0
        else:
            lead_scale = (avg_gap - self.lead_fade_end) / (
                self.lead_fade_start - self.lead_fade_end
            )

        lead_t = raw_lead_t * lead_scale
        evx, evy = self._enemy_vel
        plat, plon = _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon'])
        return plat, plon

    def _planning_center(self, gps, epos, reset=False):
        """
        對預測圓心做低通。只在真正產生命令/初始規劃時呼叫，避免同一 tick
        多次查詢時重複濾波。這能明顯降低 moving target/GPS 抖動造成的曲線折感。
        """
        raw = self._predict_enemy_center(gps, epos)
        if reset or self._smoothed_center is None:
            self._smoothed_center = raw
            return raw
        plat, plon = self._smoothed_center
        dx, dy = _ll_to_m(raw[0], raw[1], plat, plon)
        a = self.center_smooth_alpha
        filt = _m_to_ll(dx * a, dy * a, plat, plon)
        self._smoothed_center = filt
        return filt

    # ------------------------------------------------------------------ slot 生成 / 高度

    def _group_of(self, did):
        if did in self.a_drone_ids:
            return "A"
        if did in self.b_drone_ids:
            return "B"
        return None

    def _group_ids(self, group):
        src = self.a_drone_ids if group == "A" else self.b_drone_ids
        friendly = set(self._friendly_ids)
        return [d for d in src if d in friendly]

    def _layer_alt(self, group):
        """以敵機即時高度為中心：A 在上層、B 在下層。"""
        base_alt = (
            self._enemy_alt
            if self._enemy_alt is not None
            else self.formation_alt
        )
        half = self.alt_layer_gap / 2.0
        lower = max(self.min_alt_floor, base_alt - half)
        # 敵機高度過低而觸發最低高度限制時，仍保留完整 A/B 垂直間隔。
        upper = max(self.min_alt_floor, base_alt + half, lower + self.alt_layer_gap)
        return upper if group == "A" else lower

    @staticmethod
    def _position_alt(pos):
        """讀取有效相對高度；缺值或非有限值回傳 None。"""
        if not pos:
            return None
        try:
            alt = float(pos.get('alt'))
        except (TypeError, ValueError):
            return None
        return alt if math.isfinite(alt) else None

    def _position_is_fresh(self, pos, now=None):
        """檢查 GPS 水平位置是否仍在可接受的遙測時間內。"""
        if not pos:
            return False
        updated_at = pos.get('_gps_updated_at')
        # 保留舊 gps_provider/測試資料的相容性；GUI 實飛資料一定帶時間戳。
        if updated_at is None:
            return True
        try:
            updated_at = float(updated_at)
        except (TypeError, ValueError):
            return False
        if not math.isfinite(updated_at):
            return False
        now = time.monotonic() if now is None else float(now)
        age = now - updated_at
        return -0.1 <= age <= self.target_lost_timeout_sec

    @staticmethod
    def _position_received_at(pos, now):
        """取得最後一筆水平定位的 monotonic 時間，舊資料則使用目前時間。"""
        try:
            updated_at = float(pos.get('_gps_updated_at'))
        except (AttributeError, TypeError, ValueError):
            return now
        if not math.isfinite(updated_at) or updated_at > now + 0.1:
            return now
        return updated_at

    def _cross_layer_vertical_hold_ids(self, gps):
        """
        找出必須先原地垂直分層的 A/B 友機。

        只有 A/B 水平距離小於 cross_layer_hold_distance 時才介入；待實際
        高度符合 A 在 B 上方，且垂直差接近規劃層差後，才解除水平鎖定。
        """
        hold = set()
        a_ids = [d for d in self._group_ids("A") if gps.get(d)]
        b_ids = [d for d in self._group_ids("B") if gps.get(d)]
        required_gap = min(
            self._layer_alt("A") - self._layer_alt("B"),
            self.cross_layer_min_vertical_gap,
        )

        for aid in a_ids:
            apos = gps[aid]
            for bid in b_ids:
                bpos = gps[bid]
                horizontal = _haversine(
                    apos['lat'], apos['lon'], bpos['lat'], bpos['lon']
                )
                if horizontal >= self.cross_layer_hold_distance:
                    continue
                a_alt = self._position_alt(apos)
                b_alt = self._position_alt(bpos)
                # 高度缺失時採保守策略：不允許近距離跨層水平接近。
                if a_alt is None or b_alt is None or a_alt - b_alt < required_gap:
                    hold.update((aid, bid))
        return hold

    def _apply_cross_layer_vertical_gate(self, targets, gps):
        """把跨層近距離且未完成垂直分離的水平目標鎖在當前位置。"""
        hold = self._cross_layer_vertical_hold_ids(gps)
        for did in hold:
            p = gps.get(did)
            if not p or did not in targets:
                continue
            targets[did] = (p['lat'], p['lon'], self._layer_alt(self._group_of(did)))
            self._motion_dirs.pop(did, None)

        if hold != self._vertical_hold_ids:
            if hold:
                labels = ", ".join(sorted(hold))
                self.status_message.emit(
                    f"A/B 近距離：{labels} 先垂直分層，再恢復水平移動"
                )
            elif self._vertical_hold_ids:
                self.status_message.emit("A/B 垂直分層完成，恢復水平移動")
            self._vertical_hold_ids = hold
        return targets

    def _slot_alt(self, slot_index):
        group = self._slot_groups.get(slot_index, "A")
        return self._layer_alt(group)

    def _circle_slots_for_center(self, center, n):
        """
        以敵機機體座標系建立真正的 X 形包圍。

        body-frame：
          forward = 敵機 heading 0° 軸
          right   = 敵機右側 90° 軸

        四機、A/B 各兩機：
          A 層：front-right、rear-left
          B 層：front-left、rear-right

        heading=0° 時，四個 slot 固定落在 NE / NW / SW / SE 四個對角方向。
        """
        clat, clon = center
        slots = {}
        self._slot_groups = {}

        heading_deg = self._enemy_heading_deg if self._enemy_heading_deg is not None else 0.0
        h = math.radians(heading_deg)

        # Local map frame: x=East, y=North
        # heading: 0°=North, 90°=East, clockwise
        fx, fy = math.sin(h), math.cos(h)
        rx, ry = math.cos(h), -math.sin(h)
        s = 1.0 / math.sqrt(2.0)

        body_dirs = {
            "front_right": ((fx + rx) * s, (fy + ry) * s),
            "front_left":  ((fx - rx) * s, (fy - ry) * s),
            "rear_left":   ((-fx - rx) * s, (-fy - ry) * s),
            "rear_right":  ((-fx + rx) * s, (-fy + ry) * s),
        }

        group_names = {
            "A": ("front_right", "rear_left"),
            "B": ("front_left", "rear_right"),
        }

        idx = 0
        for group in ("A", "B"):
            gids = self._group_ids(group)
            m = len(gids)
            if m <= 0:
                continue

            if m == 2:
                dirs = [body_dirs[name] for name in group_names[group]]
            else:
                base_deg = 45.0 if group == "A" else -45.0
                dirs = []
                for j in range(m):
                    off = math.radians(base_deg + 360.0 * j / m)
                    bx = math.sin(off)
                    by = math.cos(off)
                    dx = bx * rx + by * fx
                    dy = bx * ry + by * fy
                    dirs.append((dx, dy))

            for dx, dy in dirs:
                x = self.circle_radius * dx
                y = self.circle_radius * dy
                lat, lon = _m_to_ll(x, y, clat, clon)
                slots[idx] = (lat, lon, self._layer_alt(group))
                self._slot_groups[idx] = group
                idx += 1

        return slots

    def _lane_offset_for(self, did):
        """同一層內給每架固定的同心圓半徑車道，避免車道在飛行中交換。"""
        group = self._group_of(did)
        gids = self._group_ids(group)
        if did not in gids or len(gids) <= 1:
            return 0.0
        i = gids.index(did)
        return (i - (len(gids) - 1) / 2.0) * self.layer_lane_spacing

    # ------------------------------------------------------------------ Bezier 路徑

    def _assignment_target_angles(self, assignment, gps, center, slots):
        """
        把 slot 角度展開成與友機目前角度相同的連續角域。
        當 assignment 保持環形順序時，所有友機的角度插值全程保持順序，
        不會因為跨過 -pi/pi 邊界而突然反向或互相超車。
        """
        rows = []
        for did in self._friendly_ids:
            p = gps.get(did)
            if not p or did not in assignment:
                return None
            x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1])
            rows.append((math.atan2(y, x), did))
        rows.sort(key=lambda z: z[0])

        src = [r[0] for r in rows]
        raw_targets = []
        prev = None
        for _, did in rows:
            slot = slots[assignment[did]]
            sx, sy = _ll_to_m(slot[0], slot[1], center[0], center[1])
            th = math.atan2(sy, sx)
            if prev is not None:
                while th <= prev:
                    th += 2.0 * math.pi
            raw_targets.append(th)
            prev = th

        # 全體只加同一個 2pi 倍數，讓總旋轉量最小，同時保持拓樸順序。
        best = None
        best_err = float('inf')
        for k in range(-2, 3):
            cand = [th + k * 2.0 * math.pi for th in raw_targets]
            err = sum((cand[i] - src[i]) ** 2 for i in range(len(src)))
            if err < best_err:
                best_err, best = err, cand
        return {rows[i][1]: best[i] for i in range(len(rows))}

    def _trajectory_for(self, did, slot_index, gps, center, slots, assignment=None):
        """
        C2 平滑、近似最短路徑的 Cartesian 收斂軌跡。

        舊版使用極座標 theta/r 插值，當起點與 slot 的方位角差較大時，
        即使沒有交叉，也可能沿敵機外圍繞出很大的弧。

        新版改成：
          base(t) = P0 -> P3 的 quintic smoothstep
          offset(t) = 小幅「向外」bump

        因此主體接近兩點間的直接路徑，只保留少量發散來增加機間分離；
        crossing / safe separation 仍由全局 assignment 的硬限制負責。
        quintic smoothstep 使起終點的一、二階導數為 0，保持平滑。
        """
        p = gps.get(did)
        if not p:
            return None
        if assignment is None:
            assignment = self._assignment

        clat, clon = center
        p0 = _ll_to_m(p['lat'], p['lon'], clat, clon)

        slot = slots[slot_index]
        p3 = _ll_to_m(slot[0], slot[1], clat, clon)

        dx = p3[0] - p0[0]
        dy = p3[1] - p0[1]
        dist_to_slot = math.hypot(dx, dy)

        # 外擴方向：以敵機/包圍中心為原點，由目前位置向外。
        # 若目前位置非常接近中心，退化成與直達方向垂直的方向。
        ox, oy = _normalize(p0[0], p0[1], fallback=_normalize(-dy, dx))

        # 不再隨距離大幅外擴。
        # 最多 spread_distance（預設 3m），近距離時再自動縮小。
        bump = min(
            self.spread_distance,
            max(0.0, 0.08 * dist_to_slot),
        )

        points = []
        for i in range(self.path_samples):
            t = i / (self.path_samples - 1)
            q = _smoothstep5(t)

            # 近似直達的 C2 smoothstep 主路徑。
            bx = p0[0] + dx * q
            by = p0[1] + dy * q

            # 小幅外擴：首尾為 0，中間最大。
            # 使用 q^2(1-q)^2 讓 offset 在兩端也非常柔順。
            shape = 16.0 * q * q * (1.0 - q) * (1.0 - q)
            x = bx + ox * bump * shape
            y = by + oy * bump * shape
            points.append((x, y))

        return {
            'p0': points[0],
            'p3': points[-1],
            'points': points,
            'length': _polyline_length(points),
        }

    def _lookahead_target(self, did, slot_index, gps, center, slots):
        traj = self._trajectory_for(
            did, slot_index, gps, center, slots, assignment=self._assignment
        )
        if traj is None:
            return None

        dist_to_slot = math.hypot(
            traj['p3'][0] - traj['p0'][0],
            traj['p3'][1] - traj['p0'][1],
        )
        if dist_to_slot <= self.final_direct_distance:
            slot = slots[slot_index]
            return slot[0], slot[1], slot[2]

        # 固定「公尺」前視，比固定參數 t 更不容易因曲線長短改變而突然加減速。
        want = min(self.lookahead_distance, max(6.0, 0.70 * dist_to_slot))
        acc = 0.0
        chosen = traj['points'][-1]
        for a, b in zip(traj['points'], traj['points'][1:]):
            seg = math.hypot(b[0] - a[0], b[1] - a[1])
            if acc + seg >= want and seg > 1e-9:
                u = (want - acc) / seg
                chosen = (a[0] + u * (b[0] - a[0]), a[1] + u * (b[1] - a[1]))
                break
            acc += seg

        lat, lon = _m_to_ll(chosen[0], chosen[1], center[0], center[1])
        cur_alt = self._position_alt(gps[did])
        if cur_alt is None:
            return None
        target_alt = slots[slot_index][2]
        # 高度用距離比例平滑，但近場直接交給 slot。
        frac = min(1.0, want / max(1e-6, traj['length']))
        q = _smoothstep5(frac)
        alt = cur_alt + q * (target_alt - cur_alt)
        return lat, lon, max(self.min_alt_floor, alt)

    def _converge_targets(self, gps, center, slots, assignment):
        targets = {}
        for did in self._friendly_ids:
            sidx = assignment.get(did)
            if sidx is None:
                return None
            target = self._lookahead_target(did, sidx, gps, center, slots)
            if target is None:
                return None
            targets[did] = target
        return targets

    def _separation_correction_xy(self, did, gps, ref_lat, ref_lon):
        """目前太靠近其他友機時，計算小幅排斥修正；不取代前進目標，只疊加在其上。"""
        p = gps.get(did)
        if not p:
            return 0.0, 0.0
        x, y = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon)
        cx = cy = 0.0
        my_group = self._group_of(did)
        for j, oid in enumerate(self._friendly_ids):
            if oid == did or self._group_of(oid) != my_group:
                continue
            op = gps.get(oid)
            if not op:
                continue
            ox, oy = _ll_to_m(op['lat'], op['lon'], ref_lat, ref_lon)
            dx, dy = x - ox, y - oy
            d = math.hypot(dx, dy)
            # 8m 是硬安全門檻，但隊形不等到 8m 才開始整理；
            # 在 formation_spacing（預設 12m）內就逐步往外展開。
            if d >= self.formation_spacing:
                continue
            if d < 1e-6:
                # 完全重合時用固定但每機不同的方向，避免除零。
                idx = self._friendly_ids.index(did)
                th = 2.0 * math.pi * idx / max(1, len(self._friendly_ids))
                ux, uy = math.cos(th), math.sin(th)
                d = 0.0
            else:
                ux, uy = dx / d, dy / d
            # 以「隊形希望間距」而非硬安全距離算修正，因此會更早開始展開。
            deficit = self.formation_spacing - d
            closeness = deficit / max(0.1, self.formation_spacing)
            boost = 1.0 + 0.7 * closeness
            mag = 0.5 * self.separation_correction_gain * deficit * boost
            cx += ux * mag
            cy += uy * mag

        n = math.hypot(cx, cy)
        if n > self.separation_correction_max > 0.0:
            k = self.separation_correction_max / n
            cx *= k
            cy *= k
        return cx, cy

    def _segments_cross_for_targets(self, gps, targets, ref_lat, ref_lon):
        """只檢查『當前位置 -> 下一個 lookahead』短線段是否彼此相交。"""
        ids = [d for d in self._friendly_ids if d in targets and gps.get(d)]
        segs = {}
        for did in ids:
            p = gps[did]
            a = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon)
            t = targets[did]
            b = _ll_to_m(t[0], t[1], ref_lat, ref_lon)
            segs[did] = (a, b)
        for i in range(len(ids)):
            for j in range(i + 1, len(ids)):
                if self._group_of(ids[i]) != self._group_of(ids[j]):
                    continue
                if _segments_intersect(segs[ids[i]][0], segs[ids[i]][1],
                                       segs[ids[j]][0], segs[ids[j]][1]):
                    return True
        return False

    def _layer_conflict_flags(self, gps, center, slots, assignment):
        """判斷各高度層是否需要啟用同心圓車道。跨層完全不互相觸發。"""
        flags = {"A": False, "B": False}
        ref_lat, ref_lon = center
        for group in ("A", "B"):
            gids = [d for d in self._group_ids(group) if d in assignment and gps.get(d)]
            for i in range(len(gids)):
                di = gids[i]
                pi = gps[di]
                aix = _ll_to_m(pi['lat'], pi['lon'], ref_lat, ref_lon)
                si = slots[assignment[di]]
                bix = _ll_to_m(si[0], si[1], ref_lat, ref_lon)
                for j in range(i + 1, len(gids)):
                    dj = gids[j]
                    pj = gps[dj]
                    ajx = _ll_to_m(pj['lat'], pj['lon'], ref_lat, ref_lon)
                    sj = slots[assignment[dj]]
                    bjx = _ll_to_m(sj[0], sj[1], ref_lat, ref_lon)
                    d = math.hypot(aix[0] - ajx[0], aix[1] - ajx[1])
                    if d < self.layer_avoid_distance or _segments_intersect(aix, bix, ajx, bjx):
                        flags[group] = True
                        break
                if flags[group]:
                    break
        return flags

    def _concentric_lane_carrot(self, did, p, slot, center, dist_to_slot):
        """
        同層避讓用的同心圓 carrot。
        - 每架固定一條暫時半徑車道。
        - 沿角向前進，同時平滑靠近該車道半徑。
        - 接近最終 slot 時 lane offset 淡出為 0，回到共同包圍半徑。
        """
        ref_lat, ref_lon = center
        px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon)
        sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon)
        r = max(0.5, math.hypot(px, py))
        th = math.atan2(py, px)
        sth = math.atan2(sy, sx)
        dth = _wrap_pi(sth - th)

        # 遠處完整使用分層車道；進入 slot 前逐步收回共同半徑。
        fade = min(1.0, max(0.0,
            (dist_to_slot - self.final_direct_distance) /
            max(1.0, self.layer_lane_fade_distance - self.final_direct_distance)
        ))
        lane_r = max(2.0, self.circle_radius + self._lane_offset_for(did) * fade)

        horizon = min(self.lookahead_distance, max(4.0, dist_to_slot))
        # 約 70% 用於角向前進、30% 用於半徑收斂，避免突然徑向橫切。
        arc_budget = 0.72 * horizon
        radial_budget = 0.35 * horizon
        th_step = min(abs(dth), arc_budget / max(2.0, r))
        nth = th + (1.0 if dth >= 0.0 else -1.0) * th_step
        dr = max(-radial_budget, min(radial_budget, lane_r - r))
        nr = max(2.0, r + dr)

        tx = nr * math.cos(nth)
        ty = nr * math.sin(nth)
        return tx, ty

    def _moving_converge_targets(self, gps, center, slots, assignment):
        """
        A/B 分層持續移動控制：
        - A/B 高度層跟隨敵機即時高度。
        - 跨層近距離且垂直間隔不足時，先鎖住水平位置完成分層。
        - 同層若過近或直達線可能交叉，切換到不同半徑的同心圓車道。
        - 非跨層垂直分離狀態時，持續保持前方 carrot。
        """
        ref_lat, ref_lon = center
        layer_avoid = self._layer_conflict_flags(gps, center, slots, assignment)
        raw = {}

        for did in self._friendly_ids:
            p = gps.get(did)
            sidx = assignment.get(did)
            if not p or sidx is None:
                return None
            slot = slots[sidx]
            px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon)
            sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon)
            vx, vy = sx - px, sy - py
            dist = math.hypot(vx, vy)

            if dist <= self.final_direct_distance:
                raw[did] = (slot[0], slot[1], self._layer_alt(self._group_of(did)))
                continue

            group = self._group_of(did)
            if layer_avoid.get(group, False):
                tx, ty = self._concentric_lane_carrot(did, p, slot, center, dist)
                dux, duy = _normalize(tx - px, ty - py)
            else:
                dux, duy = _normalize(vx, vy)
                horizon = min(self.lookahead_distance, dist)
                tx, ty = px + dux * horizon, py + duy * horizon

            # 同層近距離排斥仍保留，但只取側向，不會抵消前進分量。
            rx, ry = self._separation_correction_xy(did, gps, ref_lat, ref_lon)
            dot = rx * dux + ry * duy
            lx, ly = rx - dot * dux, ry - dot * duy
            ln = math.hypot(lx, ly)
            cap = min(self.separation_correction_max,
                      self.max_lateral_ratio * self.lookahead_distance)
            if ln > cap > 0.0:
                k = cap / ln
                lx *= k; ly *= k
            tx += lx
            ty += ly

            ndx, ndy = _normalize(tx - px, ty - py, fallback=(dux, duy))
            prev = self._motion_dirs.get(did)
            if prev is not None:
                a = self.direction_smooth_alpha
                ndx, ndy = _normalize(prev[0]*(1-a)+ndx*a,
                                      prev[1]*(1-a)+ndy*a,
                                      fallback=(ndx, ndy))
                h = min(self.lookahead_distance, max(4.0, dist))
                tx, ty = px + ndx*h, py + ndy*h
            self._motion_dirs[did] = (ndx, ndy)

            lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon)
            target_alt = self._layer_alt(group)
            cur_alt = self._position_alt(p)
            if cur_alt is None:
                return None
            alt = cur_alt + 0.35 * (target_alt - cur_alt)
            raw[did] = (lat, lon, max(self.min_alt_floor, alt))

        return self._apply_cross_layer_vertical_gate(raw, gps)

    def _recovery_spread_targets(self, gps):
        """
        找不到安全 slot assignment 時的短距離預分散。

        每架只沿「友軍形心 -> 自己」方向往外移 recovery_spread_step；
        不繞敵機、不做大圓，只把原本過密的起始幾何稍微拉開。
        """
        pts = []
        for did in self._friendly_ids:
            p = gps.get(did)
            if not p:
                return None
            pts.append((p['lat'], p['lon']))
        if not pts:
            return None

        ref_lat = sum(p[0] for p in pts) / len(pts)
        ref_lon = sum(p[1] for p in pts) / len(pts)
        xy = {}
        for did in self._friendly_ids:
            p = gps[did]
            xy[did] = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon)
        cx = sum(v[0] for v in xy.values()) / len(xy)
        cy = sum(v[1] for v in xy.values()) / len(xy)

        targets = {}
        n = len(self._friendly_ids)
        for idx, did in enumerate(self._friendly_ids):
            x, y = xy[did]
            ox, oy = x - cx, y - cy
            if math.hypot(ox, oy) < 1e-6:
                th = 2.0 * math.pi * idx / max(1, n)
                ox, oy = math.cos(th), math.sin(th)
            ox, oy = _normalize(ox, oy)
            tx = x + ox * self.recovery_spread_step
            ty = y + oy * self.recovery_spread_step
            lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon)
            current_alt = self._position_alt(gps[did])
            if current_alt is None:
                return None
            alt = max(self.min_alt_floor, current_alt)
            targets[did] = (lat, lon, alt)
        return targets

    # ------------------------------------------------------------------ assignment / crossing / separation

    def _assignment_cost(self, assignment, gps, center, slots):
        """
        評估一個完整 assignment。

        crossing 仍是硬限制。水平間距改成「漸進安全」：
        - 若兩機起始已 >= safe_separation，後續不得低於 safe_separation。
        - 若起始本來 < safe_separation，不要求瞬間跳到 8m；只要求路徑不要
          比起始距離再惡化超過 initial_sep_tolerance，並且永遠不得低於
          emergency_separation。

        這避免四機原本只隔 4~7m 時，所有 assignment 在第一個 sample 就被
        8m 硬門檻全部淘汰。
        """
        trajectories = {}
        total = 0.0

        for did in self._friendly_ids:
            sidx = assignment[did]
            traj = self._trajectory_for(did, sidx, gps, center, slots, assignment=assignment)
            if traj is None:
                return float('inf'), True, 0, 0.0
            trajectories[did] = traj
            total += traj['length']

            old = self._assignment.get(did)
            if old is not None and old != sidx:
                total += self.slot_switch_penalty

        crossing_count = 0
        separation_violation = False
        min_sep = float('inf')
        ids = self._friendly_ids

        for i in range(len(ids)):
            for j in range(i + 1, len(ids)):
                # A/B 高度層不同：只對同層做水平 crossing / separation 約束。
                if self._group_of(ids[i]) != self._group_of(ids[j]):
                    continue
                a = trajectories[ids[i]]['points']
                b = trajectories[ids[j]]['points']

                # 幾何 crossing：從頭到尾都不允許真正穿越。
                crossed = False
                for a0, a1 in zip(a, a[1:]):
                    if crossed:
                        break
                    for b0, b1 in zip(b, b[1:]):
                        if _segments_intersect(a0, a1, b0, b1):
                            crossed = True
                            break
                if crossed:
                    crossing_count += 1
                    total += self.crossing_penalty

                # 起始實際水平距離。
                initial_sep = math.hypot(a[0][0] - b[0][0], a[0][1] - b[0][1])

                # 已經足夠遠：維持完整 8m；本來較近：不能再明顯靠得更近。
                if initial_sep >= self.safe_separation:
                    required_sep = self.safe_separation
                else:
                    required_sep = max(
                        self.emergency_separation,
                        initial_sep - self.initial_sep_tolerance,
                    )

                # 同 normalized t 的預測水平距離。略過 t=0，因為那是既成狀態。
                pair_min = float('inf')
                for k in range(1, min(len(a), len(b))):
                    d = math.hypot(a[k][0] - b[k][0], a[k][1] - b[k][1])
                    pair_min = min(pair_min, d)
                min_sep = min(min_sep, pair_min)

                if pair_min < required_sep:
                    separation_violation = True
                    ratio = (required_sep - pair_min) / max(0.1, required_sep)
                    total += self.separation_penalty * (1.0 + ratio)

        if min_sep == float('inf'):
            min_sep = 9999.0

        conflict = crossing_count > 0 or separation_violation
        return total, conflict, crossing_count, min_sep

    def _cyclic_assignments(self, gps, center, slots):
        """A/B 各自在自己的高度層做 cyclic slot rotation，再取笛卡兒積組合。"""
        per_group = []
        for group in ("A", "B"):
            ids = self._group_ids(group)
            if not ids:
                continue
            rows = []
            for did in ids:
                p = gps.get(did)
                if not p:
                    return []
                x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1])
                rows.append((math.atan2(y, x), did))
            rows.sort(key=lambda z: z[0])
            ordered_ids = [d for _, d in rows]

            slot_rows = []
            for sidx, sp in slots.items():
                if self._slot_groups.get(sidx) != group:
                    continue
                x, y = _ll_to_m(sp[0], sp[1], center[0], center[1])
                slot_rows.append((math.atan2(y, x), sidx))
            slot_rows.sort(key=lambda z: z[0])
            ordered_slots = [s for _, s in slot_rows]
            if len(ordered_slots) != len(ordered_ids):
                return []

            group_candidates = []
            m = len(ordered_ids)
            for shift in range(m):
                group_candidates.append({
                    ordered_ids[i]: ordered_slots[(i + shift) % m]
                    for i in range(m)
                })
            per_group.append(group_candidates)

        if not per_group:
            return []
        out = []
        for combo in product(*per_group):
            merged = {}
            for part in combo:
                merged.update(part)
            out.append(merged)
        return out

    def _group_permutation_assignments(self, slots):
        """只允許 A->A slots、B->B slots；小隊內可做完整 permutation 保底。"""
        per_group = []
        for group in ("A", "B"):
            ids = self._group_ids(group)
            if not ids:
                continue
            sidxs = [s for s in slots if self._slot_groups.get(s) == group]
            if len(ids) != len(sidxs):
                return []
            if len(ids) > 6:
                return []
            gc = []
            for perm in permutations(sidxs):
                gc.append({did: perm[i] for i, did in enumerate(ids)})
            per_group.append(gc)
        if not per_group:
            return []
        out=[]
        for combo in product(*per_group):
            d={}
            for part in combo: d.update(part)
            out.append(d)
        return out

    def _choose_assignment(self, gps, center, slots, force=False):
        """
        Crossing 是硬限制，不再只是 penalty。

        優先搜尋保持環形順序的 n 個 rotation；只有找不到安全解時才擴大到全部
        permutation。任何 crossings > 0 或違反『漸進安全間距』的方案都不採用。
        若目前 assignment 仍安全就鎖住，不為了省幾公尺頻繁換位。
        """
        ids = list(self._friendly_ids)
        n = len(ids)
        bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')}
        if n == 0 or len(slots) != n:
            return None, bad

        # 一旦開始移動，只要目前 assignment 的「幾何路徑沒有交叉」就鎖定。
        # 暫時的間距不足交給 moving separation correction 處理，不因此突然換 slot。
        # 這可避免飛行途中目標位置跳到另一側而先煞停再重新排隊。
        if self._assignment and not force:
            cost, conflict, crossings, min_sep = self._assignment_cost(
                self._assignment, gps, center, slots
            )
            if crossings == 0:
                return dict(self._assignment), {
                    'cost': cost, 'conflict': conflict,
                    'crossings': 0, 'min_sep': min_sep,
                }

        candidates = self._cyclic_assignments(gps, center, slots)
        # cyclic 不足時只擴大到「各層內」的 permutation，不允許 A/B 交換高度層。
        all_perm = self._group_permutation_assignments(slots)
        seen = {tuple(c.get(d) for d in ids) for c in candidates}
        candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen]

        valid = []
        for assignment in candidates:
            cost, conflict, crossings, min_sep = self._assignment_cost(
                assignment, gps, center, slots
            )
            # 幾何不交叉 + 漸進安全間距，兩者都是硬條件。
            if crossings == 0 and not conflict:
                valid.append((cost, -min_sep, assignment, {
                    'cost': cost, 'conflict': False,
                    'crossings': 0, 'min_sep': min_sep,
                }))

        if not valid:
            return None, bad
        valid.sort(key=lambda x: (x[0], x[1]))
        return valid[0][2], valid[0][3]

    def _choose_progress_assignment(self, gps, center, slots):
        """
        找不到完整安全間距方案時的移動中保底。

        crossing 仍是硬限制：只從 0-crossing assignment 中選最低風險方案。
        允許暫時低於 safe_separation，之後由 moving separation correction
        在持續前進的同時把距離拉開。
        """
        ids = list(self._friendly_ids)
        n = len(ids)
        bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')}
        if n == 0 or len(slots) != n:
            return None, bad

        candidates = self._cyclic_assignments(gps, center, slots)
        all_perm = self._group_permutation_assignments(slots)
        seen = {tuple(c.get(d) for d in ids) for c in candidates}
        candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen]

        valid = []
        for assignment in candidates:
            cost, conflict, crossings, min_sep = self._assignment_cost(
                assignment, gps, center, slots
            )
            if crossings != 0:
                continue
            # 先偏好較大的預測最小間距，再看路徑成本；不接受幾何交叉。
            valid.append((-min_sep, cost, assignment, {
                'cost': cost,
                'conflict': conflict,
                'crossings': 0,
                'min_sep': min_sep,
            }))

        if not valid:
            return None, bad
        valid.sort(key=lambda x: (x[0], x[1]))
        return valid[0][2], valid[0][3]

    def _greedy_assignment(self, gps, center, slots):
        """大於 7 機時的保底 nearest-slot greedy。"""
        remain = set(slots.keys())
        assignment = {}
        for did in self._friendly_ids:
            p = gps.get(did)
            if not p:
                continue
            best_s = min(
                remain,
                key=lambda s: _haversine(
                    p['lat'], p['lon'], slots[s][0], slots[s][1]
                ),
            )
            assignment[did] = best_s
            remain.remove(best_s)
        return assignment

    # ------------------------------------------------------------------ command 更新

    def _update_converge_command(self, gps, epos, force_assignment=False):
        if self._merged_ex is None:
            return

        center = self._planning_center(gps, epos)
        slots = self._circle_slots_for_center(center, len(self._friendly_ids))
        new_assignment, info = self._choose_assignment(
            gps, center, slots, force=force_assignment
        )
        if not new_assignment:
            # 若任務已經在移動中，優先沿用既有 slot，不在半路 hold。
            # 只有「一開始尚未建立任何 assignment」時才找 provisional 0-crossing 配置。
            if self._assignment:
                new_assignment = dict(self._assignment)
                _, conflict, crossings, min_sep = self._assignment_cost(
                    new_assignment, gps, center, slots
                )
                info = {
                    'cost': 0.0, 'conflict': conflict,
                    'crossings': crossings, 'min_sep': min_sep,
                }
            else:
                new_assignment, info = self._choose_progress_assignment(gps, center, slots)
                if not new_assignment:
                    # 尚未開始飛行時若連 0-crossing topology 都沒有，才暫停。
                    self._hold_current_positions(gps)
                    self.status_message.emit("起始暫停：目前連 0-crossing 路徑都不存在")
                    return

        changed = new_assignment != self._assignment
        if changed:
            self._assignment = new_assignment
            self._last_assignment_change_at = time.monotonic()
            msg = (
                f"slot 重新分配：cross={info['crossings']}，"
                f"預測最小間距={info['min_sep']:.1f} m"
            )
            _log("INFO", f"[orchestrator] {msg}")
            self.status_message.emit(msg)

        self._last_assignment_had_conflict = info['conflict']
        targets = self._moving_converge_targets(gps, center, slots, self._assignment)
        if targets is None:
            return

        self._merged_ex.update_plan(self._targets_to_plan(targets))
        self._last_predicted_center = center
        self._last_slots = slots
        self._mark_replanned(epos)

    def _update_encircle_command(self, gps, epos, force=False):
        if self._merged_ex is None:
            return
        now = time.monotonic()
        if not force and not self._should_replan(now, epos):
            return

        center = self._planning_center(gps, epos)
        slots = self._circle_slots_for_center(center, len(self._friendly_ids))

        # ENCIRCLE 內原則上不交換 slot，以免四機繞圈互換造成穿越。
        # 只有現有 assignment 已無效才重新建立。
        if not self._assignment or any(d not in self._assignment for d in self._friendly_ids):
            assignment, _ = self._choose_assignment(gps, center, slots, force=True)
            if assignment:
                self._assignment = assignment
                self._last_assignment_change_at = now

        targets = {
            did: slots[self._assignment[did]]
            for did in self._friendly_ids
            if did in self._assignment
        }
        if len(targets) != len(self._friendly_ids):
            return

        targets = self._apply_cross_layer_vertical_gate(targets, gps)

        self._merged_ex.update_plan(self._targets_to_plan(targets))
        self._last_predicted_center = center
        self._last_slots = slots
        self._mark_replanned(epos)

    def _targets_to_plan(self, targets):
        return {
            'drone_ids': list(self._friendly_ids),
            'waypoints': [[targets[d]] for d in self._friendly_ids],
            'rendezvous_indices': [],
        }

    # ------------------------------------------------------------------ 包圍完成判斷

    def _all_in_slots(self, gps, epos, tolerance=None):
        if not self._assignment or not self._friendly_ids:
            return False
        tolerance = self.encircle_tolerance if tolerance is None else float(tolerance)

        # 水平 slot 即使已到達，只要近距離 A/B 尚未建立足夠垂直差，仍不可
        # 宣告包圍完成或提前解除「先垂直、後水平」保護。
        if self._cross_layer_vertical_hold_ids(gps):
            return False

        center = self._last_predicted_center or self._predict_enemy_center(gps, epos)
        slots = self._circle_slots_for_center(center, len(self._friendly_ids))

        for did in self._friendly_ids:
            p = gps.get(did)
            sidx = self._assignment.get(did)
            if not p or sidx is None:
                return False
            slot = slots[sidx]
            d = _haversine(p['lat'], p['lon'], slot[0], slot[1])
            if d > tolerance:
                return False
            current_alt = self._position_alt(p)
            if (current_alt is None
                    or abs(current_alt - slot[2]) > self.cross_layer_alt_tolerance):
                return False
        return True

    # ------------------------------------------------------------------ replan

    def _mark_replanned(self, epos):
        self._last_replan_at = time.monotonic()
        self._last_plan_enemy = (epos['lat'], epos['lon'])

    def _should_replan(self, now, epos):
        if self._last_plan_enemy is None:
            return True
        moved = _haversine(
            self._last_plan_enemy[0], self._last_plan_enemy[1],
            epos['lat'], epos['lon']
        )
        return (
            moved >= self.replan_move_m
            or (now - self._last_replan_at) >= self.replan_cap_sec
        )

    # ------------------------------------------------------------------ helpers

    def _positions_for(self, drone_ids, gps):
        out = []
        now = time.monotonic()
        for did in drone_ids:
            p = gps.get(did)
            if not p or not self._position_is_fresh(p, now):
                return None
            try:
                lat = float(p['lat'])
                lon = float(p['lon'])
            except (KeyError, TypeError, ValueError):
                return None
            alt = self._position_alt(p)
            if not math.isfinite(lat) or not math.isfinite(lon) or alt is None:
                return None
            out.append((lat, lon, alt))
        return out

    @staticmethod
    def _unique_keep_order(items):
        seen = set()
        out = []
        for x in items:
            if x in seen:
                continue
            seen.add(x)
            out.append(x)
        return out

    def _set_phase(self, phase):
        if phase == self.phase:
            return
        self.phase = phase
        _log("INFO", f"[orchestrator] phase -> {phase.value}")
        self.phase_changed.emit(phase.value)


# ---------------------------------------------------------------------------
# Engagement Panel

class EngagementPanel(QWidget):
    """
    交戰控制面板。

    - 敵機下拉 + 即時敵機到最近友機距離
    - A/B 僅用於偵察前的群組指定
    - 偵測確認後由 MissionOrchestrator 合併為無角色 friendly 集合
    - 速度比例 UI 保留，方便既有 GUI 發 DO_CHANGE_SPEED
    """

    enemy_changed = pyqtSignal(str)
    confirm_requested = pyqtSignal()
    stop_requested = pyqtSignal()
    groups_changed = pyqtSignal(str, str)
    apply_speed_requested = pyqtSignal(float, float)

    def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT,
                 friendly_speed=DEFAULT_FRIENDLY_SPEED):
        super().__init__(parent)
        self.r_detect = float(r_detect)
        self._init_friendly_speed = float(friendly_speed)
        self._enemy_id = ""
        self._suppress_enemy = False
        self._suppress_group = False
        self._group_sig = None
        self._build_ui()

    # ------------------------------------------------------------------ UI

    def _build_ui(self):
        root = QVBoxLayout(self)
        root.setContentsMargins(12, 12, 12, 12)
        root.setSpacing(10)
        root.setAlignment(Qt.AlignmentFlag.AlignTop)

        title = QLabel("交戰設定")
        title.setStyleSheet(
            "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;"
        )
        root.addWidget(title)

        mode_hint = QLabel(
            "偵測確認後：A/B 跟隨敵機高度分層；近距離先垂直後水平，"
            "同層以同心圓路徑避讓"
        )
        mode_hint.setWordWrap(True)
        mode_hint.setStyleSheet("color: #888; font-size: 11px;")
        root.addWidget(mode_hint)

        combo_css = (
            "QComboBox { background-color: #333; color: #EEE; border: 1px solid #555;"
            " border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }"
        )

        enemy_row = QHBoxLayout()
        enemy_row.setSpacing(8)
        el = QLabel("敵機 sysid：")
        el.setStyleSheet("color: #CCC; font-size: 12px;")
        self.enemy_combo = QComboBox()
        self.enemy_combo.setStyleSheet(combo_css)
        self.enemy_combo.addItem(_NONE_LABEL)
        self.enemy_combo.currentTextChanged.connect(self._on_enemy_changed)
        enemy_row.addWidget(el)
        enemy_row.addWidget(self.enemy_combo)
        enemy_row.addStretch()
        root.addLayout(enemy_row)

        a_row = QHBoxLayout()
        a_row.setSpacing(8)
        al = QLabel("偵察組 A：")
        al.setStyleSheet("color: #CCC; font-size: 12px;")
        self.a_group_combo = QComboBox()
        self.a_group_combo.setStyleSheet(combo_css)
        self.a_group_combo.addItem(_NONE_LABEL, None)
        self.a_group_combo.currentTextChanged.connect(self._on_group_changed)
        a_row.addWidget(al)
        a_row.addWidget(self.a_group_combo)
        a_row.addStretch()
        root.addLayout(a_row)

        b_row = QHBoxLayout()
        b_row.setSpacing(8)
        bl = QLabel("待機組 B：")
        bl.setStyleSheet("color: #CCC; font-size: 12px;")
        self.b_group_combo = QComboBox()
        self.b_group_combo.setStyleSheet(combo_css)
        self.b_group_combo.addItem(_NONE_LABEL, None)
        self.b_group_combo.currentTextChanged.connect(self._on_group_changed)
        b_row.addWidget(bl)
        b_row.addWidget(self.b_group_combo)
        b_row.addStretch()
        root.addLayout(b_row)

        spin_css = (
            "QDoubleSpinBox { background-color: #333; color: #EEE;"
            " border: 1px solid #555; border-radius: 3px; padding: 2px 4px;"
            " font-size: 12px; min-width: 70px; }"
        )
        speed_row = QHBoxLayout()
        speed_row.setSpacing(8)
        sl = QLabel("友機速度：")
        sl.setStyleSheet("color: #CCC; font-size: 12px;")
        self.friendly_speed_spin = QDoubleSpinBox()
        self.friendly_speed_spin.setStyleSheet(spin_css)
        self.friendly_speed_spin.setRange(0.5, 20.0)
        self.friendly_speed_spin.setSingleStep(0.5)
        self.friendly_speed_spin.setDecimals(1)
        self.friendly_speed_spin.setSuffix(" m/s")
        self.friendly_speed_spin.setValue(self._init_friendly_speed)

        rl = QLabel("敵/友比例：")
        rl.setStyleSheet("color: #CCC; font-size: 12px;")
        self.enemy_ratio_spin = QDoubleSpinBox()
        self.enemy_ratio_spin.setStyleSheet(spin_css)
        self.enemy_ratio_spin.setRange(0.1, 1.0)
        self.enemy_ratio_spin.setSingleStep(0.05)
        self.enemy_ratio_spin.setDecimals(2)
        self.enemy_ratio_spin.setValue(0.6)

        speed_row.addWidget(sl)
        speed_row.addWidget(self.friendly_speed_spin)
        speed_row.addWidget(rl)
        speed_row.addWidget(self.enemy_ratio_spin)
        speed_row.addStretch()
        root.addLayout(speed_row)

        apply_speed_row = QHBoxLayout()
        self.apply_speed_btn = QPushButton("套用速度")
        self.apply_speed_btn.setStyleSheet(
            "QPushButton { background-color: #00796B; color: white; border: none;"
            " padding: 6px 14px; border-radius: 4px; font-weight: bold; }"
            " QPushButton:hover { background-color: #00897B; }"
        )
        self.apply_speed_btn.clicked.connect(self._on_apply_speed)
        self.enemy_speed_hint = QLabel("")
        self.enemy_speed_hint.setStyleSheet("color: #777; font-size: 11px;")
        self._refresh_enemy_speed_hint()
        self.friendly_speed_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
        self.enemy_ratio_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
        apply_speed_row.addWidget(self.apply_speed_btn)
        apply_speed_row.addWidget(self.enemy_speed_hint)
        apply_speed_row.addStretch()
        root.addLayout(apply_speed_row)

        sep = QFrame()
        sep.setFrameShape(QFrame.Shape.HLine)
        sep.setStyleSheet("color: #444;")
        root.addWidget(sep)

        self.distance_label = QLabel("敵機 ↔ 最近友機：—")
        self.distance_label.setStyleSheet(
            "color: #AAA; font-size: 13px; padding: 4px 2px;"
        )
        root.addWidget(self.distance_label)

        hint = QLabel(f"（偵測範圍 R_detect = {self.r_detect:.0f} m）")
        hint.setStyleSheet("color: #777; font-size: 11px;")
        root.addWidget(hint)

        self.phase_label = QLabel("階段：待機")
        self.phase_label.setStyleSheet(
            "color: #64B5F6; font-size: 13px; font-weight: bold; padding: 4px 2px;"
        )
        root.addWidget(self.phase_label)

        btn_css = (
            "QPushButton {{ background-color: {bg}; color: {fg}; border: none;"
            " padding: 8px 16px; border-radius: 4px; font-weight: bold; }}"
            " QPushButton:hover {{ background-color: {hover}; }}"
            " QPushButton:disabled {{ background-color: #444; color: #888; }}"
        )
        btn_row = QHBoxLayout()
        btn_row.setSpacing(8)

        self.confirm_btn = QPushButton("開始動態包圍")
        self.confirm_btn.setStyleSheet(
            btn_css.format(bg='#F57C00', fg='white', hover='#FB8C00')
        )
        self.confirm_btn.clicked.connect(lambda: self.confirm_requested.emit())
        self.confirm_btn.setEnabled(False)

        self.stop_btn = QPushButton("停止")
        self.stop_btn.setStyleSheet(
            btn_css.format(bg='#555', fg='#DDD', hover='#666')
        )
        self.stop_btn.clicked.connect(lambda: self.stop_requested.emit())

        btn_row.addWidget(self.confirm_btn)
        btn_row.addWidget(self.stop_btn)
        btn_row.addStretch()
        root.addLayout(btn_row)

    # ------------------------------------------------------------------ 對外 API

    def current_enemy(self):
        return self._enemy_id or None

    def refresh_drone_list(self, drone_ids):
        items = [_NONE_LABEL] + sorted(drone_ids)
        current = self._enemy_id if self._enemy_id in drone_ids else ""
        self._suppress_enemy = True
        self.enemy_combo.clear()
        self.enemy_combo.addItems(items)
        idx = self.enemy_combo.findText(current) if current else 0
        self.enemy_combo.setCurrentIndex(idx if idx >= 0 else 0)
        self._suppress_enemy = False

        if current != self._enemy_id:
            self._enemy_id = current
            self.enemy_changed.emit(self._enemy_id)

    def refresh_group_list(self, group_labels):
        """group_labels: list[(group_id, display_text)]。"""
        sig = tuple(group_labels)
        if sig == self._group_sig:
            return
        self._group_sig = sig
        self._suppress_group = True

        for combo in (self.a_group_combo, self.b_group_combo):
            prev = combo.currentData()
            combo.clear()
            combo.addItem(_NONE_LABEL, None)
            for gid, text in group_labels:
                combo.addItem(text, gid)
            if prev is not None:
                i = combo.findData(prev)
                combo.setCurrentIndex(i if i >= 0 else 0)

        self._suppress_group = False

    def selected_group_ids(self):
        return self.a_group_combo.currentData(), self.b_group_combo.currentData()

    def set_configuration(self, enemy_id, a_group_id, b_group_id):
        """由 GUI 程式化選定敵機與 A/B 群組，並發出一次同步訊號。"""
        enemy_index = self.enemy_combo.findText(enemy_id)
        a_index = self.a_group_combo.findData(a_group_id)
        b_index = self.b_group_combo.findData(b_group_id)
        if min(enemy_index, a_index, b_index) < 0:
            return False

        self._suppress_enemy = True
        self._suppress_group = True
        try:
            self.enemy_combo.setCurrentIndex(enemy_index)
            self.a_group_combo.setCurrentIndex(a_index)
            self.b_group_combo.setCurrentIndex(b_index)
            self._enemy_id = enemy_id
        finally:
            self._suppress_enemy = False
            self._suppress_group = False

        self.enemy_changed.emit(enemy_id)
        self.groups_changed.emit(a_group_id, b_group_id)
        return True

    def set_phase_text(self, text):
        self.phase_label.setText(f"階段：{text}")

    def set_confirm_enabled(self, enabled):
        self.confirm_btn.setEnabled(enabled)

    def tick(self, drone_positions):
        """drone_positions: {drone_id: (lat, lon)}。"""
        enemy = self._enemy_id
        base = "color: #AAA; font-size: 13px; padding: 4px 2px;"

        if not enemy:
            self.distance_label.setText("敵機 ↔ 最近友機：（尚未選擇敵機）")
            self.distance_label.setStyleSheet(base)
            return

        epos = drone_positions.get(enemy)
        if not epos:
            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（等待敵機定位）")
            self.distance_label.setStyleSheet(base)
            return

        nearest_id = None
        nearest_d = None
        for did, pos in drone_positions.items():
            if did == enemy or not pos:
                continue
            d = _haversine(epos[0], epos[1], pos[0], pos[1])
            if nearest_d is None or d < nearest_d:
                nearest_id, nearest_d = did, d

        if nearest_id is None:
            self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（無友機定位）")
            self.distance_label.setStyleSheet(base)
            return

        color = "#E57373" if nearest_d <= self.r_detect else "#81C784"
        self.distance_label.setText(
            f"敵機 {enemy} ↔ 最近友機 {nearest_id}：{nearest_d:.1f} m"
        )
        self.distance_label.setStyleSheet(
            f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;"
        )

    # ------------------------------------------------------------------ UI callbacks

    def _on_enemy_changed(self, text):
        if self._suppress_enemy:
            return
        self._enemy_id = "" if text == _NONE_LABEL else text
        _log("INFO", f"[交戰] 敵機選擇：{self._enemy_id or '（未選擇）'}")
        self.enemy_changed.emit(self._enemy_id)

    def _on_group_changed(self, _text):
        if self._suppress_group:
            return
        a_id, b_id = self.selected_group_ids()
        self.groups_changed.emit(a_id or "", b_id or "")

    def _refresh_enemy_speed_hint(self, *_):
        v = self.friendly_speed_spin.value()
        r = self.enemy_ratio_spin.value()
        self.enemy_speed_hint.setText(
            f"→ 友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s"
        )

    def _on_apply_speed(self):
        v = self.friendly_speed_spin.value()
        r = self.enemy_ratio_spin.value()
        _log(
            "INFO",
            f"[交戰] 套用速度：友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s (比例 {r:.2f})"
        )
        self.apply_speed_requested.emit(v, r)