"""
Unit tests for speed_controller_node pure functions.
No ROS2 runtime required.
Run with: pytest jetson/ros2_ws/src/saltybot_speed_controller/test/test_speed_controller.py
"""

import pytest

# ── Pure function mirrors ─────────────────────────────────────────────────────

WALK = "walk"
JOG  = "jog"
RIDE = "ride"
_PROFILE_ORDER = [WALK, JOG, RIDE]

_PROFILES = {
    WALK: {"max_linear_vel": 1.5, "max_angular_vel": 1.2,
           "accel_limit": 0.6,  "decel_limit": 1.0,
           "target_vel_min": 0.0, "target_vel_max": 2.0},
    JOG:  {"max_linear_vel": 3.0, "max_angular_vel": 1.5,
           "accel_limit": 0.5,  "decel_limit": 0.8,
           "target_vel_min": 2.0, "target_vel_max": 5.0},
    RIDE: {"max_linear_vel": 8.0, "max_angular_vel": 1.5,
           "accel_limit": 0.3,  "decel_limit": 0.5,
           "target_vel_min": 5.0, "target_vel_max": 15.0},
}


def _estimate_target_velocity(prev_range, curr_range, dt, robot_gps_speed):
    if dt <= 0 or prev_range <= 0 or curr_range <= 0:
        return robot_gps_speed
    range_rate = (curr_range - prev_range) / dt
    return max(0.0, robot_gps_speed + range_rate)


def _select_profile(target_vel, profiles, current_profile,
                    hysteresis_up=5, hysteresis_down=10, counters=None):
    if counters is None:
        counters = {}
    desired = current_profile
    for name in _PROFILE_ORDER:
        if target_vel <= profiles[name]["target_vel_max"]:
            desired = name
            break
    else:
        desired = RIDE
    if desired == current_profile:
        counters.clear()
        return current_profile
    key = desired
    counters[key] = counters.get(key, 0) + 1
    idx_d = _PROFILE_ORDER.index(desired)
    idx_c = _PROFILE_ORDER.index(current_profile)
    threshold = hysteresis_up if idx_d > idx_c else hysteresis_down
    if counters[key] >= threshold:
        counters.clear()
        return desired
    return current_profile


def _apply_accel_limit(current, target, accel_limit, decel_limit, dt):
    delta = target - current
    # Decelerating = magnitude is decreasing (current and delta have opposite signs)
    decelerating = (current > 0 and delta < 0) or (current < 0 and delta > 0)
    limit = decel_limit if decelerating else accel_limit
    if delta > 0:
        step = min(delta,  limit * dt)
    else:
        step = max(delta, -limit * dt)
    return current + step


def _limit_velocity(linear_x, angular_z, max_linear, max_angular):
    return (max(-max_linear,  min(max_linear,  linear_x)),
            max(-max_angular, min(max_angular, angular_z)))


def _check_emergency_stop(target_vel, target_vel_prev, dt,
                           stop_threshold, hard_decel_threshold):
    if target_vel < stop_threshold:
        return True
    if dt > 0 and (target_vel_prev - target_vel) / dt > hard_decel_threshold:
        return True
    return False


def _check_gps_runaway(gps_speed, cmd_vel, runaway_factor, profile_max):
    reference = max(abs(cmd_vel), 0.5)
    return gps_speed > reference * runaway_factor and gps_speed > profile_max * 0.5


def _emergency_decel_step(current_vel, decel_limit, dt):
    if current_vel > 0:
        return max(0.0, current_vel - decel_limit * dt)
    if current_vel < 0:
        return min(0.0, current_vel + decel_limit * dt)
    return 0.0


# ── Target velocity estimation ────────────────────────────────────────────────

