AR-Autopilot/arautopilot/tests/test_pid_config.py

"""Tests for ``arautopilot.core.pid_config``."""

from __future__ import annotations

import pytest
from pydantic import ValidationError

from arautopilot.core.pid_config import (
    AccessLevel,
    GainSchedulePoint,
    PidConfig,
    PidGains,
    interpolate_gains,
)


def _base_kwargs(**overrides: object) -> dict[str, object]:
    base = {
        "inner_loop_base": PidGains(kp=2.5, ki=0.15, kd=0.30),
        "outer_loop_base": PidGains(kp=0.90, ki=0.02, kd=1.20),
    }
    base.update(overrides)
    return base


def test_basic_config_validates() -> None:
    cfg = PidConfig(**_base_kwargs())  # type: ignore[arg-type]
    assert cfg.inner_loop_freq_hz > cfg.outer_loop_freq_hz
    assert cfg.adaptive_max_deviation_pct == 50.0


def test_inner_must_be_faster_than_outer() -> None:
    with pytest.raises(ValidationError):
        PidConfig(**_base_kwargs(inner_loop_freq_hz=10.0, outer_loop_freq_hz=10.0))  # type: ignore[arg-type]
    with pytest.raises(ValidationError):
        PidConfig(**_base_kwargs(inner_loop_freq_hz=5.0, outer_loop_freq_hz=10.0))  # type: ignore[arg-type]


def test_negative_gains_rejected() -> None:
    with pytest.raises(ValidationError):
        PidGains(kp=-1.0)


def test_gain_schedule_must_be_sorted_by_speed() -> None:
    unsorted = [
        GainSchedulePoint(speed_knots=15.0, gains=PidGains(kp=0.9)),
        GainSchedulePoint(speed_knots=5.0,  gains=PidGains(kp=1.2)),
    ]
    with pytest.raises(ValidationError):
        PidConfig(**_base_kwargs(gain_schedule=unsorted))  # type: ignore[arg-type]


def test_gain_schedule_rejects_duplicate_speeds() -> None:
    dup = [
        GainSchedulePoint(speed_knots=10.0, gains=PidGains(kp=0.9)),
        GainSchedulePoint(speed_knots=10.0, gains=PidGains(kp=1.0)),
    ]
    with pytest.raises(ValidationError):
        PidConfig(**_base_kwargs(gain_schedule=dup))  # type: ignore[arg-type]


def test_interpolate_at_endpoints() -> None:
    schedule = [
        GainSchedulePoint(speed_knots=5.0,  gains=PidGains(kp=1.20, ki=0.03, kd=0.80)),
        GainSchedulePoint(speed_knots=28.0, gains=PidGains(kp=0.55, ki=0.01, kd=1.80)),
    ]
    lo = interpolate_gains(schedule, 5.0)
    hi = interpolate_gains(schedule, 28.0)
    assert lo == schedule[0].gains
    assert hi == schedule[1].gains


def test_interpolate_holds_below_and_above_range() -> None:
    schedule = [
        GainSchedulePoint(speed_knots=5.0,  gains=PidGains(kp=1.20)),
        GainSchedulePoint(speed_knots=28.0, gains=PidGains(kp=0.55)),
    ]
    assert interpolate_gains(schedule, 0.0).kp == pytest.approx(1.20)
    assert interpolate_gains(schedule, 100.0).kp == pytest.approx(0.55)


def test_interpolate_linear_midpoint() -> None:
    schedule = [
        GainSchedulePoint(speed_knots=5.0,  gains=PidGains(kp=1.00, ki=0.04, kd=0.80)),
        GainSchedulePoint(speed_knots=15.0, gains=PidGains(kp=0.50, ki=0.02, kd=1.20)),
    ]
    # Midpoint at 10 kn should be the midpoint of every gain.
    mid = interpolate_gains(schedule, 10.0)
    assert mid.kp == pytest.approx(0.75)
    assert mid.ki == pytest.approx(0.03)
    assert mid.kd == pytest.approx(1.00)


def test_interpolate_empty_raises() -> None:
    with pytest.raises(ValueError):
        interpolate_gains([], 10.0)


def test_adaptive_bound_enforces_50_percent_envelope() -> None:
    """Brief section 6: 'ganancias adaptativas nunca salen de ±50% respecto a las base'."""
    cfg = PidConfig(**_base_kwargs(adaptive_max_deviation_pct=50.0))  # type: ignore[arg-type]
    base = cfg.outer_loop_base  # kp=0.90, ki=0.02, kd=1.20

    # Within bound
    assert cfg.is_within_adaptive_bound(PidGains(kp=base.kp * 1.49, ki=base.ki, kd=base.kd))
    assert cfg.is_within_adaptive_bound(PidGains(kp=base.kp * 0.51, ki=base.ki, kd=base.kd))

    # Outside bound
    assert not cfg.is_within_adaptive_bound(PidGains(kp=base.kp * 1.51, ki=base.ki, kd=base.kd))
    assert not cfg.is_within_adaptive_bound(PidGains(kp=base.kp * 0.49, ki=base.ki, kd=base.kd))


def test_adaptive_bound_with_zero_base_requires_zero_candidate() -> None:
    cfg = PidConfig(
        **_base_kwargs(  # type: ignore[arg-type]
            outer_loop_base=PidGains(kp=1.0, ki=0.0, kd=0.0),
        )
    )
    assert cfg.is_within_adaptive_bound(PidGains(kp=1.0, ki=0.0, kd=0.0))
    assert not cfg.is_within_adaptive_bound(PidGains(kp=1.0, ki=0.01, kd=0.0))


def test_adaptive_max_deviation_capped_at_50() -> None:
    # Brief says ±50% is the hard ceiling. The model should refuse higher.
    with pytest.raises(ValidationError):
        PidConfig(**_base_kwargs(adaptive_max_deviation_pct=60.0))  # type: ignore[arg-type]


def test_access_level_enum_has_three_levels() -> None:
    assert {a.value for a in AccessLevel} == {"operator", "technician", "integrator"}