"""
test_velocity_ramp.py — Unit tests for _velocity_ramp.py (Issue #350).

Covers:
  - Linear ramp-up (acceleration)
  - Linear ramp-down (deceleration)
  - Angular ramp-up / ramp-down
  - Asymmetric accel vs decel limits
  - Emergency stop (both targets = 0.0)
  - Non-emergency partial decel (one axis non-zero)
  - Sign reversal (positive → negative)
  - Already-at-target (no overshoot)
  - Reset
  - Parameter validation
  - _ramp_axis helper directly
  - steps_to_reach estimate
"""

from __future__ import annotations

import math
import pytest

from saltybot_bringup._velocity_ramp import (
    RampParams,
    VelocityRamp,
    _ramp_axis,
)


# ─────────────────────────────────────────────────────────────────────────────
# _ramp_axis helper
# ─────────────────────────────────────────────────────────────────────────────

class TestRampAxis:

    def _p(self, accel=1.0, decel=1.0) -> RampParams:
        return RampParams(max_accel=accel, max_decel=decel)

    def test_advances_toward_target(self):
        val = _ramp_axis(0.0, 1.0, self._p(accel=0.5), dt=1.0)
        assert abs(val - 0.5) < 1e-9

    def test_reaches_target_exactly(self):
        val = _ramp_axis(0.9, 1.0, self._p(accel=0.5), dt=1.0)
        assert val == 1.0  # remaining gap 0.1 < max_change 0.5

    def test_no_overshoot(self):
        val = _ramp_axis(0.8, 1.0, self._p(accel=5.0), dt=1.0)
        assert val == 1.0

    def test_negative_direction(self):
        val = _ramp_axis(0.0, -1.0, self._p(accel=0.5), dt=1.0)
        assert abs(val - (-0.5)) < 1e-9

    def test_decel_used_when_magnitude_falling(self):
        # current=1.0, target=0.5 → magnitude falling → use decel=0.2
        val = _ramp_axis(1.0, 0.5, self._p(accel=1.0, decel=0.2), dt=1.0)
        assert abs(val - 0.8) < 1e-9

    def test_accel_used_when_magnitude_rising(self):
        # current=0.5, target=1.0 → magnitude rising → use accel=0.3
        val = _ramp_axis(0.5, 1.0, self._p(accel=0.3, decel=1.0), dt=1.0)
        assert abs(val - 0.8) < 1e-9

    def test_sign_reversal_uses_decel(self):
        # current=0.5 (positive), target=-0.5 → opposite sign → decel
        val = _ramp_axis(0.5, -0.5, self._p(accel=1.0, decel=0.1), dt=1.0)
        assert abs(val - 0.4) < 1e-9

    def test_already_at_target_no_change(self):
        val = _ramp_axis(1.0, 1.0, self._p(), dt=0.02)
        assert val == 1.0

    def test_zero_to_zero_no_change(self):
        val = _ramp_axis(0.0, 0.0, self._p(), dt=0.02)
        assert val == 0.0

    def test_small_dt(self):
        val = _ramp_axis(0.0, 10.0, self._p(accel=1.0), dt=0.02)
        assert abs(val - 0.02) < 1e-9

    def test_negative_to_less_negative(self):
        # current=-1.0, target=-0.5 → magnitude falling (decelerating)
        val = _ramp_axis(-1.0, -0.5, self._p(accel=1.0, decel=0.2), dt=1.0)
        assert abs(val - (-0.8)) < 1e-9

    def test_negative_to_more_negative(self):
        # current=-0.5, target=-1.0 → magnitude rising (accelerating)
        val = _ramp_axis(-0.5, -1.0, self._p(accel=0.3, decel=1.0), dt=1.0)
        assert abs(val - (-0.8)) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# VelocityRamp construction
# ─────────────────────────────────────────────────────────────────────────────

