"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。

移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
限幅（S6）、机体系→NED 输出旋转（A4）。
"""

import math

import pytest

from visual_track_control.config import (HeadingMode, NoDistanceAction,
                                         TrackingConfig)
from visual_track_control.tracking_control_law import TrackingControlLaw
from visual_track_control.types import (ControlMode, ControlSnapshot,
                                        DegradedReason, TrackState)


def make_config() -> TrackingConfig:
    """测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
    画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
    偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
    """
    cfg = TrackingConfig()
    cfg.control.frequency_hz = 20.0
    cfg.control.visual_stale_ms = 200
    cfg.control.distance_stale_ms = 500
    cfg.control.attitude_stale_ms = 300
    cfg.camera.image_width = 1000
    cfg.camera.image_height = 1000
    cfg.camera.fov_h_deg = 90.0
    cfg.camera.fov_v_deg = 90.0
    cfg.heading.mode = HeadingMode.RATE
    cfg.heading.gain = 1.0
    cfg.heading.yaw_rate_limit_dps = 30.0
    cfg.heading.yaw_slew_limit_dps2 = 100000.0
    cfg.vertical.vz_gain_mps_per_deg = 0.1
    cfg.vertical.vz_limit_mps = 2.0
    cfg.distance.d_exp_m = 10.0
    cfg.distance.kp = 0.5
    cfg.distance.ki = 0.0
    cfg.distance.kd = 0.0
    cfg.distance.integral_limit = 0.0
    cfg.distance.derivative_filter_coef = 0.0
    cfg.distance.approach_velocity_limit_mps = 3.0
    cfg.distance.retreat_velocity_limit_mps = 1.0
    cfg.distance.no_distance_action = NoDistanceAction.HOLD
    cfg.distance.no_distance_approach_limit_mps = 0.5
    cfg.accel_limit.ax_mps2 = 1000.0
    cfg.accel_limit.ay_mps2 = 1000.0
    cfg.accel_limit.az_mps2 = 1000.0
    return cfg


def make_snapshot(now_ms: int) -> ControlSnapshot:
    """新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
    return ControlSnapshot(
        track=TrackState(tracked=True, is_predicted=False,
                         center=(500.0, 500.0)),
        track_time_ms=now_ms,
        attitude_present=True,
        attitude_time_ms=now_ms,
    )


# ---- 超龄降级（§7.1 / §9.2） ----

def test_attitude_missing_degrades():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.attitude_present = False
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
    assert not out.valid


def test_attitude_stale_degrades():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.attitude_time_ms = 600  # 400 ms 前 > 300 ms 阈值
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE


def test_vision_stale_degrades():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track_time_ms = 700  # 300 ms 前 > 200 ms 阈值
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.VISION_STALE


def test_never_sampled_track_degrades():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track_time_ms = 0  # 从未取样
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.VISION_STALE


def test_attitude_stale_beats_vision_stale():
    # 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.attitude_present = False
    snap.track_time_ms = 0
    out = law.update(snap, 1000)
    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE


# ---- 判丢刹车（§6.4） ----

def test_lost_target_brakes():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.tracked = False
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.BRAKE_HOVER
    assert out.valid
    assert out.vx_mps == 0.0
    assert out.vy_mps == 0.0
    assert out.vz_mps == 0.0


# ---- Coast（§6.4） ----

def test_coast_sets_flag_and_keeps_tracking():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.is_predicted = True
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.VELOCITY_HEADING
    assert out.coast_active
    assert out.valid


# ---- 水平航向通道（§4.2，A2） ----

def test_rate_mode_maps_beta_to_yaw_rate():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.center = (600.0, 500.0)  # 目标偏右 100 像素
    out = law.update(snap, 1000)
    beta_deg = math.degrees(math.atan(0.2))  # ≈ 11.31°
    assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6)  # gain=1，右转正
    assert out.yaw_deg == 0.0


def test_rate_mode_clamps_to_yaw_rate_limit():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.center = (950.0, 500.0)  # 大偏移：β≈43.5° > 30°/s 上限
    out = law.update(snap, 1000)
    assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9)


def test_position_mode_composes_absolute_heading():
    # A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
    cfg = make_config()
    cfg.heading.mode = HeadingMode.POSITION
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.track.center = (600.0, 500.0)
    snap.yaw_rad = math.radians(45.0)
    out = law.update(snap, 1000)
    beta_deg = math.degrees(math.atan(0.2))
    assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6)
    assert out.yaw_rate_dps == 0.0


def test_position_mode_wraps_around_180():
    cfg = make_config()
    cfg.heading.mode = HeadingMode.POSITION
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.track.center = (600.0, 500.0)
    snap.yaw_rad = math.radians(175.0)
    out = law.update(snap, 1000)
    beta_deg = math.degrees(math.atan(0.2))
    # 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
    assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6)
    assert -180.0 < out.yaw_deg < 180.0


# ---- 垂直统一速度支路（§5，A1） ----

def test_vertical_offset_drives_vz():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.center = (500.0, 600.0)  # 目标偏低 100 像素 → α>0 → 下降
    out = law.update(snap, 1000)
    alpha_deg = math.degrees(math.atan(0.2))
    assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6)
    assert out.mode is ControlMode.VELOCITY_HEADING  # A1：不切姿态微调模式


def test_vertical_velocity_clamped():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.track.center = (500.0, 990.0)  # α≈44.5° × 0.1 = 4.45 > 2 上限
    out = law.update(snap, 1000)
    assert out.vz_mps == pytest.approx(2.0, abs=1e-9)


# ---- 距离通道（§6.2 / §6.3） ----

