"""virtual_pod_test.py —— 模拟吊舱几何与 S1 符号约定回归测试。

钉死 S1 台架标定结论（2026-10-09）：目标在左下时 off_target_x/y 均为负，
即 X 右正、Y 上正；并验证不可见/超视场 → 搜索态 + 脱靶量归 0（对齐真实
吊舱"丢失时脱靶量恒 0"的行为）。
"""

import math
import time

import pytest

from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
                                    VirtualTargetConfig)


def _pod(target, own):
    return VirtualPod(target, lambda: own)


def _static_target(ahead=30.0, right=0.0, down=0.0):
    """放在锚点正前方（机头朝北）的静止目标。"""
    cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
                              arc_speed_mps=0.0)
    return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)


def _carrier(yaw_deg=0.0):
    return CarrierView(north_m=0.0, east_m=0.0, down_m=0.0,
                       yaw_rad=math.radians(yaw_deg))


def _telemetry(pod):
    frames = pod.poll()
    assert len(frames) == 1
    return frames[0]


# ---- S1 符号约定回归：目标位置 → 脱靶量符号 --------------------------------
def test_target_right_gives_positive_off_x():
    # 目标在右前方：rel_e>0、机头朝北 → 脱靶量 X 应为正（右正）
    tgt = _static_target(ahead=10.0, right=5.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert tel.off_target_x_deg > 0.0
    assert tel.off_target_x_deg == pytest.approx(math.degrees(math.atan2(5.0, 10.0)),
                                                 abs=1e-9)


def test_target_left_gives_negative_off_x():
    tgt = _static_target(ahead=10.0, right=-5.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert tel.off_target_x_deg < 0.0


def test_target_below_gives_negative_off_y():
    # S1 实测：目标偏下 → off_target_y 为负（上正）。11.3° 在垂直视场(35°)内
    tgt = _static_target(ahead=10.0, down=2.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert tel.off_target_y_deg < 0.0
    assert tel.off_target_y_deg == pytest.approx(-math.degrees(math.atan2(2.0, 10.0)),
                                                 abs=1e-9)


def test_target_above_gives_positive_off_y():
    tgt = _static_target(ahead=10.0, down=-2.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert tel.off_target_y_deg > 0.0


def test_yaw_rotation_follows_body_frame():
    # 机头偏东 20°：目标在正北 → 目标在机体左侧 20° → off_x 为负
    tgt = _static_target(ahead=10.0)
    tel = _telemetry(_pod(tgt, _carrier(yaw_deg=20.0)))
    assert tel.off_target_x_deg == pytest.approx(-20.0, abs=1e-9)


# ---- 无电机 / 无激光语义 ---------------------------------------------------
def test_frame_angles_always_zero_and_laser_none():
    tgt = _static_target()
    tel = _telemetry(_pod(tgt, _carrier()))
    assert tel.frame_yaw_deg == 0.0
    assert tel.frame_pitch_deg == 0.0
    assert tel.frame_roll_deg == 0.0
    assert tel.laser_range_m is None


# ---- 锁定门控 --------------------------------------------------------------
def test_visible_target_locks():
    tel = _telemetry(_pod(_static_target(), _carrier()))
    assert tel.is_target_locked


def test_hidden_target_searches_with_zero_off():
    tgt = _static_target()
    tgt.set_visible(False)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert not tel.is_target_locked
    assert tel.off_target_x_deg == 0.0 and tel.off_target_y_deg == 0.0


def test_target_behind_searches():
    # 机头朝北、目标在正南 10m → fwd<0 → 搜索态
    cfg = VirtualTargetConfig(ahead_m=-10.0, right_m=0.0, down_m=0.0,
                              arc_speed_mps=0.0)
    tgt = VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert not tel.is_target_locked


def test_target_out_of_fov_searches():
    # 目标几乎在正右方（β→90°）超出默认 FOV 60° → 搜索态
    tgt = _static_target(ahead=1.0, right=100.0)
    tel = _telemetry(_pod(tgt, _carrier()))
    assert not tel.is_target_locked


# ---- PodLink 接口 ----------------------------------------------------------
def test_send_counts_and_close_marks():
    pod = _pod(_static_target(), _carrier())
    pod.send(b"\x00" * 4)
    pod.send(b"\x00" * 4)
    assert pod.attitude_frames_rx == 2
    pod.close()
    assert pod.closed