"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。

移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。
C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射，
改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、
A2 掩码三分支的语义等价）：

  角速率非零 → 机体速度设定点 + 非零 yawspeed；
  航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw；
  两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）；
  刹车 → 全零机体速度设定点。
  传输失败（异常）→ 返回 False 且不向外抛。
"""

import asyncio

import pytest

from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
from visual_track_control.types import ControlOutput


@pytest.fixture(autouse=True)
def _stub_mavsdk(fake_mavsdk):
    """本模块全部用例与真实 libcmavsdk.so 脱钩(树莓派原生库符号缺失实测)。"""
    return fake_mavsdk


class FakeOffboard:
    """捕获式 offboard mock：记录每次调用的设定点，可注入失败。"""

    def __init__(self):
        self.body_setpoints = []
        self.ned_setpoints = []
        self.attempts = 0
        self.start_count = 0
        self.stop_count = 0
        self.succeed = True

    async def set_velocity_body(self, setpoint):
        self.attempts += 1  # 与 C++ CaptureTransport 一致：失败也计入发送次数
        if not self.succeed:
            raise RuntimeError("mock transport failure")
        self.body_setpoints.append(setpoint)

    async def set_velocity_ned(self, setpoint):
        self.attempts += 1
        if not self.succeed:
            raise RuntimeError("mock transport failure")
        self.ned_setpoints.append(setpoint)

    async def start(self):
        if not self.succeed:
            raise RuntimeError("mock start failure")
        self.start_count += 1

    async def stop(self):
        self.stop_count += 1

    @property
    def send_count(self):
        return self.attempts


class FakeDrone:
    def __init__(self):
        self.offboard = FakeOffboard()


def run(coro):
    return asyncio.run(coro)


def make_gateway():
    drone = FakeDrone()
    return MavsdkCommandGateway(drone), drone.offboard


def test_start_sends_initial_setpoint_then_starts():
    gateway, offboard = make_gateway()
    assert run(gateway.start())
    assert offboard.start_count == 1
    # PX4 要求先流式发送首条设定点（全零）再请求启动
    assert len(offboard.body_setpoints) == 1
    sp = offboard.body_setpoints[0]
    assert sp.forward_m_s == 0.0
    assert sp.right_m_s == 0.0
    assert sp.down_m_s == 0.0
    assert sp.yawspeed_deg_s == 0.0


def test_start_failure_returns_false():
    gateway, offboard = make_gateway()
    offboard.succeed = False
    assert not run(gateway.start())


def _make_velocity_heading_output():
    from visual_track_control.types import ControlMode
    out = ControlOutput()
    out.mode = ControlMode.VELOCITY_HEADING
    return out


def test_yaw_rate_branch_uses_body_frame_with_yawspeed():
    from visual_track_control.types import ControlMode
    gateway, offboard = make_gateway()
    out = _make_velocity_heading_output()
    out.vx_body_mps = 1.5
    out.vz_mps = 0.25
    out.yaw_rate_dps = 5.0
    assert run(gateway.send_velocity_heading(out, 12345))
    assert offboard.send_count == 1
    assert len(offboard.body_setpoints) == 1
    sp = offboard.body_setpoints[0]
    assert sp.forward_m_s == pytest.approx(1.5)
    assert sp.right_m_s == 0.0
    assert sp.down_m_s == pytest.approx(0.25)
    assert sp.yawspeed_deg_s == pytest.approx(5.0)
    assert offboard.ned_setpoints == []


def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw():
    # 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向
    gateway, offboard = make_gateway()
    out = _make_velocity_heading_output()
    out.vx_mps = 2.0
    out.vy_mps = -1.0
    out.vz_mps = 0.5
    out.yaw_deg = 10.0
    out.yaw_rate_dps = 0.0
    assert run(gateway.send_velocity_heading(out, 1))
    assert offboard.send_count == 1
    assert len(offboard.ned_setpoints) == 1
    sp = offboard.ned_setpoints[0]
    assert sp.north_m_s == pytest.approx(2.0)
    assert sp.east_m_s == pytest.approx(-1.0)
    assert sp.down_m_s == pytest.approx(0.5)
    assert sp.yaw_deg == pytest.approx(10.0)
    assert offboard.body_setpoints == []


def test_both_zero_keeps_heading_via_body_zero_yawspeed():
    # A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向，
    # 不得误发绝对航向 0（正北）。
    gateway, offboard = make_gateway()
    out = _make_velocity_heading_output()
    out.vx_body_mps = 1.0
    out.yaw_rate_dps = 0.0
    out.yaw_deg = 0.0
    assert run(gateway.send_velocity_heading(out, 1))
    assert len(offboard.body_setpoints) == 1
    sp = offboard.body_setpoints[0]
    assert sp.forward_m_s == pytest.approx(1.0)
    assert sp.yawspeed_deg_s == 0.0
    assert offboard.ned_setpoints == []  # 不得走绝对航向支路


def test_brake_hover_sends_zero_velocity_body():
    gateway, offboard = make_gateway()
    assert run(gateway.send_brake_hover(777))
    assert offboard.send_count == 1
    sp = offboard.body_setpoints[0]
    assert sp.forward_m_s == 0.0
    assert sp.right_m_s == 0.0
    assert sp.down_m_s == 0.0
    assert sp.yawspeed_deg_s == 0.0


def test_transport_failure_propagates_as_false():
    gateway, offboard = make_gateway()
    offboard.succeed = False
    out = _make_velocity_heading_output()
    assert not run(gateway.send_velocity_heading(out, 1))
    assert not run(gateway.send_brake_hover(1))
    assert offboard.send_count == 2


def test_stop_calls_offboard_stop():
    gateway, offboard = make_gateway()
    run(gateway.stop())
    assert offboard.stop_count == 1