AR-Autopilot/arautopilot/tests/test_adaptive_tuner.py

"""Tests for AdaptiveTuner -- Sprint 8."""

from __future__ import annotations

import pytest

from arautopilot.core.adaptive_tuner import AdaptiveTuner
from arautopilot.core.pid_config import PidConfig, PidGains


def _make_config(
    adaptive_enabled: bool = True,
    adaptive_max_deviation_pct: float = 50.0,
) -> PidConfig:
    return PidConfig(
        inner_loop_base=PidGains(kp=1.0, ki=0.1, kd=0.05),
        outer_loop_base=PidGains(kp=2.0, ki=0.2, kd=0.1),
        adaptive_enabled=adaptive_enabled,
        adaptive_max_deviation_pct=adaptive_max_deviation_pct,
    )


def _make_tuner(
    kp: float = 2.0,
    ki: float = 0.4,
    adaptive_max_deviation_pct: float = 50.0,
    window_steps: int = 10,
    step_pct: float = 0.1,
) -> AdaptiveTuner:
    config = PidConfig(
        inner_loop_base=PidGains(kp=1.0, ki=0.1, kd=0.05),
        outer_loop_base=PidGains(kp=kp, ki=ki, kd=0.1),
        adaptive_enabled=True,
        adaptive_max_deviation_pct=adaptive_max_deviation_pct,
    )
    base = PidGains(kp=kp, ki=ki, kd=0.1)
    return AdaptiveTuner(config=config, base_gains=base, window_steps=window_steps, step_pct=step_pct)


class TestAdaptiveDisabled:
    def test_returns_none_when_disabled(self):
        config = _make_config(adaptive_enabled=False)
        tuner = AdaptiveTuner(
            config=config,
            base_gains=PidGains(kp=2.0, ki=0.2, kd=0.1),
        )
        for _ in range(200):
            result = tuner.step(5.0)
        assert result is None


class TestBufferFill:
    def test_returns_none_before_buffer_full(self):
        tuner = _make_tuner(window_steps=20)
        for i in range(19):
            assert tuner.step(5.0) is None

    def test_may_adapt_once_buffer_is_full(self):
        tuner = _make_tuner(window_steps=10, step_pct=0.1)
        result = None
        for _ in range(10):
            result = tuner.step(5.0)  # large persistent error → increase Kp
        # Should have triggered an adaptation on the 10th step
        assert result is not None


class TestOscillationDetection:
    def test_oscillating_signal_reduces_kp(self):
        tuner = _make_tuner(window_steps=10, kp=2.0, step_pct=0.1)
        original_kp = tuner._current_kp
        # Fill buffer with alternating signs (100% sign flips)
        result = None
        for i in range(10):
            err = 3.0 if i % 2 == 0 else -3.0
            result = tuner.step(err)
        # After oscillation detection, Kp should be reduced
        if result is not None:
            assert result.kp < original_kp

    def test_steady_error_increases_kp(self):
        tuner = _make_tuner(window_steps=10, kp=2.0, step_pct=0.1, ki=0.2)
        original_kp = tuner._current_kp
        result = None
        for _ in range(10):
            result = tuner.step(5.0)  # sustained positive error
        assert result is not None
        assert result.kp > original_kp

    def test_small_error_within_deadband_no_adapt(self):
        tuner = _make_tuner(window_steps=10)
        tuner.dead_band_deg = 2.0
        result = None
        for _ in range(10):
            result = tuner.step(0.5)  # within dead band
        assert result is None


class TestBoundsClamping:
    def test_kp_never_exceeds_upper_bound(self):
        tuner = _make_tuner(kp=2.0, adaptive_max_deviation_pct=20.0, window_steps=5, step_pct=0.5)
        for _ in range(200):
            tuner.step(10.0)  # large error, keep trying to increase
        assert tuner._current_kp <= 2.0 * 1.20 + 1e-9

    def test_kp_never_below_lower_bound(self):
        tuner = _make_tuner(kp=2.0, adaptive_max_deviation_pct=20.0, window_steps=5, step_pct=0.5)
        for i in range(200):
            err = 5.0 if i % 2 == 0 else -5.0
        assert tuner._current_kp >= 2.0 * 0.80 - 1e-9

    def test_ki_stays_within_bound(self):
        tuner = _make_tuner(kp=2.0, ki=0.4, adaptive_max_deviation_pct=30.0, window_steps=5, step_pct=0.5)
        for _ in range(200):
            tuner.step(10.0)
        assert tuner._current_ki <= 0.4 * 1.30 + 1e-9

    def test_kd_unchanged_after_adaptation(self):
        tuner = _make_tuner(window_steps=10, step_pct=0.1)
        original_kd = tuner._current_kd
        for _ in range(50):
            tuner.step(5.0)
        assert tuner._current_kd == pytest.approx(original_kd)


class TestKiRatioPreservation:
    def test_ki_kp_ratio_preserved_after_adapt(self):
        tuner = _make_tuner(kp=2.0, ki=0.4, window_steps=10, step_pct=0.1)
        base_ratio = 0.4 / 2.0
        result = None
        for _ in range(10):
            result = tuner.step(5.0)
        if result is not None:
            new_ratio = result.ki / result.kp
            assert new_ratio == pytest.approx(base_ratio, rel=0.01)


class TestReset:
    def test_reset_restores_base_gains(self):
        tuner = _make_tuner(kp=2.0, ki=0.4, window_steps=10, step_pct=0.1)
        for _ in range(10):
            tuner.step(5.0)
        tuner.reset()
        assert tuner._current_kp == pytest.approx(2.0)
        assert tuner._current_ki == pytest.approx(0.4)

    def test_reset_clears_buffer(self):
        tuner = _make_tuner(window_steps=10)
        for _ in range(8):
            tuner.step(5.0)
        tuner.reset()
        assert len(tuner._error_buffer) == 0

    def test_current_gains_property(self):
        tuner = _make_tuner(kp=2.0, ki=0.4)
        g = tuner.current_gains
        assert g.kp == pytest.approx(2.0)
        assert g.ki == pytest.approx(0.4)