"""
Unit tests for cmd_vel_bridge_node logic.
No ROS2 runtime required — tests pure functions extracted from the node.
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd_vel_bridge.py
"""

import pytest


# ── Pure function mirrors (no ROS2 imports) ──────────────────────────────────

def _clamp(v, lo, hi):
    return max(lo, min(hi, v))


def _ramp_toward(current, target, max_step):
    diff = target - current
    if abs(diff) <= max_step:
        return target
    return current + (max_step if diff > 0 else -max_step)


def cmd_vel_to_targets(linear_x, angular_z,
                        max_linear_vel=0.5, max_angular_vel=2.0,
                        speed_scale=1000.0, steer_scale=-500.0):
    """Mirror of CmdVelBridgeNode._cmd_vel_cb target computation."""
    lin_x = _clamp(linear_x,  -max_linear_vel,  max_linear_vel)
    ang_z = _clamp(angular_z, -max_angular_vel,  max_angular_vel)
    speed = int(_clamp(lin_x * speed_scale, -1000.0, 1000.0))
    steer = int(_clamp(ang_z * steer_scale, -1000.0, 1000.0))
    return speed, steer


def apply_ramp(current_speed, current_steer, target_speed, target_steer, step):
    """Mirror of CmdVelBridgeNode._control_cb ramp logic."""
    new_speed = _ramp_toward(current_speed, target_speed, step)
    new_steer = _ramp_toward(current_steer, target_steer, step)
    return new_speed, new_steer


# ── Velocity limit tests ──────────────────────────────────────────────────────

class TestVelocityLimits:
    def test_zero_twist_zero_targets(self):
        speed, steer = cmd_vel_to_targets(0.0, 0.0)
        assert speed == 0
        assert steer == 0

    def test_within_limits_passes_through(self):
        speed, steer = cmd_vel_to_targets(0.3, 1.0)
        assert speed == 300
        assert steer == -500  # steer_scale=-500 * 1.0

    def test_speed_clamped_at_max_linear_vel(self):
        # 2.0 m/s >> 0.5 m/s limit → clamp to 0.5 m/s → 500 ESC
        speed, _ = cmd_vel_to_targets(2.0, 0.0)
        assert speed == 500

    def test_speed_clamped_at_min_linear_vel(self):
        speed, _ = cmd_vel_to_targets(-2.0, 0.0)
        assert speed == -500

    def test_angular_clamped_at_max(self):
        # 5.0 rad/s >> 2.0 rad/s limit → 2.0 * -500 = -1000
        _, steer = cmd_vel_to_targets(0.0, 5.0)
        assert steer == -1000

    def test_angular_clamped_at_min(self):
        _, steer = cmd_vel_to_targets(0.0, -5.0)
        assert steer == 1000

    def test_full_forward_at_max_limit(self):
        # Exactly at limit: 0.5 m/s * 1000 = 500
        speed, _ = cmd_vel_to_targets(0.5, 0.0)
        assert speed == 500

    def test_custom_limits(self):
        speed, _ = cmd_vel_to_targets(1.0, 0.0,
                                       max_linear_vel=1.0, speed_scale=1000.0)
        assert speed == 1000

    def test_negative_steer_scale_flips_sign(self):
        # +angular.z (CCW/left) → negative ESC steer (right turn in ESC convention)
        _, steer = cmd_vel_to_targets(0.0, 1.0, steer_scale=-500.0)
        assert steer == -500

    def test_positive_steer_scale_no_flip(self):
        _, steer = cmd_vel_to_targets(0.0, 1.0, steer_scale=500.0)
        assert steer == 500


# ── Ramp tests ────────────────────────────────────────────────────────────────

class TestRamp:
    def test_ramp_toward_positive_target(self):
        # step=10: 0 → 10 in one tick
        new_s, new_t = apply_ramp(0, 0, 100, 0, step=10)
        assert new_s == 10
        assert new_t == 0

    def test_ramp_does_not_overshoot(self):
        # step=10, gap=5: should snap to target
        new_s, _ = apply_ramp(95, 0, 100, 0, step=10)
        assert new_s == 100

    def test_ramp_toward_zero(self):
        new_s, _ = apply_ramp(50, 0, 0, 0, step=10)
        assert new_s == 40

    def test_ramp_toward_negative(self):
        new_s, _ = apply_ramp(0, 0, -30, 0, step=10)
        assert new_s == -10

    def test_ramp_already_at_target(self):
        new_s, new_t = apply_ramp(100, -200, 100, -200, step=10)
        assert new_s == 100
        assert new_t == -200

    def test_ramp_rate_500_at_50hz(self):
        # ramp_rate=500 ESC/s, 50Hz → step=10 per tick
        step = 500 // 50
        assert step == 10
        # 50 ticks to go 0→500
        speed = 0
        for _ in range(50):
            speed = _ramp_toward(speed, 500, step)
        assert speed == 500

    def test_ramp_both_channels_independent(self):
        new_s, new_t = apply_ramp(0, 0, 100, -100, step=10)
        assert new_s == 10
        assert new_t == -10


