"""
test_rover_kinematics.py — Unit tests for kinematics.py (Issue #110).

No ROS2 runtime required — pure Python.
"""

import math
import pytest

from saltybot_rover_driver.kinematics import (
    two_wheel_speeds,
    four_wheel_speeds,
    speed_to_pwm,
    compute_wheel_speeds,
    odometry_from_wheel_speeds,
)


# ─── two_wheel_speeds ────────────────────────────────────────────────────────

class TestTwoWheelSpeeds:

    def test_straight_ahead(self):
        vl, vr = two_wheel_speeds(1.0, 0.0, 0.28)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_straight_reverse(self):
        vl, vr = two_wheel_speeds(-1.0, 0.0, 0.28)
        assert vl == pytest.approx(-1.0)
        assert vr == pytest.approx(-1.0)

    def test_point_turn_ccw(self):
        """Pure CCW rotation: linear_x=0, angular_z>0 → left slower, right faster."""
        vl, vr = two_wheel_speeds(0.0, 1.0, 0.28)
        assert vl == pytest.approx(-0.14)
        assert vr == pytest.approx(0.14)

    def test_point_turn_cw(self):
        """Pure CW rotation: linear_x=0, angular_z<0 → left faster, right slower."""
        vl, vr = two_wheel_speeds(0.0, -1.0, 0.28)
        assert vl == pytest.approx(0.14)
        assert vr == pytest.approx(-0.14)

    def test_forward_left_curve(self):
        vl, vr = two_wheel_speeds(1.0, 0.5, 0.28)
        assert vl == pytest.approx(1.0 - 0.5 * 0.28 / 2.0)
        assert vr == pytest.approx(1.0 + 0.5 * 0.28 / 2.0)

    def test_zero_inputs(self):
        vl, vr = two_wheel_speeds(0.0, 0.0, 0.28)
        assert vl == pytest.approx(0.0)
        assert vr == pytest.approx(0.0)

    def test_zero_track_width(self):
        """track_width=0 should not crash; both wheels get linear_x."""
        vl, vr = two_wheel_speeds(1.0, 1.0, 0.0)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_symmetry(self):
        """Reversing angular_z mirrors left/right."""
        vl1, vr1 = two_wheel_speeds(0.5, 0.3, 0.28)
        vl2, vr2 = two_wheel_speeds(0.5, -0.3, 0.28)
        assert vl1 == pytest.approx(vr2)
        assert vr1 == pytest.approx(vl2)


# ─── four_wheel_speeds ───────────────────────────────────────────────────────

class TestFourWheelSpeeds:

    def test_straight(self):
        vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.0, 0.28)
        assert vfl == pytest.approx(1.0)
        assert vlr == pytest.approx(1.0)
        assert vrf == pytest.approx(1.0)
        assert vrr == pytest.approx(1.0)

    def test_front_rear_share_speed(self):
        """Front and rear on each side must have identical speeds."""
        vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.5, 0.28)
        assert vfl == pytest.approx(vlr)
        assert vrf == pytest.approx(vrr)

    def test_point_turn(self):
        vfl, vlr, vrf, vrr = four_wheel_speeds(0.0, 1.0, 0.28)
        expected_l = -0.14
        expected_r = 0.14
        assert vfl == pytest.approx(expected_l)
        assert vlr == pytest.approx(expected_l)
        assert vrf == pytest.approx(expected_r)
        assert vrr == pytest.approx(expected_r)

    def test_returns_four_values(self):
        result = four_wheel_speeds(1.0, 0.3, 0.28)
        assert len(result) == 4

    def test_consistent_with_two_wheel(self):
        """4-wheel is just 2-wheel spread to four channels."""
        vl, vr = two_wheel_speeds(0.8, 0.4, 0.30)
        vfl, vlr, vrf, vrr = four_wheel_speeds(0.8, 0.4, 0.30)
        assert vfl == pytest.approx(vl)
        assert vlr == pytest.approx(vl)
        assert vrf == pytest.approx(vr)
        assert vrr == pytest.approx(vr)


# ─── speed_to_pwm ────────────────────────────────────────────────────────────

class TestSpeedToPwm:

    def test_zero_speed_is_neutral(self):
        assert speed_to_pwm(0.0, 2.0, 1500, 500) == 1500

    def test_full_forward(self):
        assert speed_to_pwm(2.0, 2.0, 1500, 500) == 2000

    def test_full_reverse(self):
        assert speed_to_pwm(-2.0, 2.0, 1500, 500) == 1000

    def test_half_forward(self):
        assert speed_to_pwm(1.0, 2.0, 1500, 500) == 1750

    def test_half_reverse(self):
        assert speed_to_pwm(-1.0, 2.0, 1500, 500) == 1250

    def test_over_max_clamps_to_2000(self):
        assert speed_to_pwm(5.0, 2.0, 1500, 500) == 2000

    def test_under_min_clamps_to_1000(self):
        assert speed_to_pwm(-5.0, 2.0, 1500, 500) == 1000

    def test_zero_max_speed_returns_neutral(self):
        assert speed_to_pwm(1.0, 0.0, 1500, 500) == 1500

    def test_negative_max_speed_returns_neutral(self):
        assert speed_to_pwm(1.0, -1.0, 1500, 500) == 1500

    def test_returns_int(self):
        result = speed_to_pwm(0.5, 2.0, 1500, 500)
        assert isinstance(result, int)

    def test_custom_neutral_and_range(self):
        """Supports non-standard neutral/range values."""
        pwm = speed_to_pwm(1.0, 2.0, 1400, 400)
        assert pwm == 1600   # 1400 + 0.5*400

    def test_quarter_speed(self):
        pwm = speed_to_pwm(0.5, 2.0, 1500, 500)
        assert pwm == 1625   # 1500 + 0.25*500

    def test_rounding(self):
        """Result should be integer-rounded, not truncated."""
        # speed=1/3 of max → 1500 + (1/3)*500 = 1666.67 → rounds to 1667
        pwm = speed_to_pwm(2.0 / 3.0, 2.0, 1500, 500)
        assert pwm == 1667