class TestTargetVelocityEstimation:
    def test_target_faster_than_robot(self):
        # Robot at 2 m/s GPS, range increasing 3 m/s → target at 5 m/s
        v = _estimate_target_velocity(5.0, 8.0, 1.0, 2.0)   # range: 5→8 in 1s
        assert v == pytest.approx(5.0)

    def test_target_same_speed_constant_range(self):
        v = _estimate_target_velocity(5.0, 5.0, 1.0, 3.0)
        assert v == pytest.approx(3.0)

    def test_target_stopped_range_closing(self):
        # Robot at 3 m/s, range closing 3 m/s → target = 3 - 3 = 0 → clamped to 0
        v = _estimate_target_velocity(8.0, 5.0, 1.0, 3.0)
        assert v == pytest.approx(0.0)

    def test_invalid_dt_returns_gps_speed(self):
        assert _estimate_target_velocity(5.0, 6.0, 0.0, 2.5) == pytest.approx(2.5)

    def test_invalid_prev_range_returns_gps_speed(self):
        assert _estimate_target_velocity(0.0, 6.0, 1.0, 2.5) == pytest.approx(2.5)

    def test_result_non_negative(self):
        # Range closing much faster than robot → would be negative → clamp to 0
        v = _estimate_target_velocity(10.0, 3.0, 0.5, 1.0)
        assert v >= 0.0

    def test_high_speed_ride_estimate(self):
        # Robot at 6 m/s, range increasing 2 m/s → target at 8 m/s
        v = _estimate_target_velocity(4.0, 5.0, 0.5, 6.0)   # 2 m/s range rate
        assert v == pytest.approx(8.0)


# ── Profile selection ─────────────────────────────────────────────────────────

class TestProfileSelection:
    def test_walk_range_stays_walk(self):
        assert _select_profile(1.0, _PROFILES, WALK) == WALK

    def test_jog_range_stays_jog(self):
        assert _select_profile(3.5, _PROFILES, JOG) == JOG

    def test_ride_range_stays_ride(self):
        assert _select_profile(7.0, _PROFILES, RIDE) == RIDE

    def test_upgrade_needs_hysteresis_up(self):
        # Should NOT upgrade immediately
        counters = {}
        profile = WALK
        # 4 ticks at jog speed — not enough (hysteresis_up=5)
        for _ in range(4):
            profile = _select_profile(3.0, _PROFILES, profile,
                                      hysteresis_up=5, counters=counters)
        assert profile == WALK

    def test_upgrade_happens_at_threshold(self):
        counters = {}
        profile = WALK
        for _ in range(5):
            profile = _select_profile(3.0, _PROFILES, profile,
                                      hysteresis_up=5, counters=counters)
        assert profile == JOG

    def test_downgrade_needs_more_hysteresis(self):
        counters = {}
        profile = JOG
        for _ in range(9):
            profile = _select_profile(1.0, _PROFILES, profile,
                                      hysteresis_down=10, counters=counters)
        assert profile == JOG   # not yet

    def test_downgrade_happens_at_threshold(self):
        counters = {}
        profile = JOG
        for _ in range(10):
            profile = _select_profile(1.0, _PROFILES, profile,
                                      hysteresis_down=10, counters=counters)
        assert profile == WALK

    def test_counters_cleared_on_stable(self):
        counters = {}
        # Partial upgrade attempt then target returns to walk range
        _select_profile(3.0, _PROFILES, WALK, hysteresis_up=5, counters=counters)
        _select_profile(3.0, _PROFILES, WALK, hysteresis_up=5, counters=counters)
        assert JOG in counters   # counter accumulated
        _select_profile(1.0, _PROFILES, WALK, hysteresis_up=5, counters=counters)
        assert len(counters) == 0   # cleared when stable

    def test_above_all_profiles_selects_ride(self):
        profile = _select_profile(20.0, _PROFILES, RIDE)
        assert profile == RIDE

    def test_boundary_walk_max(self):
        # 2.0 m/s is exactly at walk max → stays walk
        assert _select_profile(2.0, _PROFILES, WALK) == WALK

    def test_just_above_walk_max_triggers_upgrade(self):
        counters = {}
        profile = WALK
        for _ in range(5):
            profile = _select_profile(2.01, _PROFILES, profile,
                                      hysteresis_up=5, counters=counters)
        assert profile == JOG


# ── Acceleration limiter ──────────────────────────────────────────────────────

