"""Tests for the vessel-heading simulator."""

from __future__ import annotations

import pytest

from arautopilot.studio.simulator.vessel_heading import (
    VesselHeadingConfig,
    VesselHeadingSimulator,
    heading_error_deg,
)


def test_zero_rudder_holds_heading() -> None:
    sim = VesselHeadingSimulator()
    sim.reset(heading_deg=42.0)
    for _ in range(2000):
        sim.step(dt=0.01, rudder_deg=0.0)
    assert sim.state.heading_deg == pytest.approx(42.0, abs=1e-3)


def test_positive_rudder_turns_starboard() -> None:
    sim = VesselHeadingSimulator()
    sim.reset(heading_deg=0.0)
    for _ in range(2000):
        sim.step(dt=0.01, rudder_deg=5.0)
    # After 20 s with +5 deg of rudder, heading should advance (mod 360).
    assert sim.state.rate_of_turn_dps > 0.0
    assert sim.state.heading_deg != 0.0


def test_negative_rudder_turns_port() -> None:
    sim = VesselHeadingSimulator()
    sim.reset(heading_deg=0.0)
    for _ in range(2000):
        sim.step(dt=0.01, rudder_deg=-5.0)
    assert sim.state.rate_of_turn_dps < 0.0


def test_speed_increases_yaw_response() -> None:
    sim_slow = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=5.0))
    sim_fast = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=20.0))
    sim_slow.reset()
    sim_fast.reset()
    for _ in range(2000):
        sim_slow.step(dt=0.01, rudder_deg=5.0)
        sim_fast.step(dt=0.01, rudder_deg=5.0)
    # Fast vessel turns farther in the same time.
    assert abs(sim_fast.state.rate_of_turn_dps) > abs(sim_slow.state.rate_of_turn_dps)


def test_heading_wraps_at_360() -> None:
    sim = VesselHeadingSimulator()
    sim.reset(heading_deg=359.0, rate_of_turn_dps=10.0)
    for _ in range(20):
        sim.step(dt=0.1, rudder_deg=0.0)
    # heading must remain in [0, 360)
    assert 0.0 <= sim.state.heading_deg < 360.0


def test_invalid_dt() -> None:
    sim = VesselHeadingSimulator()
    sim.reset()
    with pytest.raises(ValueError):
        sim.step(dt=0.0, rudder_deg=5.0)


# ----------------------------------------------------------------------------
# heading_error_deg
# ----------------------------------------------------------------------------


@pytest.mark.parametrize("sp, meas, expected", [
    (90.0, 80.0, 10.0),
    (80.0, 90.0, -10.0),
    (0.0, 0.0, 0.0),
    (0.0, 359.0, 1.0),         # crossing 0 going stbd
    (359.0, 0.0, -1.0),
    (180.0, 0.0, 180.0),
    (0.0, 180.0, 180.0),       # ambiguity at 180 -- convention is +180
    (10.0, 350.0, 20.0),
    (350.0, 10.0, -20.0),
])
def test_heading_error_shortest_arc(sp: float, meas: float, expected: float) -> None:
    assert heading_error_deg(sp, meas) == pytest.approx(expected, abs=1e-9)