AR-Autopilot/arautopilot/tests/test_integration_cascade.py

"""Integration: full cascade PID closed-loop simulation -- Sprint 9.

Tests that the three-layer cascade (outer heading PID → inner rudder PID →
rudder dynamics → vessel heading) converges to a heading setpoint within the
acceptance limits of the brief.

No mocks — all four simulator modules run together.
"""

from __future__ import annotations

import math

import pytest

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


def _run_cascade(
    *,
    initial_heading: float,
    setpoint_deg: float,
    duration_s: float = 120.0,
    outer_dt: float = 0.1,    # 10 Hz
    inner_dt: float = 0.02,   # 50 Hz
    speed_kn: float = 10.0,
) -> tuple[float, float]:
    """Return (final_heading_error_deg, max_rudder_deg_reached)."""
    outer = PidOuter(PidOuterConfig(
        base_kp=0.9, base_ki=0.02, base_kd=1.2,
        deadband_deg=0.5, rot_ff_gain=1.5, max_rudder_deg=35.0,
    ))
    inner = PidInner(PidInnerConfig(
        kp=2.5, ki=0.15, kd=0.30,
        deadband_deg=0.5, min_useful_pwm_pct=12.0,
    ))
    rudder = RudderSimulator(RudderDynamicsConfig(
        actuator_gain=0.2, friction=4.0, max_angle_deg=35.0,
    ))
    vessel = VesselHeadingSimulator(VesselHeadingConfig(
        rudder_response_gain=0.18, yaw_damping=0.8, speed_kn=speed_kn,
    ))
    vessel.reset(heading_deg=initial_heading)

    t = 0.0
    max_rudder = 0.0
    rudder_setpoint = 0.0
    outer_acc = 0.0

    while t < duration_s:
        # Outer loop tick (10 Hz)
        if outer_acc >= outer_dt - 1e-9:
            outer_acc = 0.0
            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,
                allowed=True,
            )

        # Inner loop tick (50 Hz)
        pwm = inner.step(
            setpoint_deg=rudder_setpoint,
            measured_deg=rudder.state.angle_deg,
            allowed=True,
        )
        rudder.step(dt=inner_dt, pwm_pct=pwm)
        vessel.step(dt=inner_dt, rudder_deg=rudder.state.angle_deg, speed_kn=speed_kn)

        max_rudder = max(max_rudder, abs(rudder.state.angle_deg))
        outer_acc += inner_dt
        t += inner_dt

    final_error = abs(heading_error_deg(setpoint_deg, vessel.state.heading_deg))
    return final_error, max_rudder


class TestCascadeConvergence:
    def test_small_heading_change_converges(self):
        err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=10.0)
        assert err < 6.0, f"Did not converge: final error = {err:.2f} deg"

    def test_90_degree_turn_converges(self):
        err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=90.0, duration_s=180.0)
        assert err < 8.0, f"Did not converge: final error = {err:.2f} deg"

    def test_wrap_around_north_converges(self):
        # 350° → 10° = +20 degrees through north
        err, _ = _run_cascade(initial_heading=350.0, setpoint_deg=10.0)
        assert err < 6.0, f"Wrap-around did not converge: final error = {err:.2f} deg"

    def test_180_degree_turn_converges(self):
        err, _ = _run_cascade(initial_heading=0.0, setpoint_deg=180.0, duration_s=240.0)
        assert err < 10.0, f"180° turn did not converge: final error = {err:.2f} deg"

    def test_rudder_stays_within_limits(self):
        _, max_rudder = _run_cascade(initial_heading=0.0, setpoint_deg=90.0)
        assert max_rudder <= 35.0 + 1e-6

    def test_slow_speed_converges(self):
        # At low speed the vessel turns slower but must still converge
        err, _ = _run_cascade(
            initial_heading=0.0, setpoint_deg=30.0, speed_kn=3.0, duration_s=240.0
        )
        assert err < 10.0, f"Low-speed case did not converge: final error = {err:.2f} deg"

    def test_same_heading_no_overshoot(self):
        # Already on setpoint: rudder should barely move
        err, max_rudder = _run_cascade(
            initial_heading=90.0, setpoint_deg=90.0, duration_s=30.0
        )
        assert err < 2.0
        assert max_rudder < 5.0


class TestCascadeEnergyBudget:
    def test_integrator_does_not_wind_up(self):
        # After convergence, integrator should be small (no sustained error)
        outer = PidOuter(PidOuterConfig(base_kp=0.9, base_ki=0.02, base_kd=1.2))
        inner = PidInner()
        rudder = RudderSimulator()
        vessel = VesselHeadingSimulator()
        vessel.reset(heading_deg=0.0)
        setpoint = 45.0
        outer_acc = 0.0
        rudder_sp = 0.0
        for _ in range(int(60.0 / 0.02)):
            if outer_acc >= 0.1 - 1e-9:
                outer_acc = 0.0
                rudder_sp = outer.step(
                    heading_setpoint_deg=setpoint,
                    heading_measured_deg=vessel.state.heading_deg,
                    rate_of_turn_dps=vessel.state.rate_of_turn_dps,
                    speed_kn=10.0,
                    allowed=True,
                )
            pwm = inner.step(setpoint_deg=rudder_sp, measured_deg=rudder.state.angle_deg, allowed=True)
            rudder.step(dt=0.02, pwm_pct=pwm)
            vessel.step(dt=0.02, rudder_deg=rudder.state.angle_deg)
            outer_acc += 0.02
        # Integral should have settled to a small value after convergence
        assert abs(outer.state.integral) < 20.0