AR-Autopilot/arautopilot/tests/test_pid_outer_python.py

"""Tests for the Python PID outer loop, including the cascade with the
inner PID + rudder simulator + vessel-heading simulator."""

from __future__ import annotations

import pytest

from arautopilot.studio.simulator.pid_inner import PidInner, PidInnerConfig
from arautopilot.studio.simulator.pid_outer import (
    GainPoint,
    PidOuter,
    PidOuterConfig,
    interpolate_gains,
)
from arautopilot.studio.simulator.rudder_dynamics import (
    RudderDynamicsConfig,
    RudderSimulator,
)
from arautopilot.studio.simulator.vessel_heading import (
    HeadingRunRecorder,
    VesselHeadingConfig,
    VesselHeadingSimulator,
    heading_error_deg,
)


# ----------------------------------------------------------------------------
# Gain scheduling
# ----------------------------------------------------------------------------


def test_interpolate_empty_schedule_returns_base() -> None:
    kp, ki, kd = interpolate_gains([], 10.0, 0.9, 0.02, 1.2)
    assert (kp, ki, kd) == (0.9, 0.02, 1.2)


def test_interpolate_endpoint_hold() -> None:
    sched = [
        GainPoint(5.0, 1.2, 0.03, 0.8),
        GainPoint(28.0, 0.55, 0.01, 1.8),
    ]
    assert interpolate_gains(sched, 0.0, 0, 0, 0) == (1.2, 0.03, 0.8)
    assert interpolate_gains(sched, 100.0, 0, 0, 0) == (0.55, 0.01, 1.8)


def test_interpolate_midpoint() -> None:
    sched = [
        GainPoint(5.0, 1.0, 0.04, 0.8),
        GainPoint(15.0, 0.5, 0.02, 1.2),
    ]
    kp, ki, kd = interpolate_gains(sched, 10.0, 0, 0, 0)
    assert kp == pytest.approx(0.75)
    assert ki == pytest.approx(0.03)
    assert kd == pytest.approx(1.0)


# ----------------------------------------------------------------------------
# Standalone outer-PID behaviour
# ----------------------------------------------------------------------------


def test_zero_error_in_deadband_produces_zero_output() -> None:
    pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
                                   deadband_deg=2.0, rot_ff_gain=0.0))
    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=101.0,
                   rate_of_turn_dps=0.0, speed_kn=10.0)
    assert out == 0.0


def test_positive_error_commands_starboard_rudder() -> None:
    pid = PidOuter(PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
                                   deadband_deg=0.0, rot_ff_gain=0.0,
                                   rate_limit_dps=10000.0))
    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
                   rate_of_turn_dps=0.0, speed_kn=10.0)
    assert out > 0.0


def test_rot_feed_forward_subtracts_rotation() -> None:
    """With positive ROT (already turning stbd) the outer PID should ease
    off the rudder compared to ROT=0 -- the same error commands less rudder."""
    cfg_no_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
                                deadband_deg=0.0, rot_ff_gain=0.0,
                                rate_limit_dps=10000.0)
    cfg_with_ff = PidOuterConfig(base_kp=2.0, base_ki=0.0, base_kd=0.0,
                                  deadband_deg=0.0, rot_ff_gain=3.0,
                                  rate_limit_dps=10000.0)
    pid_a = PidOuter(cfg_no_ff)
    pid_b = PidOuter(cfg_with_ff)
    a = pid_a.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
                   rate_of_turn_dps=5.0, speed_kn=10.0)
    b = pid_b.step(heading_setpoint_deg=100.0, heading_measured_deg=90.0,
                   rate_of_turn_dps=5.0, speed_kn=10.0)
    assert a > b  # ROT feed-forward eases rudder when already turning stbd


def test_output_saturates_to_max_rudder() -> None:
    pid = PidOuter(PidOuterConfig(base_kp=50.0, base_ki=0.0, base_kd=0.0,
                                   deadband_deg=0.0, rot_ff_gain=0.0,
                                   max_rudder_deg=35.0, rate_limit_dps=10000.0))
    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
                   rate_of_turn_dps=0.0, speed_kn=10.0)
    assert out == 35.0


def test_rate_limit_caps_rudder_setpoint_slew() -> None:
    """With rate_limit_dps=5 and dt=0.1, the rudder setpoint can change at
    most 0.5 deg per tick."""
    pid = PidOuter(PidOuterConfig(base_kp=10.0, base_ki=0.0, base_kd=0.0,
                                   deadband_deg=0.0, rot_ff_gain=0.0,
                                   rate_limit_dps=5.0, freq_hz=10.0))
    out1 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
                    rate_of_turn_dps=0.0, speed_kn=10.0)
    assert out1 == pytest.approx(0.5, abs=1e-6)
    out2 = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=0.0,
                    rate_of_turn_dps=0.0, speed_kn=10.0)
    assert out2 == pytest.approx(1.0, abs=1e-6)


