"""
_velocity_ramp.py — Acceleration/deceleration limiter for cmd_vel (Issue #350).

Pure-Python library (no ROS2 dependencies) for full unit-test coverage.

Behaviour
---------
The VelocityRamp class applies independent rate limits to the linear-x and
angular-z components of a 2D velocity command:

  - Linear  acceleration  limit : max_lin_accel  (m/s²)
  - Linear  deceleration  limit : max_lin_decel  (m/s²)  — may differ from accel
  - Angular acceleration  limit : max_ang_accel  (rad/s²)
  - Angular deceleration  limit : max_ang_decel  (rad/s²)

"Deceleration" applies when the magnitude of the velocity is *decreasing*
(including moving toward zero from either sign direction), while "acceleration"
applies when the magnitude is increasing.

Emergency stop
--------------
When both linear and angular targets are exactly 0.0 the ramp is bypassed and
the output is forced to (0.0, 0.0) immediately.  This allows a watchdog or
safety node to halt the robot instantly without waiting for the deceleration
ramp.

Usage
-----
    ramp = VelocityRamp(dt=0.02)          # 50 Hz
    out_lin, out_ang = ramp.step(1.0, 0.0)   # target linear=1 m/s, angular=0
    out_lin, out_ang = ramp.step(1.0, 0.0)   # ramp climbs toward 1.0 m/s
    out_lin, out_ang = ramp.step(0.0, 0.0)   # emergency stop → (0.0, 0.0)
"""

from __future__ import annotations

from dataclasses import dataclass


@dataclass
class RampParams:
    """Acceleration / deceleration limits for one velocity axis."""
    max_accel: float   # magnitude / second — applied when |v| increasing
    max_decel: float   # magnitude / second — applied when |v| decreasing


def _ramp_axis(
    current: float,
    target:  float,
    params:  RampParams,
    dt:      float,
) -> float:
    """
    Advance *current* one timestep toward *target* with rate limiting.

    Parameters
    ----------
    current : current velocity on this axis
    target  : desired velocity on this axis
    params  : accel / decel limits
    dt      : timestep in seconds

    Returns
    -------
    New velocity, clamped so the change per step never exceeds the limits.

    Notes on accel vs decel selection
    ----------------------------------
    We treat the motion as decelerating when the target is closer to zero
    than the current value, i.e. |target| < |current| or they have opposite
    signs.  In all other cases we use the acceleration limit.
    """
    delta = target - current

    # Choose limit: decel if velocity magnitude is falling, else accel.
    is_decelerating = (
        abs(target) < abs(current)
        or (current > 0 and target < 0)
        or (current < 0 and target > 0)
    )
    limit = params.max_decel if is_decelerating else params.max_accel

    max_change = limit * dt
    if abs(delta) <= max_change:
        return target
    return current + max_change * (1.0 if delta > 0 else -1.0)


class VelocityRamp:
    """
    Smooths a stream of (linear_x, angular_z) velocity commands by applying
    configurable acceleration and deceleration limits.

    Parameters
    ----------
    dt              : timestep in seconds (must match the control loop rate)
    max_lin_accel   : maximum linear  acceleration  (m/s²)
    max_lin_decel   : maximum linear  deceleration  (m/s²); defaults to max_lin_accel
    max_ang_accel   : maximum angular acceleration  (rad/s²)
    max_ang_decel   : maximum angular deceleration  (rad/s²); defaults to max_ang_accel
    """

    def __init__(
        self,
        dt:            float = 0.02,    # 50 Hz
        max_lin_accel: float = 0.5,     # m/s²
        max_lin_decel: float | None = None,
        max_ang_accel: float = 1.0,     # rad/s²
        max_ang_decel: float | None = None,
    ) -> None:
        if dt <= 0:
            raise ValueError(f'dt must be positive, got {dt}')
        if max_lin_accel <= 0:
            raise ValueError(f'max_lin_accel must be positive, got {max_lin_accel}')
        if max_ang_accel <= 0:
            raise ValueError(f'max_ang_accel must be positive, got {max_ang_accel}')

        self._dt  = dt
        self._lin = RampParams(
            max_accel=max_lin_accel,
            max_decel=max_lin_decel if max_lin_decel is not None else max_lin_accel,
        )
        self._ang = RampParams(
            max_accel=max_ang_accel,
            max_decel=max_ang_decel if max_ang_decel is not None else max_ang_accel,
        )

        self._cur_lin: float = 0.0
        self._cur_ang: float = 0.0

    # ── Public API ────────────────────────────────────────────────────────────

    def step(self, target_lin: float, target_ang: float) -> tuple[float, float]:
        """
        Advance the ramp one timestep.

        Parameters
        ----------
        target_lin : desired linear  velocity (m/s)
        target_ang : desired angular velocity (rad/s)

        Returns
        -------
        (smoothed_linear, smoothed_angular) tuple.

        If both target_lin and target_ang are exactly 0.0, an emergency stop
        is applied and (0.0, 0.0) is returned immediately.
        """
        # Emergency stop: bypass ramp entirely
        if target_lin == 0.0 and target_ang == 0.0:
            self._cur_lin = 0.0
            self._cur_ang = 0.0
            return 0.0, 0.0

        self._cur_lin = _ramp_axis(self._cur_lin, target_lin, self._lin, self._dt)
        self._cur_ang = _ramp_axis(self._cur_ang, target_ang, self._ang, self._dt)
        return self._cur_lin, self._cur_ang

    def reset(self) -> None:
        """Reset internal state to zero (e.g. on node restart or re-enable)."""
        self._cur_lin = 0.0
        self._cur_ang = 0.0

    @property
    def current_linear(self) -> float:
        """Current smoothed linear velocity (m/s)."""
        return self._cur_lin

    @property
    def current_angular(self) -> float:
        """Current smoothed angular velocity (rad/s)."""
        return self._cur_ang

    @property
    def dt(self) -> float:
        return self._dt

    # ── Derived ───────────────────────────────────────────────────────────────

    def steps_to_reach(self, target_lin: float, target_ang: float) -> int:
        """
        Estimate how many steps() are needed to reach the target from the
        current state (useful for test assertions).

        This is an approximation based on the worst-case axis; it does not
        account for the emergency-stop shortcut.
        """
        lin_steps = 0
        ang_steps = 0
        if self._lin.max_accel > 0:
            lin_steps = int(abs(target_lin - self._cur_lin) / (self._lin.max_accel * self._dt)) + 1
        if self._ang.max_accel > 0:
            ang_steps = int(abs(target_ang - self._cur_ang) / (self._ang.max_accel * self._dt)) + 1
        return max(lin_steps, ang_steps)