# ─── compute_wheel_speeds ────────────────────────────────────────────────────

class TestComputeWheelSpeeds:

    def test_two_wheel_mode_returns_two_values(self):
        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        assert len(result) == 2

    def test_four_wheel_mode_returns_four_values(self):
        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
        assert len(result) == 4

    def test_clipping_two_wheel(self):
        """Sharp turn at full speed: outer wheel must be clipped to max."""
        result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        vl, vr = result
        assert vl >= -2.0
        assert vr <= 2.0

    def test_clipping_four_wheel(self):
        result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=True, max_speed_ms=2.0)
        for v in result:
            assert -2.0 <= v <= 2.0

    def test_straight_no_clip(self):
        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        assert result == pytest.approx((1.0, 1.0))

    def test_four_wheel_symmetric(self):
        vfl, vlr, vrf, vrr = compute_wheel_speeds(0.5, 0.2, 0.28, four_wheel=True, max_speed_ms=2.0)
        assert vfl == pytest.approx(vlr)
        assert vrf == pytest.approx(vrr)

    def test_zero_inputs_two_wheel(self):
        result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        assert result == pytest.approx((0.0, 0.0))

    def test_zero_inputs_four_wheel(self):
        result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
        assert result == pytest.approx((0.0, 0.0, 0.0, 0.0))

    def test_negative_speed_two_wheel(self):
        vl, vr = compute_wheel_speeds(-1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        assert vl == pytest.approx(-1.0)
        assert vr == pytest.approx(-1.0)


# ─── odometry_from_wheel_speeds ──────────────────────────────────────────────

class TestOdometryFromWheelSpeeds:

    def test_straight(self):
        lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.28)
        assert lin == pytest.approx(1.0)
        assert ang == pytest.approx(0.0)

    def test_point_turn_ccw(self):
        """Equal and opposite: left=-v, right=+v → angular = 2v/track."""
        v = 0.14
        lin, ang = odometry_from_wheel_speeds(-v, v, 0.28)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(1.0)

    def test_point_turn_cw(self):
        v = 0.14
        lin, ang = odometry_from_wheel_speeds(v, -v, 0.28)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(-1.0)

    def test_zero_inputs(self):
        lin, ang = odometry_from_wheel_speeds(0.0, 0.0, 0.28)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(0.0)

    def test_zero_track_width_no_crash(self):
        lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.0)
        assert lin == pytest.approx(1.0)
        assert ang == pytest.approx(0.0)

    def test_inverse_of_two_wheel_speeds(self):
        """odometry_from_wheel_speeds should be the inverse of two_wheel_speeds."""
        lin_in, ang_in = 0.7, 0.5
        track = 0.28
        vl, vr = two_wheel_speeds(lin_in, ang_in, track)
        lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track)
        assert lin_out == pytest.approx(lin_in)
        assert ang_out == pytest.approx(ang_in)

    def test_reverse(self):
        lin, ang = odometry_from_wheel_speeds(-1.0, -1.0, 0.28)
        assert lin == pytest.approx(-1.0)
        assert ang == pytest.approx(0.0)


# ─── Integration: kinematics round-trip ─────────────────────────────────────

class TestRoundTrip:

    @pytest.mark.parametrize("lin,ang", [
        (1.0,  0.0),
        (0.0,  1.0),
        (0.5,  0.5),
        (-0.5, 0.3),
        (1.0, -0.8),
    ])
    def test_roundtrip_two_wheel(self, lin, ang):
        """compute → odometry_from → same (lin, ang) for unclipped inputs."""
        track = 0.28
        vl, vr = compute_wheel_speeds(lin, ang, track, four_wheel=False, max_speed_ms=2.0)
        lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track)
        assert lin_out == pytest.approx(lin, abs=1e-9)
        assert ang_out == pytest.approx(ang, abs=1e-9)

    def test_pwm_neutral_at_zero_speed(self):
        pwm = speed_to_pwm(0.0, 2.0, 1500, 500)
        assert pwm == 1500

    def test_pwm_full_deflection_forward(self):
        vl, vr = compute_wheel_speeds(2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        pwm_l = speed_to_pwm(vl, 2.0, 1500, 500)
        pwm_r = speed_to_pwm(vr, 2.0, 1500, 500)
        assert pwm_l == 2000
        assert pwm_r == 2000

    def test_pwm_full_deflection_reverse(self):
        vl, vr = compute_wheel_speeds(-2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
        pwm_l = speed_to_pwm(vl, 2.0, 1500, 500)
        pwm_r = speed_to_pwm(vr, 2.0, 1500, 500)
        assert pwm_l == 1000
        assert pwm_r == 1000

    def test_four_wheel_all_channels_driven(self):
        """All four PWMs should be non-neutral during forward motion."""
        vfl, vlr, vrf, vrr = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
        pwms = [speed_to_pwm(v, 2.0, 1500, 500) for v in (vfl, vlr, vrf, vrr)]
        assert all(p > 1500 for p in pwms)
