"""virtual_pod_bridge_test.py —— VirtualPod × PodBridge × TrackingControlLaw 集成测试。

用部署配置(pod_bridge.json)把"SITL 闭环段"在无飞控下跑通，端到端复核 S1 标定：
  目标偏右 → 控制律应输出正 yaw_rate（右转）；
  目标偏下 → 应输出正 vz（下降）；
  目标消失 → 应判丢零速刹车。
"""

import asyncio
from dataclasses import replace
from pathlib import Path

import pytest

from pod_bridge.bridge import PodBridge
from pod_bridge.config import load_from_json
from pod_bridge.px4_gateway import OwnState
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
                                    VirtualTargetConfig)

_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"


class FakePx4:
    def __init__(self, yaw_rad=0.0):
        self._yaw = yaw_rad
        self.clock = 1000          # 测试循环每拍推进，与 step(now_ms) 同步
        self.velocity_calls = []
        self.brake_calls = []

    def own_state(self):
        return OwnState(has_attitude=True, has_position=True, position_valid=True,
                        fix_type=3, yaw_rad=self._yaw, time_ms=self.clock,
                        position_time_ms=self.clock)

    async def start_telemetry(self): pass
    async def stop_telemetry(self): pass
    async def start_offboard(self): return True
    async def stop_offboard(self): pass
    async def send_velocity_heading(self, out, time_boot_ms=None):
        self.velocity_calls.append(out); return True
    async def send_brake_hover(self, time_boot_ms=None):
        self.brake_calls.append(time_boot_ms); return True


def _bridge(target, own):
    cfg = load_from_json(str(_DEFAULT_CONFIG))  # 部署配置：L1 enabled、S1 符号
    pod = VirtualPod(target, lambda: own)
    px4 = FakePx4(yaw_rad=own.yaw_rad)
    return PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0), px4


def _static_target(ahead=10.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 _run_ticks(bridge, px4, n, t0=1000, dt=50):
    """跑 n 拍（每拍 50ms，节拍与 20Hz 一致），返回末拍输出。"""
    out = None
    for i in range(n):
        t = t0 + i * dt
        px4.clock = t
        out = asyncio.run(bridge.step(t))
    return out


def test_deployed_config_turns_right_for_right_target():
    # 目标在右前方：S1 标定 beta_sign=+1 → yaw_rate 应为正（右转）
    tgt = _static_target(ahead=10.0, right=5.0)
    bridge, px4 = _bridge(tgt, CarrierView())
    out = _run_ticks(bridge, px4, 5)
    assert out.mode.name == "VELOCITY_HEADING"
    assert out.yaw_rate_dps > 0.0
    assert px4.velocity_calls  # 正常速度航向指令
    assert not px4.brake_calls


def test_deployed_config_descends_for_below_target():
    # 目标偏下：S1 标定 alpha_sign=-1（吊舱上正→控制律下正）→ vz 应为正（下降）
    tgt = _static_target(ahead=10.0, down=2.0)
    bridge, px4 = _bridge(tgt, CarrierView())
    out = _run_ticks(bridge, px4, 10)
    assert out.mode.name == "VELOCITY_HEADING"
    assert out.vz_mps > 0.0


def test_deployed_config_slow_approach_forward():
    # L1 无距离：slow_approach 0.2 m/s 起步（决策3 低限速）
    tgt = _static_target()
    bridge, px4 = _bridge(tgt, CarrierView())
    out = _run_ticks(bridge, px4, 10)
    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9


def test_target_lost_brakes_through_pipeline():
    tgt = _static_target()
    bridge, px4 = _bridge(tgt, CarrierView())
    _run_ticks(bridge, px4, 3)
    tgt.set_visible(False)  # 目标消失 → 吊舱转搜索态 → 判丢刹车
    out = _run_ticks(bridge, px4, 2, t0=1000 + 3 * 50)
    assert out.mode.name == "BRAKE_HOVER"
    assert px4.brake_calls