"""
incline_detector.py — Terrain incline detection from IMU pitch (Issue #142).

Method
──────
  For a ground robot (rover/tank), the vehicle body tilts with the terrain,
  so IMU pitch ≈ terrain incline + vibration noise.

  A first-order discrete low-pass filter separates the slow incline component
  from fast vibrations and balance corrections:

      incline_filtered[n] = (1−α) · incline_filtered[n−1]  +  α · pitch_raw[n]
      α = dt / (τ + dt)

  The motor bias required to compensate for gravity on the incline is:

      bias_ms = k_bias · sin(incline_rad)

  Positive incline = uphill (front higher than rear) → positive forward bias
  needed to counteract gravity pulling the robot backward.

Pure module — no ROS2 dependency.
"""

from __future__ import annotations

import math


class InclineDetector:
    """
    Incline estimator via first-order low-pass filter on IMU pitch.

    Parameters
    ----------
    lpf_tau_s       : low-pass filter time constant (s); larger = smoother
    control_rate_hz : expected update rate (Hz)
    max_incline_rad : hard clamp on reported incline magnitude (rad)
    """

    def __init__(
        self,
        lpf_tau_s: float = 1.5,
        control_rate_hz: float = 50.0,
        max_incline_rad: float = 0.70,   # ≈ 40°
    ):
        self._tau = max(1e-3, lpf_tau_s)
        self._dt = 1.0 / max(1.0, control_rate_hz)
        self._max = abs(max_incline_rad)

        self._incline_rad: float = 0.0
        self._initialised: bool = False

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

    @property
    def incline_rad(self) -> float:
        return self._incline_rad

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

    def update(self, pitch_rad: float, dt: float | None = None) -> float:
        """
        Submit one pitch sample and return the filtered incline estimate.

        Parameters
        ----------
        pitch_rad : IMU pitch angle (rad); +ve = front-up / uphill
        dt        : elapsed time since last sample (s); uses 1/rate if None

        Returns
        -------
        Filtered incline in radians, clamped to ±max_incline_rad.
        """
        step = dt if dt is not None and dt > 0.0 else self._dt

        if not self._initialised:
            # Seed filter with first measurement to avoid large initial transient
            self._incline_rad = pitch_rad
            self._initialised = True
            return max(-self._max, min(self._max, self._incline_rad))

        alpha = step / (self._tau + step)
        self._incline_rad = (1.0 - alpha) * self._incline_rad + alpha * pitch_rad
        return max(-self._max, min(self._max, self._incline_rad))

    def reset(self) -> None:
        self._incline_rad = 0.0
        self._initialised = False

    # ── Motor bias ────────────────────────────────────────────────────────────

    @staticmethod
    def motor_bias(incline_rad: float, k_bias: float = 1.0) -> float:
        """
        Forward motor bias (m/s) to compensate for gravitational pull on incline.

        Positive bias = add forward thrust for uphill.
        Negative bias = reduce thrust / add braking for downhill.

        Parameters
        ----------
        incline_rad : filtered incline angle (rad)
        k_bias      : tuning gain; scale bias magnitude
        """
        return k_bias * math.sin(incline_rad)
