"""frame_transform_test.py —— rotate_body_to_ned 单元测试。

移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
垂直分量不变、模长守恒。
"""

import math

import pytest

from visual_track_control.frame_transform import rotate_body_to_ned


def test_zero_yaw_is_identity():
    north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
    assert north == pytest.approx(1.0, abs=1e-12)   # 北 = 前
    assert east == pytest.approx(0.5, abs=1e-12)    # 东 = 右
    assert down == pytest.approx(-2.0, abs=1e-12)


def test_yaw_east_90_maps_forward_to_east():
    north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0)  # 机头朝东
    assert north == pytest.approx(0.0, abs=1e-9)  # 北向 ≈ 0
    assert east == pytest.approx(2.0, abs=1e-9)   # 东向 = 前向速度


def test_right_is_east_when_heading_north():
    north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
    assert north == pytest.approx(0.0, abs=1e-12)
    assert east == pytest.approx(1.0, abs=1e-12)


def test_down_component_unchanged():
    _, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
    assert down == 3.5


def test_preserves_horizontal_norm():
    # 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
    north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
    assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
                                                    abs=1e-9)