"""Unit tests for :mod:`gowelcome.control.pid` (PIDController + normalize_angle).

Pure / stdlib only -- no cv2, ultralytics, SDK or hardware.
"""

from __future__ import annotations

import math

import pytest

from gowelcome.control.pid import PIDController, normalize_angle


# --------------------------------------------------------------------------- #
# PIDController
# --------------------------------------------------------------------------- #
def test_proportional_output():
    """With ki=kd=0 the output is exactly kp * error."""
    pid = PIDController(kp=2.0)
    assert pid.update(3.0, dt=0.1) == pytest.approx(6.0)
    # The sign of the output tracks the sign of the error.
    assert pid.update(-1.5, dt=0.1) == pytest.approx(-3.0)


def test_integral_accumulates():
    """The integral term builds up over successive steps of constant error."""
    pid = PIDController(kp=0.0, ki=1.0)
    out1 = pid.update(1.0, dt=0.5)   # integral = 0.5 -> out 0.5
    out2 = pid.update(1.0, dt=0.5)   # integral = 1.0 -> out 1.0
    assert out2 > out1
    assert out1 == pytest.approx(0.5)
    assert out2 == pytest.approx(1.0)


def test_integral_limit_clamps_windup():
    """integral_limit caps the accumulated integral (anti-windup)."""
    pid = PIDController(kp=0.0, ki=1.0, integral_limit=0.3)
    # Drive a large constant error for several steps; integral must not exceed
    # the clamp regardless of how long we accumulate.
    out = 0.0
    for _ in range(20):
        out = pid.update(5.0, dt=0.5)
    assert pid.integral == pytest.approx(0.3)
    assert out == pytest.approx(0.3)


def test_deadband_zeroes_integral_and_derivative():
    """Inside the deadband the integral and derivative terms are suppressed.

    The proportional term still acts (per the controller's documented
    behaviour), but integral build-up and derivative kick are zeroed.
    """
    pid = PIDController(kp=1.0, ki=10.0, kd=10.0, deadband=0.5)
    # Error below the deadband: integral and derivative contributions dropped,
    # so the output collapses to just kp * error.
    out = pid.update(0.2, dt=0.1)
    assert pid.integral == pytest.approx(0.0)
    assert out == pytest.approx(0.2)  # 1.0 * 0.2 only


def test_output_limits_clamp():
    """output_limits saturate the controller output on both sides."""
    pid = PIDController(kp=10.0, output_limits=(-1.0, 1.0))
    assert pid.update(5.0, dt=0.1) == pytest.approx(1.0)    # would be 50 -> clamp hi
    assert pid.update(-5.0, dt=0.1) == pytest.approx(-1.0)  # would be -50 -> clamp lo
    # Within the band, no clamping.
    assert pid.update(0.05, dt=0.1) == pytest.approx(0.5)


def test_reset_clears_state():
    """reset() clears the integral accumulator and previous-error memory."""
    pid = PIDController(kp=0.0, ki=1.0)
    pid.update(1.0, dt=1.0)
    assert pid.integral != 0.0
    pid.reset()
    assert pid.integral == pytest.approx(0.0)
    assert pid.prev_error == pytest.approx(0.0)


def test_nonpositive_dt_is_guarded():
    """dt <= 0 must not corrupt the integral or blow up the derivative."""
    pid = PIDController(kp=1.0, ki=1.0, kd=1.0)
    out = pid.update(2.0, dt=0.0)
    # Integral unchanged (still zero) and derivative treated as zero -> P only.
    assert pid.integral == pytest.approx(0.0)
    assert out == pytest.approx(2.0)


# --------------------------------------------------------------------------- #
# normalize_angle
# --------------------------------------------------------------------------- #
def test_normalize_angle_wraps_into_range():
    """Angles wrap into (-pi, pi]."""
    assert normalize_angle(0.0) == pytest.approx(0.0)
    assert normalize_angle(math.pi / 2) == pytest.approx(math.pi / 2)
    # 3*pi wraps to +/- pi (same point on the circle).
    assert abs(normalize_angle(3.0 * math.pi)) == pytest.approx(math.pi)
    # Just past +pi wraps to near -pi.
    assert normalize_angle(math.pi + 0.1) == pytest.approx(-math.pi + 0.1)
    # A large negative angle (whole 2*pi turns) wraps back to the same point.
    wrapped = normalize_angle(-4.0 * math.pi + 0.25)
    assert -math.pi <= wrapped <= math.pi
    assert wrapped == pytest.approx(0.25, abs=1e-6)


def test_normalize_angle_is_periodic():
    """Adding full 2*pi turns does not change the normalized result."""
    for base in (0.3, -1.2, 2.9):
        assert normalize_angle(base) == pytest.approx(
            normalize_angle(base + 2.0 * math.pi), abs=1e-9
        )