class TestAccelLimit:
    _DT = 0.02   # 50 Hz

    def test_accel_step(self):
        # 0 → 1 at 0.5 m/s², 0.02s tick → step = 0.01
        v = _apply_accel_limit(0.0, 1.0, 0.5, 1.0, self._DT)
        assert v == pytest.approx(0.01)

    def test_decel_step(self):
        # 1 → 0 at 1.0 m/s², 0.02s → step = 0.02
        v = _apply_accel_limit(1.0, 0.0, 0.5, 1.0, self._DT)
        assert v == pytest.approx(0.98)

    def test_does_not_overshoot(self):
        # Close to target (gap < step)
        v = _apply_accel_limit(0.995, 1.0, 0.5, 1.0, self._DT)
        assert v == pytest.approx(1.0)

    def test_does_not_undershoot(self):
        v = _apply_accel_limit(0.005, 0.0, 0.5, 1.0, self._DT)
        assert v == pytest.approx(0.0)

    def test_negative_to_positive(self):
        # -1 → 0 (decelerating negative) at decel_limit
        v = _apply_accel_limit(-1.0, 0.0, 0.5, 1.0, self._DT)
        assert v == pytest.approx(-0.98)   # reduced magnitude

    def test_zero_to_zero(self):
        assert _apply_accel_limit(0.0, 0.0, 0.5, 1.0, self._DT) == pytest.approx(0.0)

    def test_ride_accel_gentler_than_walk(self):
        dt = 0.02
        v_walk = _apply_accel_limit(0.0, 5.0,
                                     _PROFILES[WALK]["accel_limit"],
                                     _PROFILES[WALK]["decel_limit"], dt)
        v_ride = _apply_accel_limit(0.0, 5.0,
                                     _PROFILES[RIDE]["accel_limit"],
                                     _PROFILES[RIDE]["decel_limit"], dt)
        assert v_walk > v_ride   # walk accelerates faster than ride

    def test_time_to_reach_walk_max(self):
        # walk: accel=0.6 m/s², target=1.5 m/s, dt=0.02s
        # expected ticks ≈ 1.5 / (0.6 * 0.02) = 125 ticks
        v = 0.0
        ticks = 0
        for _ in range(200):
            v = _apply_accel_limit(v, 1.5, 0.6, 1.0, 0.02)
            ticks += 1
            if v >= 1.5 - 1e-6:
                break
        assert v == pytest.approx(1.5, abs=1e-4)
        assert 120 <= ticks <= 130   # ≈ 125 ticks


# ── Velocity limiter ──────────────────────────────────────────────────────────

class TestLimitVelocity:
    def test_within_limits(self):
        lin, ang = _limit_velocity(1.0, 0.5, 8.0, 1.5)
        assert lin == pytest.approx(1.0)
        assert ang == pytest.approx(0.5)

    def test_linear_clamped_high(self):
        lin, _ = _limit_velocity(10.0, 0.0, 8.0, 1.5)
        assert lin == pytest.approx(8.0)

    def test_linear_clamped_low(self):
        lin, _ = _limit_velocity(-10.0, 0.0, 8.0, 1.5)
        assert lin == pytest.approx(-8.0)

    def test_angular_clamped(self):
        _, ang = _limit_velocity(0.0, 5.0, 8.0, 1.5)
        assert ang == pytest.approx(1.5)

    def test_zero_passthrough(self):
        lin, ang = _limit_velocity(0.0, 0.0, 8.0, 1.5)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(0.0)


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

class TestEmergencyStop:
    def test_triggers_below_stop_threshold(self):
        assert _check_emergency_stop(0.2, 3.0, 0.02, 0.4, 3.0) is True

    def test_triggers_on_hard_decel(self):
        # target_vel drops from 5 to 3 in 0.02s → decel = 100 m/s² >> 3.0
        assert _check_emergency_stop(3.0, 5.0, 0.02, 0.4, 3.0) is True

    def test_no_trigger_gradual_decel(self):
        # 5 → 4 in 0.02s → decel = 50 m/s² >> threshold... wait, 50 > 3
        # Let me use a smaller decel: 5 → 4.9 in 0.02s → decel = 5 m/s²... still > 3
        # Use 5 → 4.9 in 0.1s → decel = 1.0 m/s²
        assert _check_emergency_stop(4.9, 5.0, 0.1, 0.4, 3.0) is False

    def test_no_trigger_at_cruising_speed(self):
        assert _check_emergency_stop(6.0, 6.0, 0.02, 0.4, 3.0) is False

    def test_boundary_exactly_at_stop_threshold(self):
        # target_vel == stop_threshold → NOT < → no trigger
        # prev must be close enough that decel check doesn't also fire:
        # (0.42 - 0.40) / 0.02 = 1.0 m/s² < 3.0 → OK
        assert _check_emergency_stop(0.4, 0.42, 0.02, 0.4, 3.0) is False

    def test_triggers_just_below_threshold(self):
        assert _check_emergency_stop(0.39, 1.0, 0.02, 0.4, 3.0) is True


# ── GPS runaway detection ─────────────────────────────────────────────────────

