"""Minimal PID controller + angle helper.

Adapted from ``Robot/go2_dog_behavior/simple_controllers.py``: keeps the
anti-windup integral clamp, the deadband-zeroes-integral-and-derivative
behaviour, and output saturation. The output-deadband compensation feature of
the original is intentionally dropped -- the visual servo does its own
min-forward gating, so it is not needed here.

Pure module: only depends on :mod:`math`, so it imports and unit-tests with no
heavy/optional dependencies.
"""

from __future__ import annotations

import math
from typing import Optional, Tuple


def normalize_angle(angle: float) -> float:
    """Wrap ``angle`` (radians) into ``[-pi, pi]``.

    Args:
        angle: Angle in radians, any magnitude.

    Returns:
        The equivalent angle in ``[-pi, pi]`` via ``atan2(sin, cos)``.
    """
    return math.atan2(math.sin(angle), math.cos(angle))


class PIDController:
    """A scalar PID controller with anti-windup, deadband, and output clamp.

    The controller is stateful (it accumulates an integral and remembers the
    previous error for the derivative term); call :meth:`reset` to clear that
    state, e.g. when re-acquiring a target.
    """

    def __init__(
        self,
        kp: float,
        ki: float = 0.0,
        kd: float = 0.0,
        output_limits: Tuple[Optional[float], Optional[float]] = (None, None),
        integral_limit: Optional[float] = None,
        deadband: float = 0.0,
    ) -> None:
        """Configure the gains and limits.

        Args:
            kp: Proportional gain.
            ki: Integral gain.
            kd: Derivative gain.
            output_limits: ``(min_output, max_output)``; use ``None`` on either
                side to leave that bound unclamped.
            integral_limit: Symmetric clamp on the accumulated integral
                (anti-windup); ``None`` disables it.
            deadband: When ``abs(error)`` is below this, the integral and
                derivative terms are zeroed (the proportional term still acts).
        """
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.min_output, self.max_output = output_limits
        self.integral_limit = integral_limit
        self.deadband = deadband
        self.integral: float = 0.0
        self.prev_error: float = 0.0

    def update(self, error: float, dt: float) -> float:
        """Advance the controller one step and return the control output.

        Args:
            error: The current error (setpoint - measurement, or any signed
                error convention the caller prefers).
            dt: Timestep in seconds. Non-positive ``dt`` is guarded: the
                derivative term is taken as zero and the integral is left
                unchanged for that step.

        Returns:
            The (optionally clamped) PID output.
        """
        # Integrate (skip when dt is non-positive to avoid bogus accumulation).
        if dt > 0:
            self.integral += error * dt
            if self.integral_limit is not None:
                self.integral = max(
                    -self.integral_limit, min(self.integral, self.integral_limit)
                )
            derivative = (error - self.prev_error) / dt
        else:
            derivative = 0.0

        # Inside the deadband, suppress integral build-up and derivative kick.
        if abs(error) < self.deadband:
            self.integral = 0.0
            derivative = 0.0

        output = self.kp * error + self.ki * self.integral + self.kd * derivative

        # Saturate to the configured output limits.
        if self.max_output is not None:
            output = min(self.max_output, output)
        if self.min_output is not None:
            output = max(self.min_output, output)

        self.prev_error = error
        return output

    def reset(self) -> None:
        """Clear the integral accumulator and the remembered previous error."""
        self.integral = 0.0
        self.prev_error = 0.0