class TestVelocityRampConstruction:

    def test_default_params(self):
        r = VelocityRamp()
        assert r.dt == 0.02
        assert r.current_linear  == 0.0
        assert r.current_angular == 0.0

    def test_custom_dt(self):
        r = VelocityRamp(dt=0.05)
        assert r.dt == 0.05

    def test_invalid_dt_raises(self):
        with pytest.raises(ValueError):
            VelocityRamp(dt=0.0)

    def test_negative_dt_raises(self):
        with pytest.raises(ValueError):
            VelocityRamp(dt=-0.01)

    def test_invalid_lin_accel_raises(self):
        with pytest.raises(ValueError):
            VelocityRamp(max_lin_accel=0.0)

    def test_invalid_ang_accel_raises(self):
        with pytest.raises(ValueError):
            VelocityRamp(max_ang_accel=-1.0)

    def test_asymmetric_decel_stored(self):
        r = VelocityRamp(max_lin_accel=0.5, max_lin_decel=2.0)
        assert r._lin.max_accel == 0.5
        assert r._lin.max_decel == 2.0

    def test_decel_defaults_to_accel(self):
        r = VelocityRamp(max_lin_accel=0.5)
        assert r._lin.max_decel == 0.5


# ─────────────────────────────────────────────────────────────────────────────
# Linear ramp-up
# ─────────────────────────────────────────────────────────────────────────────

class TestLinearRampUp:
    """
    VelocityRamp(dt=1.0, max_lin_accel=0.5) → 0.5 m/s per step.
    """

    def _ramp(self, **kw):
        return VelocityRamp(dt=1.0, max_lin_accel=0.5, max_ang_accel=1.0, **kw)

    def test_first_step(self):
        r = self._ramp()
        lin, _ = r.step(1.0, 0.0)
        assert abs(lin - 0.5) < 1e-9

    def test_second_step(self):
        r = self._ramp()
        r.step(1.0, 0.0)
        lin, _ = r.step(1.0, 0.0)
        assert abs(lin - 1.0) < 1e-9

    def test_reaches_target(self):
        r = self._ramp()
        lin = None
        for _ in range(10):
            lin, _ = r.step(1.0, 0.0)
        assert lin == 1.0

    def test_no_overshoot(self):
        r = self._ramp()
        outputs = [r.step(1.0, 0.0)[0] for _ in range(20)]
        assert all(v <= 1.0 + 1e-9 for v in outputs)

    def test_current_linear_updated(self):
        r = self._ramp()
        r.step(1.0, 0.0)
        assert abs(r.current_linear - 0.5) < 1e-9

    def test_negative_target(self):
        r = self._ramp()
        lin, _ = r.step(-1.0, 0.0)
        assert abs(lin - (-0.5)) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# Linear deceleration
# ─────────────────────────────────────────────────────────────────────────────

class TestLinearDecel:

    def _ramp(self, decel=None):
        return VelocityRamp(
            dt=1.0, max_lin_accel=1.0,
            max_lin_decel=decel if decel else 1.0,
            max_ang_accel=1.0,
        )

    def _at_speed(self, r: VelocityRamp, speed: float) -> None:
        """Bring ramp to given linear speed."""
        for _ in range(20):
            r.step(speed, 0.0)

    def test_decel_from_1_to_0(self):
        r = self._ramp(decel=0.5)
        self._at_speed(r, 1.0)
        lin, _ = r.step(0.5, 0.0)   # slow down: target < current
        assert abs(lin - 0.5) < 0.01  # 0.5 step downward

    def test_asymmetric_faster_decel(self):
        """With max_lin_decel=2.0, deceleration is twice as fast."""
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_lin_decel=2.0, max_ang_accel=1.0)
        self._at_speed(r, 1.0)
        lin, _ = r.step(0.0, 0.0)   # decelerate — but target=0 triggers e-stop
        # e-stop bypasses ramp
        assert lin == 0.0

    def test_partial_decel_non_zero_target(self):
        """With target=0.5 and current=1.0, decel limit applies (non-zero → no e-stop)."""
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_lin_decel=2.0, max_ang_accel=1.0)
        self._at_speed(r, 2.0)
        # target=0.5 angular=0.1 (non-zero) → ramp decel applies
        lin, _ = r.step(0.5, 0.1)
        # current was 2.0, decel=2.0 → max step = 2.0 → should reach 0.5 immediately
        assert abs(lin - 0.5) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# Angular ramp
