"""los_consistency.py —— 吊舱角度「运动学一致性」校验（替代旧的 require_vision）。

为什么需要它
------------
吊舱协议有两处已知缺陷（部署文档 §10）：
  * bit2 锁定位在目标丢失后**不复位**；
  * 脱靶量没有无效标志位，**丢失时表现为 0**。
只信 bit2 会让飞机朝"过期坐标/幽灵目标"继续飞。

原设计用「我方双目视觉」作第二判据（`require_vision`）。但后续决定**吊舱取代
双目 + YOLO 成为唯一目标源**，于是"我方视觉"不再存在：该判据要么恒 False
（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。

本模块给出**不依赖任何额外传感器**的替代判据：
    本吊舱无电机、框架刚性固连机体 → 相机光轴随刚体转动。
    因此目标在世界系静止时，机头转 Δψ，脱靶量必须变化约 −Δψ。
    若机头已经明显转动（|Δψ| ≥ 阈值）而脱靶量几乎不动 → 角度冻结/幽灵，
    判为不一致（fail-safe：当丢目标处理）。

判别量：`residual = Δβ + Δψ`（β 为右正视线角，ψ 为 NED 航向，均取度）。
  * 一致：|residual| ≤ max_residual_deg（目标在世界系缓慢移动也会落在余量内）
  * 不一致：|residual| 超限且 |Δψ| 足够大（否则是"没有激励"，不做判断）

局限（必须在文档里写清）：它只能证明「角度是活的」，**不能证明「是那个目标」**。
若吊舱锁错了物体但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接
独立传感器（双目/其他），不是本模块能覆盖的。
"""

from collections import deque
from dataclasses import dataclass
import math


def wrap_deg_180(deg: float) -> float:
    """角度回绕到 [-180, 180)。"""
    d = math.fmod(deg + 180.0, 360.0)
    if d < 0.0:
        d += 360.0
    return d - 180.0


@dataclass
class LosConsistencyConfig:
    """一致性判据参数（全外置）。"""

    enabled: bool = True            # False → 关掉判据（退回"只信 bit2"）
    window_ms: int = 3000           # 比较窗口：用"当前"与"约 window 前"的样本比对
    min_yaw_change_deg: float = 20.0   # 窗口内机头至少转这么多才做判断（激励门槛）
    max_residual_deg: float = 8.0      # |Δβ+Δψ| 超过即判不一致
    history_factor: float = 2.0     # 历史缓冲 = window × factor


@dataclass
class LosStatus:
    """单次判定结果（供门控与日志）。"""

    consistent: bool = True     # 是否一致（默认一致 = 不干扰正常跟踪）
    excited: bool = False       # 本窗口是否达到"激励门槛"（转过足够角度）
    residual_deg: float = 0.0   # Δβ + Δψ
    delta_yaw_deg: float = 0.0
    delta_beta_deg: float = 0.0

    @property
    def frozen(self) -> bool:
        """判为"角度冻结/幽灵"：有激励但不一致。"""
        return self.excited and not self.consistent


class LosConsistencyMonitor:
    """按窗口比较 Δβ 与 Δψ，判断吊舱角度是否与机体转动一致。

    用法：每个控制周期喂一次（仅在吊舱报告锁定期间喂），
    `update(now_ms, yaw_deg, beta_deg)` 返回 `LosStatus`。
    吊舱解锁时应调用 `reset()`，避免上一次跟踪的历史污染下一次。
    """

    def __init__(self, cfg: LosConsistencyConfig | None = None) -> None:
        self._cfg = cfg or LosConsistencyConfig()
        self._samples: deque = deque()

    def reset(self) -> None:
        self._samples.clear()

    def update(self, now_ms: int, yaw_deg: float, beta_deg: float) -> LosStatus:
        cfg = self._cfg
        if not cfg.enabled:
            return LosStatus(consistent=True, excited=False)

        self._samples.append((now_ms, float(yaw_deg), float(beta_deg)))
        keep_ms = cfg.window_ms * max(1.0, cfg.history_factor)
        while self._samples and now_ms - self._samples[0][0] > keep_ms:
            self._samples.popleft()

        # 参考样本：窗口内**最老**的那个
        ref = None
        for sample in self._samples:
            if now_ms - sample[0] <= cfg.window_ms:
                ref = sample
                break
        if ref is None:
            return LosStatus(consistent=True, excited=False)

        span_ms = now_ms - ref[0]
        # 历史不足一个窗口的 3/4 时，Δψ 天然偏小，不做判断（避免误判丢目标）
        if span_ms < cfg.window_ms * 0.75:
            return LosStatus(consistent=True, excited=False)

        delta_yaw = wrap_deg_180(yaw_deg - ref[1])
        delta_beta = beta_deg - ref[2]
        residual = delta_beta + delta_yaw
        excited = abs(delta_yaw) >= cfg.min_yaw_change_deg
        consistent = (not excited) or (abs(residual) <= cfg.max_residual_deg)
        return LosStatus(consistent=consistent, excited=excited,
                         residual_deg=residual, delta_yaw_deg=delta_yaw,
                         delta_beta_deg=delta_beta)