def test_distance_approach_when_far():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.target_distance_m = 30.0  # Δd = +20 → kp 输出 10 → 限幅 3
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    out = law.update(snap, 1000)
    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)


def test_distance_retreat_clamped_when_too_close():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.target_distance_m = 5.0  # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    out = law.update(snap, 1000)
    assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)


def test_no_distance_hold_zeros_vx():
    law = TrackingControlLaw(make_config())  # 基准配置 HOLD
    snap = make_snapshot(1000)
    snap.distance_valid = False
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.VELOCITY_HEADING
    assert out.vx_mps == 0.0


def test_no_distance_slow_approach():
    cfg = make_config()
    cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.distance_valid = False
    out = law.update(snap, 1000)
    assert out.vx_mps == pytest.approx(0.5, abs=1e-9)


def test_no_distance_exit_waits_grace_then_degrades():
    # S1 拍板：EXIT 需持续无距离达门限才降级；grace 期间按 SLOW_APPROACH 行为
    cfg = make_config()
    cfg.distance.no_distance_action = NoDistanceAction.EXIT
    cfg.distance.no_distance_exit_grace_ms = 200
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.distance_valid = False
    out1 = law.update(snap, 1000)
    assert out1.mode is ControlMode.VELOCITY_HEADING
    assert out1.vx_mps == pytest.approx(0.5, abs=1e-9)
    snap.track_time_ms = 1200  # 保持视觉/姿态新鲜，聚焦距离通道
    snap.attitude_time_ms = 1200
    out2 = law.update(snap, 1200)  # 恰达门限 → 降级
    assert out2.mode is ControlMode.DEGRADED_HOLD
    assert out2.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
    assert not out2.valid


def test_no_distance_exit_grace_resets_on_valid_frame():
    cfg = make_config()
    cfg.distance.no_distance_action = NoDistanceAction.EXIT
    cfg.distance.no_distance_exit_grace_ms = 200
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.distance_valid = False
    assert law.update(snap, 1000).mode is ControlMode.VELOCITY_HEADING

    snap.track_time_ms = 1100  # 有效距离帧：重置计时
    snap.attitude_time_ms = 1100
    snap.distance_valid = True
    snap.target_distance_m = 30.0
    snap.distance_time_ms = 1100
    assert law.update(snap, 1100).mode is ControlMode.VELOCITY_HEADING

    snap.track_time_ms = 1250  # 再次无效：重新计时
    snap.attitude_time_ms = 1250
    snap.distance_valid = False
    snap.target_distance_m = 0.0
    assert law.update(snap, 1250).mode is ControlMode.VELOCITY_HEADING  # 0 ms < 200 ms

    snap.track_time_ms = 1500
    snap.attitude_time_ms = 1500
    out = law.update(snap, 1500)  # 250 ms ≥ 200 ms → 降级
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT


def test_no_distance_exit_zero_grace_degrades_immediately():
    # grace = 0 保留旧"首个无效拍即退出"行为
    cfg = make_config()
    cfg.distance.no_distance_action = NoDistanceAction.EXIT
    cfg.distance.no_distance_exit_grace_ms = 0
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.distance_valid = False
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.DEGRADED_HOLD
    assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
    assert not out.valid


def test_stale_distance_falls_back_to_no_distance():
    law = TrackingControlLaw(make_config())  # HOLD
    snap = make_snapshot(1000)
    snap.target_distance_m = 30.0
    snap.distance_valid = True
    snap.distance_time_ms = 400  # 600 ms 前 > 500 ms 阈值 → 距离不可用
    out = law.update(snap, 1000)
    assert out.mode is ControlMode.VELOCITY_HEADING
    assert out.vx_mps == 0.0


# ---- 首拍/限幅（§8、S6） ----

def test_first_tick_vx_slews_from_zero():
    # S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
    cfg = make_config()
    cfg.accel_limit.ax_mps2 = 2.0
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.target_distance_m = 30.0  # 通道输出本应为 3.0
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    out = law.update(snap, 1000)  # 首拍：dt 按 1/20 Hz 兜底
    assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9)  # 0.1


# ---- 机体系 → NED 旋转（§2.3，A4） ----

def test_output_rotated_to_ned_by_yaw():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.yaw_rad = math.pi / 2.0  # 机头朝东
    snap.target_distance_m = 30.0
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    out = law.update(snap, 1000)
    # 机体前向 3.0 → 东向 3.0，北向 ≈ 0
    assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
    assert out.vy_mps == pytest.approx(3.0, abs=1e-9)


def test_zero_yaw_keeps_forward_as_north():
    law = TrackingControlLaw(make_config())
    snap = make_snapshot(1000)
    snap.yaw_rad = 0.0
    snap.target_distance_m = 30.0
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    out = law.update(snap, 1000)
    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
    assert out.vy_mps == pytest.approx(0.0, abs=1e-9)


# ---- Reset（§6.4） ----

def test_reset_restores_slew_from_zero():
    cfg = make_config()
    cfg.accel_limit.ax_mps2 = 2.0
    law = TrackingControlLaw(cfg)
    snap = make_snapshot(1000)
    snap.target_distance_m = 30.0
    snap.distance_valid = True
    snap.distance_time_ms = 1000
    for t in range(1000, 3000, 50):
        snap.track_time_ms = t
        snap.distance_time_ms = t
        snap.attitude_time_ms = t
        law.update(snap, t)  # 若干拍后 vx 已爬升到限幅 3.0
    law.reset()
    snap.track_time_ms = 3000
    snap.distance_time_ms = 3000
    snap.attitude_time_ms = 3000
    out = law.update(snap, 3000)
    # Reset 后限幅器归零：又从 0.1 开始渐变
    assert out.vx_mps == pytest.approx(0.1, abs=1e-9)