# ─────────────────────────────────────────────────────────────────────────────

class TestAngularRamp:

    def _ramp(self, **kw):
        return VelocityRamp(dt=1.0, max_lin_accel=1.0, max_ang_accel=0.5, **kw)

    def test_angular_first_step(self):
        r = self._ramp()
        _, ang = r.step(0.0, 1.0)
        # target_lin=0 & target_ang=1 → not e-stop (only ang non-zero)
        # Wait — step(0.0, 1.0): only lin=0, ang=1 → not BOTH zero → ramp applies
        assert abs(ang - 0.5) < 1e-9

    def test_angular_reaches_target(self):
        r = self._ramp()
        ang = None
        for _ in range(10):
            _, ang = r.step(0.0, 1.0)
        assert ang == 1.0

    def test_angular_decel(self):
        r = self._ramp(max_ang_decel=0.25)
        for _ in range(10):
            r.step(0.0, 1.0)
        # decel with max_ang_decel=0.25: step is 0.25 per second
        _, ang = r.step(0.0, 0.5)
        assert abs(ang - 0.75) < 1e-9

    def test_angular_negative(self):
        r = self._ramp()
        _, ang = r.step(0.0, -1.0)
        assert abs(ang - (-0.5)) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# Emergency stop
# ─────────────────────────────────────────────────────────────────────────────

class TestEmergencyStop:

    def _at_full_speed(self) -> VelocityRamp:
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_ang_accel=0.5)
        for _ in range(10):
            r.step(2.0, 2.0)
        return r

    def test_estop_returns_zero_immediately(self):
        r = self._at_full_speed()
        lin, ang = r.step(0.0, 0.0)
        assert lin == 0.0
        assert ang == 0.0

    def test_estop_updates_internal_state(self):
        r = self._at_full_speed()
        r.step(0.0, 0.0)
        assert r.current_linear  == 0.0
        assert r.current_angular == 0.0

    def test_estop_from_rest_still_zero(self):
        r = VelocityRamp(dt=0.02)
        lin, ang = r.step(0.0, 0.0)
        assert lin == 0.0 and ang == 0.0

    def test_estop_then_ramp_resumes(self):
        r = self._at_full_speed()
        r.step(0.0, 0.0)           # e-stop
        lin, _ = r.step(1.0, 0.0)  # ramp from 0 → first step
        assert lin > 0.0
        assert lin < 1.0

    def test_partial_zero_not_estop(self):
        """lin=0 but ang≠0 should NOT trigger e-stop."""
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_ang_accel=0.5)
        for _ in range(10):
            r.step(1.0, 1.0)
        # Now command lin=0, ang=0.5 — not an e-stop
        lin, ang = r.step(0.0, 0.5)
        # lin should ramp down (decel), NOT snap to 0
        assert lin > 0.0
        assert ang < 1.0   # angular also ramping down toward 0.5

    def test_negative_zero_not_estop(self):
        """step(-0.0, 0.0) — Python -0.0 == 0.0, should still e-stop."""
        r = VelocityRamp(dt=0.02)
        for _ in range(20):
            r.step(1.0, 0.0)
        lin, ang = r.step(-0.0, 0.0)
        assert lin == 0.0 and ang == 0.0


# ─────────────────────────────────────────────────────────────────────────────
# Sign reversal
# ─────────────────────────────────────────────────────────────────────────────

