"""
Unit tests for Jetson→ESP32-S3 command serialization logic.
Tests Twist→speed/steer conversion and frame formatting.
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
"""

import pytest


# ── Minimal stubs (no ROS2 runtime needed) ───────────────────────────────────

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


def twist_to_frame(linear_x, angular_z, speed_scale=1000.0, steer_scale=-500.0):
    """Mirror of SaltybotCmdNode._cmd_vel_cb frame building."""
    speed = int(_clamp(linear_x  * speed_scale, -1000.0, 1000.0))
    steer = int(_clamp(angular_z * steer_scale, -1000.0, 1000.0))
    return f"C{speed},{steer}\n".encode("ascii"), speed, steer


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

def test_zero_twist_produces_zero_cmd():
    frame, speed, steer = twist_to_frame(0.0, 0.0)
    assert frame == b"C0,0\n"
    assert speed == 0
    assert steer == 0


def test_full_forward():
    frame, speed, steer = twist_to_frame(1.0, 0.0)
    assert frame == b"C1000,0\n"
    assert speed == 1000


def test_full_reverse():
    frame, speed, steer = twist_to_frame(-1.0, 0.0)
    assert frame == b"C-1000,0\n"
    assert speed == -1000


def test_left_turn_positive_angular_z():
    # Default steer_scale=-500: +angular.z → negative steer
    frame, speed, steer = twist_to_frame(0.0, 1.0)
    assert steer == -500
    assert b"C0,-500\n" == frame


def test_right_turn_negative_angular_z():
    frame, speed, steer = twist_to_frame(0.0, -1.0)
    assert steer == 500
    assert b"C0,500\n" == frame


def test_speed_clamped_at_max():
    _, speed, _ = twist_to_frame(5.0, 0.0)   # 5 m/s >> 1 m/s max
    assert speed == 1000


def test_speed_clamped_at_min():
    _, speed, _ = twist_to_frame(-5.0, 0.0)
    assert speed == -1000


def test_steer_clamped_at_max():
    # angular.z=-5 rad/s with steer_scale=-500 → +2500 → clamped to +1000
    _, _, steer = twist_to_frame(0.0, -5.0)
    assert steer == 1000


def test_steer_clamped_at_min():
    _, _, steer = twist_to_frame(0.0, 5.0)
    assert steer == -1000


def test_combined_motion():
    frame, speed, steer = twist_to_frame(0.5, -0.4)
    assert speed == 500
    assert steer == int(_clamp(-0.4 * -500.0, -1000.0, 1000.0))  # +200
    assert frame == b"C500,200\n"


def test_custom_scales():
    # speed_scale=500 → 1 m/s = 500 ESC units
    frame, speed, steer = twist_to_frame(1.0, 0.0, speed_scale=500.0, steer_scale=-250.0)
    assert speed == 500
    assert frame == b"C500,0\n"


def test_heartbeat_frame():
    assert b"H\n" == b"H\n"   # constant — just verifies expected bytes


def test_zero_cmd_frame():
    """C0,0\\n must be sent on shutdown."""
    frame, _, _ = twist_to_frame(0.0, 0.0)
    assert frame == b"C0,0\n"