"""
slip_detector.py — Wheel slip detection for SaltyBot (Issue #142).

Method
──────
  Compare commanded speed (from /cmd_vel) to measured speed (from odometry).
  When the robot is commanded to move but the measured speed falls significantly
  below the command, the difference is attributed to wheel/track slip.

      slip_ratio = max(0, |v_cmd| − |v_actual|) / |v_cmd|

  slip_ratio = 0.0  →  no slip (actual matches commanded)
  slip_ratio = 1.0  →  full slip (wheels spinning, no forward motion)

  Detection is suppressed below `min_speed_ms` to avoid noise at low speed.

  A smoothed slip estimate is maintained via exponential moving average
  (alpha per update) to reduce transient spikes.

Pure module — no ROS2 dependency.
"""

from __future__ import annotations


class SlipDetector:
    """
    Wheel/track slip detector from commanded vs measured speed.

    Parameters
    ----------
    slip_threshold  : slip_ratio above which is_slipping = True
    min_speed_ms    : minimum |v_cmd| to attempt detection (avoids noise)
    smoothing_alpha : EMA coefficient ∈ (0, 1]; 1 = no smoothing
    """

    def __init__(
        self,
        slip_threshold: float = 0.30,
        min_speed_ms: float = 0.15,
        smoothing_alpha: float = 0.20,
    ):
        self._threshold = max(0.0, min(1.0, slip_threshold))
        self._min_speed = abs(min_speed_ms)
        self._alpha = max(1e-6, min(1.0, smoothing_alpha))
        self._smoothed: float = 0.0

    # ── Properties ────────────────────────────────────────────────────────────

    @property
    def smoothed_slip(self) -> float:
        """EMA-smoothed slip ratio."""
        return self._smoothed

    # ── Update ────────────────────────────────────────────────────────────────

    def update(
        self,
        cmd_speed_ms: float,
        actual_speed_ms: float,
    ) -> tuple[float, bool]:
        """
        Compute slip from one speed pair.

        Parameters
        ----------
        cmd_speed_ms    : commanded forward speed (m/s); signed
        actual_speed_ms : measured forward speed (m/s); signed (from odom)

        Returns
        -------
        (slip_ratio, is_slipping)
        slip_ratio  : 0.0 – 1.0 (instantaneous, unsmoothed)
        is_slipping : True if smoothed slip exceeds threshold
        """
        abs_cmd = abs(cmd_speed_ms)

        if abs_cmd < self._min_speed:
            # Decay smoothed slip toward zero when not commanding movement
            self._smoothed = (1.0 - self._alpha) * self._smoothed
            return 0.0, False

        abs_actual = abs(actual_speed_ms)
        raw_slip = max(0.0, min(1.0, (abs_cmd - abs_actual) / abs_cmd))

        self._smoothed = (1.0 - self._alpha) * self._smoothed + self._alpha * raw_slip

        return raw_slip, self._smoothed > self._threshold

    def reset(self) -> None:
        self._smoothed = 0.0
