AR-Autopilot/arautopilot/tests/test_true_course.py

"""Tests for TrueCoursePilot -- Sprint 5."""

from __future__ import annotations

import math
import pytest

from arautopilot.studio.simulator.true_course import TrueCoursePilot, TrueCourseConfig
from arautopilot.studio.simulator.pid_outer import PidOuterConfig


def _pilot(xte_gain: float = 0.5, max_corr: float = 20.0) -> TrueCoursePilot:
    cfg = TrueCourseConfig(
        pid=PidOuterConfig(base_kp=1.0, base_ki=0.0, base_kd=0.0,
                           rot_ff_gain=0.0, rate_limit_dps=360.0),
        xte_gain=xte_gain,
        xte_max_correction_deg=max_corr,
    )
    return TrueCoursePilot(cfg)


# ---------------------------------------------------------------------------
#  TRUE_COURSE mode
# ---------------------------------------------------------------------------

class TestTrueCoursePilot:
    def test_zero_error_zero_output(self):
        p = _pilot()
        out = p.step_true_course(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
        )
        assert out == pytest.approx(0.0, abs=0.6)

    def test_positive_error_steer_starboard(self):
        """COG is west of setpoint → steer to starboard (positive rudder)."""
        p = _pilot()
        out = p.step_true_course(
            cog_setpoint_deg=90.0,
            measured_cog_deg=80.0,   # 10° behind
            sog_kn=8.0,
        )
        assert out > 0.0

    def test_negative_error_steer_port(self):
        p = _pilot()
        out = p.step_true_course(
            cog_setpoint_deg=90.0,
            measured_cog_deg=100.0,
            sog_kn=8.0,
        )
        assert out < 0.0

    def test_360_boundary_crossing(self):
        """Setpoint=5°, measured=355° → shortest arc = +10° → steer stbd."""
        p = _pilot()
        out = p.step_true_course(
            cog_setpoint_deg=5.0,
            measured_cog_deg=355.0,
            sog_kn=8.0,
        )
        assert out > 0.0

    def test_not_allowed_returns_zero(self):
        p = _pilot()
        out = p.step_true_course(
            cog_setpoint_deg=90.0,
            measured_cog_deg=80.0,
            sog_kn=8.0,
            allowed=False,
        )
        assert out == pytest.approx(0.0)

    def test_xte_correction_zero_in_true_course_mode(self):
        p = _pilot()
        p.step_true_course(cog_setpoint_deg=90.0, measured_cog_deg=90.0, sog_kn=8.0)
        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)


# ---------------------------------------------------------------------------
#  TRACK_KEEPING mode
# ---------------------------------------------------------------------------

class TestTrackKeeping:
    def test_no_xte_correction_when_nan(self):
        p = _pilot()
        p.step_track_keeping(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
            xte_m=math.nan,
        )
        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)
        assert p.tc_state.effective_setpoint_deg == pytest.approx(90.0)

    def test_positive_xte_steers_port(self):
        """XTE>0 = vessel to starboard → correction negative → setpoint shifted port."""
        p = _pilot(xte_gain=1.0)
        p.step_track_keeping(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
            xte_m=5.0,   # 5 m to starboard
        )
        assert p.tc_state.xte_correction_deg == pytest.approx(-5.0)
        assert p.tc_state.effective_setpoint_deg == pytest.approx(85.0)

    def test_negative_xte_steers_starboard(self):
        p = _pilot(xte_gain=1.0)
        p.step_track_keeping(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
            xte_m=-5.0,
        )
        assert p.tc_state.xte_correction_deg == pytest.approx(5.0)
        assert p.tc_state.effective_setpoint_deg == pytest.approx(95.0)

    def test_xte_correction_clamped(self):
        p = _pilot(xte_gain=1.0, max_corr=10.0)
        p.step_track_keeping(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
            xte_m=50.0,   # huge XTE → clamp to 10°
        )
        assert p.tc_state.xte_correction_deg == pytest.approx(-10.0)

    def test_stale_xte_ignored(self):
        """If xte_age_s > max_age_s, correction must be zero."""
        p = _pilot(xte_gain=1.0)
        p.step_track_keeping(
            cog_setpoint_deg=90.0,
            measured_cog_deg=90.0,
            sog_kn=8.0,
            xte_m=10.0,
            xte_age_s=10.0,   # stale (default max_age_s=5)
        )
        assert p.tc_state.xte_correction_deg == pytest.approx(0.0)

    def test_convergence_to_track(self):
        """Simulate 30 steps: XTE should shrink (very basic closed-loop sanity)."""
        from arautopilot.studio.simulator.vessel_heading import heading_error_deg

        p = TrueCoursePilot(TrueCourseConfig(
            pid=PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
                               rot_ff_gain=0.0, rate_limit_dps=360.0,
                               max_rudder_deg=35.0),
            xte_gain=0.5,
            xte_max_correction_deg=20.0,
        ))

        xte = 20.0   # 20 m to starboard
        cog = 90.0   # vessel COG
        dt = 0.1     # 10 Hz
        sog = 5.0    # kn ≈ 2.57 m/s

        for _ in range(200):
            rudder = p.step_track_keeping(
                cog_setpoint_deg=90.0,
                measured_cog_deg=cog,
                sog_kn=sog,
                xte_m=xte,
            )
            cog_change = rudder * 0.5 * dt
            cog = (cog + cog_change) % 360.0
            # Cross-track rate: -SOG * cos(COG) for east-going track (COG=90 nominal)
            xte -= sog * 0.5144 * math.cos(math.radians(cog)) * dt

        assert abs(xte) < 18.0   # must have improved from initial 20 m

    def test_reset_clears_state(self):
        p = _pilot()
        p.step_track_keeping(
            cog_setpoint_deg=90.0, measured_cog_deg=80.0,
            sog_kn=8.0, xte_m=5.0,
        )
        p.reset()
        assert p.pid_state.integral == pytest.approx(0.0)