"""los_consistency_test.py —— 吊舱角度运动学一致性判据的纯逻辑测试。

背景：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标脱靶量必变 ≈−Δψ。
判据 `residual = Δβ + Δψ`：超限且激励足够 → 判不一致（角度冻结/幽灵目标）。
"""

import pytest

from pod_bridge.los_consistency import (LosConsistencyConfig,
                                        LosConsistencyMonitor, wrap_deg_180)


def _mon(cfg=None):
    return LosConsistencyMonitor(cfg or LosConsistencyConfig())


def _drive(mon, n, dt_ms, yaw0=0.0, beta0=0.0, yaw_rate=0.0, beta_rate=0.0,
           t0=0):
    """按固定角速度喂 n 拍，返回最后一次状态。"""
    status = None
    for i in range(n):
        t = t0 + (i + 1) * dt_ms
        yaw = yaw0 + yaw_rate * (i + 1) * dt_ms / 1000.0
        beta = beta0 + beta_rate * (i + 1) * dt_ms / 1000.0
        status = mon.update(t, yaw, beta)
    return status


# --- 条件开关 ---------------------------------------------------------------
def test_disabled_always_consistent():
    mon = _mon(LosConsistencyConfig(enabled=False))
    status = _drive(mon, 100, 50, yaw_rate=30.0, beta_rate=0.0)
    assert status.consistent is True
    assert status.excited is False


def test_no_history_is_not_excited():
    mon = _mon()
    status = mon.update(1000, 0.0, 10.0)
    assert status.consistent is True
    assert status.excited is False


def test_insufficient_span_is_not_excited():
    """窗口未积累到 3/4 之前不做判断（避免一开始就误判丢目标）。"""
    mon = _mon(LosConsistencyConfig(window_ms=3000))
    status = _drive(mon, 20, 50, yaw_rate=40.0, beta_rate=0.0)   # 仅 1s
    assert status.excited is False
    assert status.consistent is True


# --- 一致：机头转、脱靶量反向跟随 -------------------------------------------
def test_consistent_rotation_is_ok():
    """右转 20°/s 且 β 以 −20°/s 变化 → 残差≈0。"""
    mon = _mon()
    status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-20.0)  # 4s
    assert status.excited is True
    assert status.consistent is True
    assert status.residual_deg == pytest.approx(0.0, abs=0.5)


def test_consistent_left_rotation_is_ok():
    """左转（负角速度）且 β 正向跟随 → 同样一致（符号对称）。"""
    mon = _mon()
    status = _drive(mon, 80, 50, yaw_rate=-20.0, beta_rate=20.0)
    assert status.excited is True
    assert status.consistent is True


def test_target_motion_within_residual_tolerance_is_ok():
    """目标自身在动（β 变化与 −Δψ 不完全相等）但残差在容差内 → 仍判一致。"""
    mon = _mon(LosConsistencyConfig(max_residual_deg=8.0))
    # 4s 内 Δψ=80°，β 只反向变 76° → 残差 4°（< 8°）
    status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=-19.0)
    assert status.excited is True
    assert status.consistent is True


# --- 不一致：角度冻结/幽灵目标 ---------------------------------------------
def test_frozen_beta_while_yawing_is_inconsistent():
    """机头转 80° 而脱靶量纹丝不动 → 角度冻结 → 判不一致。

    比较窗口为 3s（默认），故 4s 数据里只比较最后 3s：Δψ = 20°/s × 3s = 60°。
    """
    mon = _mon()
    status = _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
    assert status.excited is True
    assert status.consistent is False
    assert status.frozen is True
    assert status.residual_deg == pytest.approx(60.0, abs=2.0)   # 窗口内 Δψ


def test_small_yaw_change_does_not_judge_even_if_residual_big():
    """转得不够多（< 激励门槛）时不判不一致 —— 避免误杀。"""
    mon = _mon(LosConsistencyConfig(min_yaw_change_deg=20.0))
    # 窗口内只转 8°，虽然 β 没动（残差 8°）但未达激励门槛
    status = _drive(mon, 80, 50, yaw_rate=2.0, beta_rate=0.0)
    assert status.excited is False
    assert status.consistent is True


# --- 历史管理 ---------------------------------------------------------------
def test_reset_clears_history():
    mon = _mon()
    _drive(mon, 80, 50, yaw_rate=20.0, beta_rate=0.0)
    assert mon._samples                      # 有历史
    mon.reset()
    assert not mon._samples
    status = mon.update(9999, 0.0, 0.0)
    assert status.excited is False


def test_old_samples_dropped():
    """超出历史保留期的样本被丢弃，不会永久污染判定。"""
    mon = _mon(LosConsistencyConfig(window_ms=1000, history_factor=2.0))
    _drive(mon, 40, 50, yaw_rate=50.0, beta_rate=0.0)      # 2s
    # 再喂一拍"很久之后"，旧样本应被清掉
    status = mon.update(60_000, 0.0, 0.0)
    assert status.excited is False


# --- 角度回绕 ---------------------------------------------------------------
@pytest.mark.parametrize("deg,expect", [
    (0.0, 0.0), (179.0, 179.0), (181.0, -179.0),
    (-181.0, 179.0), (359.0, -1.0),
])
def test_wrap_deg_180(deg, expect):
    assert wrap_deg_180(deg) == pytest.approx(expect)


def test_wrap_around_north_is_consistent():
    """航向跨过 ±180 与 β 同步变化时，不应因回绕误判。"""
    mon = _mon()
    # 从 yaw=170 转到 190（即 -170），β 从 10 到 -10
    for i in range(80):
        t = (i + 1) * 50
        yaw = 170.0 + 20.0 * (i + 1) * 0.05
        beta = 10.0 - 20.0 * (i + 1) * 0.05
        status = mon.update(t, wrap_deg_180(yaw), beta)
    assert status.excited is True
    assert status.consistent is True