def test_allowed_false_bleeds_integrator() -> None:
    pid = PidOuter()
    pid.state.integral = 10.0
    out = pid.step(heading_setpoint_deg=100.0, heading_measured_deg=80.0,
                   rate_of_turn_dps=0.0, speed_kn=10.0, allowed=False)
    assert out == 0.0
    assert abs(pid.state.integral) < 10.0


# ----------------------------------------------------------------------------
# End-to-end cascade: outer + inner + rudder dynamics + vessel heading
# ----------------------------------------------------------------------------


def _run_cascade(
    setpoint_deg: float,
    *,
    initial_heading: float = 0.0,
    seconds: float = 60.0,
    speed_kn: float = 10.0,
) -> tuple[HeadingRunRecorder, VesselHeadingSimulator, RudderSimulator]:
    vessel = VesselHeadingSimulator(VesselHeadingConfig(speed_kn=speed_kn))
    vessel.reset(heading_deg=initial_heading)
    rudder = RudderSimulator()
    rudder.reset()

    outer = PidOuter(PidOuterConfig(
        base_kp=1.5, base_ki=0.04, base_kd=2.0,
        deadband_deg=0.2,
        rot_ff_gain=2.0,
        max_rudder_deg=35.0,
        rate_limit_dps=30.0,
        freq_hz=10.0,
    ))
    inner = PidInner(PidInnerConfig(
        kp=15.0, ki=2.0, kd=1.0,
        deadband_deg=0.1,
        rate_limit_dps=10000.0,
        deadband_pct=0.0,
        min_useful_pwm_pct=0.0,
        integral_clamp=200.0,
        freq_hz=50.0,
    ))
    outer.reset()
    inner.reset()

    rec = HeadingRunRecorder()
    dt_sim = 0.002                 # 500 Hz physics
    dt_inner = inner.config.dt()   # 50 Hz
    dt_outer = outer.config.dt()   # 10 Hz
    rudder_setpoint = 0.0
    inner_carry = 0.0
    outer_carry = 0.0
    cmd = 0.0
    t = 0.0
    steps = int(seconds / dt_sim)
    for _ in range(steps):
        outer_carry += dt_sim
        if outer_carry + 1e-12 >= dt_outer:
            rudder_setpoint = outer.step(
                heading_setpoint_deg=setpoint_deg,
                heading_measured_deg=vessel.state.heading_deg,
                rate_of_turn_dps=vessel.state.rate_of_turn_dps,
                speed_kn=speed_kn,
            )
            outer_carry -= dt_outer
        inner_carry += dt_sim
        if inner_carry + 1e-12 >= dt_inner:
            cmd = inner.step(setpoint_deg=rudder_setpoint,
                             measured_deg=rudder.state.angle_deg)
            inner_carry -= dt_inner
        rudder.step(dt=dt_sim, pwm_pct=cmd)
        vessel.step(dt=dt_sim, rudder_deg=rudder.state.angle_deg,
                     speed_kn=speed_kn)
        t += dt_sim
        rec.record(t=t, setpoint_deg=setpoint_deg, heading_sim=vessel,
                    rudder_setpoint_deg=rudder_setpoint,
                    rudder_actual_deg=rudder.state.angle_deg)
    return rec, vessel, rudder


def test_cascade_settles_on_30_deg_step() -> None:
    """A 30 deg heading change should settle within +-2 deg of target in 60 s
    using the cascade outer + inner + rudder dynamics + vessel heading."""
    rec, _, _ = _run_cascade(30.0, initial_heading=0.0, seconds=60.0)
    final_err = rec.final_heading_error_deg(30.0)
    assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0"


def test_cascade_settles_on_negative_step() -> None:
    """Heading change from 90 to 60 deg (turn -30) should also converge."""
    rec, _, _ = _run_cascade(60.0, initial_heading=90.0, seconds=60.0)
    final_err = rec.final_heading_error_deg(60.0)
    assert final_err <= 2.0, f"final heading error {final_err} deg, want <= 2.0"


def test_cascade_crosses_360_correctly() -> None:
    """From heading 350 to setpoint 10: should turn 20 deg starboard, not
    340 deg port. End within +-4 deg of 10 (wrap-around is the harder
    convergence case because the rudder reverses near the crossing)."""
    rec, _, _ = _run_cascade(10.0, initial_heading=350.0, seconds=60.0)
    final_err = rec.final_heading_error_deg(10.0)
    assert final_err <= 4.0, f"final heading error {final_err} deg, want <= 4.0"
    # Rotation direction: the recorded ROT samples should be predominantly
    # positive (starboard) -- mean over the run.
    mean_rot = sum(s.rot_dps for s in rec.samples) / len(rec.samples)
    assert mean_rot > 0.0