class TestSignReversal:

    def test_reversal_decelerates_first(self):
        """Reversing from +1 to -1 should pass through 0, not jump instantly."""
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_lin_decel=0.5, max_ang_accel=1.0)
        for _ in range(5):
            r.step(1.0, 0.1)     # reach ~1.0 forward; use ang=0.1 to avoid e-stop
        outputs = []
        for _ in range(15):
            lin, _ = r.step(-1.0, 0.1)
            outputs.append(lin)
        # Velocity should pass through zero on its way to -1
        assert any(v < 0 for v in outputs), "Should cross zero during reversal"
        assert any(v > 0 for v in outputs[:3]), "Should start from positive side"

    def test_reversal_completes(self):
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_lin_decel=0.5, max_ang_accel=1.0)
        for _ in range(5):
            r.step(1.0, 0.1)
        for _ in range(20):
            lin, _ = r.step(-1.0, 0.1)
        assert abs(lin - (-1.0)) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# Reset
# ─────────────────────────────────────────────────────────────────────────────

class TestReset:

    def test_reset_clears_state(self):
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_ang_accel=0.5)
        r.step(2.0, 2.0)
        r.reset()
        assert r.current_linear  == 0.0
        assert r.current_angular == 0.0

    def test_after_reset_ramp_from_zero(self):
        r = VelocityRamp(dt=1.0, max_lin_accel=0.5, max_ang_accel=0.5)
        for _ in range(5):
            r.step(2.0, 0.0)
        r.reset()
        lin, _ = r.step(1.0, 0.0)
        assert abs(lin - 0.5) < 1e-9


# ─────────────────────────────────────────────────────────────────────────────
# Monotonicity
# ─────────────────────────────────────────────────────────────────────────────

class TestMonotonicity:

    def test_linear_ramp_up_monotonic(self):
        r = VelocityRamp(dt=0.02, max_lin_accel=0.5, max_ang_accel=1.0)
        prev = 0.0
        for _ in range(100):
            lin, _ = r.step(1.0, 0.0)
            assert lin >= prev - 1e-9
            prev = lin

    def test_linear_ramp_down_monotonic(self):
        r = VelocityRamp(dt=0.02, max_lin_accel=0.5, max_ang_accel=1.0)
        for _ in range(200):
            r.step(1.0, 0.0)
        prev = r.current_linear
        for _ in range(100):
            lin, _ = r.step(0.5, 0.1)   # decel toward 0.5 (non-zero ang avoids e-stop)
            assert lin <= prev + 1e-9
            prev = lin

    def test_angular_ramp_monotonic(self):
        r = VelocityRamp(dt=0.02, max_lin_accel=1.0, max_ang_accel=1.0)
        prev = 0.0
        for _ in range(50):
            _, ang = r.step(0.1, 2.0)
            assert ang >= prev - 1e-9
            prev = ang


# ─────────────────────────────────────────────────────────────────────────────
# Timing / rate accuracy
# ─────────────────────────────────────────────────────────────────────────────

class TestRateAccuracy:

    def test_50hz_correct_step_size(self):
        """At 50 Hz with 0.5 m/s² → step = 0.01 m/s per tick."""
        r = VelocityRamp(dt=0.02, max_lin_accel=0.5, max_ang_accel=1.0)
        lin, _ = r.step(1.0, 0.0)
        assert abs(lin - 0.01) < 1e-9

    def test_10hz_correct_step_size(self):
        """At 10 Hz with 0.5 m/s² → step = 0.05 m/s per tick."""
        r = VelocityRamp(dt=0.1, max_lin_accel=0.5, max_ang_accel=1.0)
        lin, _ = r.step(1.0, 0.0)
        assert abs(lin - 0.05) < 1e-9

    def test_steps_to_target_50hz(self):
        """At 50 Hz, 0.5 m/s², reaching 1.0 m/s takes 100 steps (2 s)."""
        r = VelocityRamp(dt=0.02, max_lin_accel=0.5, max_ang_accel=1.0)
        steps = 0
        while r.current_linear < 1.0 - 1e-9:
            r.step(1.0, 0.0)
            steps += 1
            assert steps < 200, "Should converge in under 200 steps"
        assert 99 <= steps <= 101

    def test_steps_to_reach_estimate(self):
        r = VelocityRamp(dt=0.02, max_lin_accel=0.5, max_ang_accel=1.0)
        est = r.steps_to_reach(1.0, 0.0)
        assert est > 0