# ── Mode gate tests ───────────────────────────────────────────────────────────

class TestModeGate:
    MODE_MANUAL    = 0
    MODE_ASSISTED  = 1
    MODE_AUTONOMOUS = 2

    def _apply_mode_gate(self, esp32_mode, current_speed, current_steer,
                          target_speed, target_steer, step=10):
        """Mirror of _control_cb mode gate logic."""
        if esp32_mode != self.MODE_AUTONOMOUS:
            # Reset ramp state, send zero
            return 0, 0, 0, 0  # (current_speed, current_steer, sent_speed, sent_steer)
        new_s = _ramp_toward(current_speed, target_speed, step)
        new_t = _ramp_toward(current_steer, target_steer, step)
        return new_s, new_t, new_s, new_t

    def test_manual_mode_sends_zero(self):
        _, _, sent_s, sent_t = self._apply_mode_gate(
            self.MODE_MANUAL, 100, -50, 500, -300)
        assert sent_s == 0
        assert sent_t == 0

    def test_assisted_mode_sends_zero(self):
        _, _, sent_s, sent_t = self._apply_mode_gate(
            self.MODE_ASSISTED, 100, -50, 500, -300)
        assert sent_s == 0
        assert sent_t == 0

    def test_autonomous_mode_ramps(self):
        _, _, sent_s, sent_t = self._apply_mode_gate(
            self.MODE_AUTONOMOUS, 0, 0, 500, -300, step=10)
        assert sent_s == 10
        assert sent_t == -10

    def test_mode_gate_resets_ramp_state(self):
        # While in MANUAL, current is reset to 0
        cur_s, cur_t, _, _ = self._apply_mode_gate(
            self.MODE_MANUAL, 500, -200, 500, -200)
        assert cur_s == 0
        assert cur_t == 0
        # Re-entering AUTONOMOUS ramps from 0, not from 500
        _, _, sent_s, _ = self._apply_mode_gate(
            self.MODE_AUTONOMOUS, cur_s, cur_t, 500, 0, step=10)
        assert sent_s == 10  # ramped from 0, not jumped to 500


# ── Deadman switch tests ──────────────────────────────────────────────────────

class TestDeadman:
    def test_deadman_zeros_targets_on_timeout(self):
        """Simulate no /cmd_vel for > timeout → targets zeroed."""
        target_speed = 500
        target_steer = -300
        # last_cmd_vel was 1.0s ago, timeout is 0.5s → trigger
        last_cmd_vel = 0.0
        now = 0.6  # 0.6s elapsed
        timeout = 0.5

        if last_cmd_vel > 0.0 and now - last_cmd_vel > timeout:
            target_speed = 0
            target_steer = 0

        assert target_speed == 0
        assert target_steer == 0

    def test_deadman_does_not_trigger_before_timeout(self):
        target_speed = 500
        last_cmd_vel = 0.0
        now = 0.3  # only 0.3s elapsed
        timeout = 0.5

        if last_cmd_vel > 0.0 and now - last_cmd_vel > timeout:
            target_speed = 0

        assert target_speed == 500  # unchanged

    def test_deadman_does_not_trigger_at_t0_before_first_cmd(self):
        # last_cmd_vel==0 means no /cmd_vel ever received — don't zero yet
        target_speed = 0
        last_cmd_vel = 0.0  # never received
        now = 10.0
        timeout = 0.5

        if last_cmd_vel > 0.0 and now - last_cmd_vel > timeout:
            target_speed = 999  # should not reach here

        assert target_speed == 0


# ── Frame format ──────────────────────────────────────────────────────────────

class TestFrameFormat:
    def test_zero_frame(self):
        frame = f"C{0},{0}\n".encode("ascii")
        assert frame == b"C0,0\n"

    def test_drive_frame(self):
        speed, steer = cmd_vel_to_targets(0.3, -1.0)
        frame = f"C{speed},{steer}\n".encode("ascii")
        assert frame == b"C300,500\n"

    def test_heartbeat_frame(self):
        assert b"H\n" == b"H\n"