class TestGpsRunaway:
    def test_runaway_detected(self):
        # GPS 5 m/s, commanded 2 m/s → 5 > 2*1.5=3, 5 > 8*0.5=4 → True
        assert _check_gps_runaway(5.0, 2.0, 1.5, 8.0) is True

    def test_no_runaway_normal(self):
        # GPS 2.1 m/s, commanded 2 m/s → 2.1 not > 3.0
        assert _check_gps_runaway(2.1, 2.0, 1.5, 8.0) is False

    def test_no_runaway_stationary(self):
        # Robot stopped (GPS 0.1 m/s, cmd 0), reference = max(0,0.5) = 0.5
        # 0.1 not > 0.5*1.5=0.75
        assert _check_gps_runaway(0.1, 0.0, 1.5, 1.5) is False

    def test_runaway_only_at_meaningful_speed(self):
        # GPS=6, cmd=4, factor=1.5 → 6 > 4*1.5=6 → False (not strictly greater)
        assert _check_gps_runaway(6.0, 4.0, 1.5, 8.0) is False

    def test_runaway_needs_gps_above_half_profile_max(self):
        # Even if ratio is high, below 50% of profile max → no trigger
        # profile_max=8 → threshold=4; GPS=3 < 4 → no trigger
        assert _check_gps_runaway(3.0, 0.5, 1.5, 8.0) is False


# ── Emergency decel step ──────────────────────────────────────────────────────

class TestEmergencyDecelStep:
    _DT = 0.02
    _DECEL = 2.0

    def test_positive_speed_decreases(self):
        v = _emergency_decel_step(5.0, self._DECEL, self._DT)
        assert v == pytest.approx(5.0 - 2.0 * 0.02)

    def test_negative_speed_increases(self):
        v = _emergency_decel_step(-5.0, self._DECEL, self._DT)
        assert v == pytest.approx(-5.0 + 2.0 * 0.02)

    def test_does_not_go_past_zero_positive(self):
        v = _emergency_decel_step(0.03, self._DECEL, self._DT)
        assert v == pytest.approx(0.0)   # 0.03 - 0.04 → clamped to 0

    def test_does_not_go_past_zero_negative(self):
        v = _emergency_decel_step(-0.03, self._DECEL, self._DT)
        assert v == pytest.approx(0.0)

    def test_zero_stays_zero(self):
        assert _emergency_decel_step(0.0, self._DECEL, self._DT) == 0.0

    def test_full_stop_from_ride_speed(self):
        # 8 m/s at 2.0 m/s² → ticks to stop = 8 / (2.0 * 0.02) = 200 ticks = 4s
        v = 8.0
        ticks = 0
        for _ in range(500):
            v = _emergency_decel_step(v, 2.0, 0.02)
            ticks += 1
            if v == 0.0:
                break
        assert v == 0.0
        assert 195 <= ticks <= 205


# ── Integration: profile + accel + limit ─────────────────────────────────────

class TestIntegration:
    """End-to-end control step simulation."""

    def _step(self, current, request, profile_name, dt=0.02):
        p = _PROFILES[profile_name]
        capped_lin, capped_ang = _limit_velocity(
            request, 0.0, p["max_linear_vel"], p["max_angular_vel"])
        new_lin = _apply_accel_limit(
            current, capped_lin, p["accel_limit"], p["decel_limit"], dt)
        return new_lin

    def test_walk_stays_under_1p5(self):
        v = 0.0
        for _ in range(300):
            v = self._step(v, 5.0, WALK)   # request far above limit
        assert v <= 1.5 + 1e-6

    def test_ride_reaches_8_from_zero(self):
        v = 0.0
        for _ in range(5000):
            v = self._step(v, 8.0, RIDE)
        assert v == pytest.approx(8.0, abs=1e-4)

    def test_emergency_overrides_profile(self):
        v = 6.0   # at ride speed
        # Emergency: decel toward 0
        for _ in range(500):
            v = _emergency_decel_step(v, 2.0, 0.02)
            if v == 0.0:
                break
        assert v == pytest.approx(0.0)

    def test_profile_upgrade_then_downgrade_cycle(self):
        counters = {}
        profile = WALK
        # Upgrade: 5 ticks at jog speed
        for _ in range(5):
            profile = _select_profile(3.0, _PROFILES, profile,
                                      hysteresis_up=5, counters=counters)
        assert profile == JOG
        # Downgrade: 10 ticks at walk speed
        counters = {}
        for _ in range(10):
            profile = _select_profile(1.0, _PROFILES, profile,
                                      hysteresis_down=10, counters=counters)
        assert profile == WALK
