"""
test_tank_kinematics.py — Unit tests for kinematics.py (Issue #122).

No ROS2 runtime required — pure Python.
"""

import math
import pytest

from saltybot_tank_driver.kinematics import (
    tank_speeds,
    skid_steer_speeds,
    speed_to_pwm,
    compute_track_speeds,
    expand_to_4ch,
    odometry_from_track_speeds,
    _clamp_slip,
    DRIVE_MODES,
    SLIP_MIN,
    SLIP_MAX,
)


# ─── _clamp_slip ─────────────────────────────────────────────────────────────

class TestClampSlip:

    def test_zero_passes_through(self):
        assert _clamp_slip(0.0) == 0.0

    def test_typical_value(self):
        assert _clamp_slip(0.3) == pytest.approx(0.3)

    def test_below_min_clamped(self):
        assert _clamp_slip(-0.1) == SLIP_MIN

    def test_above_max_clamped(self):
        assert _clamp_slip(1.0) == SLIP_MAX

    def test_at_max_boundary(self):
        assert _clamp_slip(0.99) == pytest.approx(0.99)

    def test_large_value_clamped(self):
        assert _clamp_slip(5.0) == SLIP_MAX


# ─── tank_speeds ─────────────────────────────────────────────────────────────

class TestTankSpeeds:

    def test_straight_no_slip(self):
        vl, vr = tank_speeds(1.0, 0.0, 0.30, slip_factor=0.0)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_point_turn_no_slip(self):
        """Pure CCW rotation with no slip."""
        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
        assert vl == pytest.approx(-0.15)
        assert vr == pytest.approx(0.15)

    def test_point_turn_cw_no_slip(self):
        vl, vr = tank_speeds(0.0, -1.0, 0.30, slip_factor=0.0)
        assert vl == pytest.approx(0.15)
        assert vr == pytest.approx(-0.15)

    def test_zero_inputs(self):
        vl, vr = tank_speeds(0.0, 0.0, 0.30, slip_factor=0.0)
        assert vl == pytest.approx(0.0)
        assert vr == pytest.approx(0.0)

    def test_slip_increases_angular_command(self):
        """With slip>0, angular component is amplified so output diff is larger."""
        vl0, vr0 = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
        vl3, vr3 = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.3)
        # More slip → larger differential to achieve same ω
        assert abs(vr3 - vl3) > abs(vr0 - vl0)

    def test_slip_zero_equals_skid_steer(self):
        """slip_factor=0 must be identical to skid_steer_speeds."""
        vl_t, vr_t = tank_speeds(0.7, 0.4, 0.30, slip_factor=0.0)
        vl_s, vr_s = skid_steer_speeds(0.7, 0.4, 0.30)
        assert vl_t == pytest.approx(vl_s)
        assert vr_t == pytest.approx(vr_s)

    def test_known_slip_value(self):
        """slip=0.5 → angular scaled by 1/(1-0.5) = 2."""
        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.5)
        expected_half = (1.0 / (1.0 - 0.5)) * 0.30 / 2.0   # = 0.30
        assert vl == pytest.approx(-expected_half)
        assert vr == pytest.approx(expected_half)

    def test_negative_slip_clamped_to_zero(self):
        """Negative slip_factor is clamped to 0."""
        vl_neg, vr_neg = tank_speeds(0.0, 1.0, 0.30, slip_factor=-0.5)
        vl_0,   vr_0   = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
        assert vl_neg == pytest.approx(vl_0)
        assert vr_neg == pytest.approx(vr_0)

    def test_symmetry_linear(self):
        """Reversing linear_x reverses both speeds equally."""
        vl_f, vr_f = tank_speeds(1.0, 0.5, 0.30, slip_factor=0.2)
        vl_r, vr_r = tank_speeds(-1.0, 0.5, 0.30, slip_factor=0.2)
        assert vl_r == pytest.approx(vl_f - 2.0)
        assert vr_r == pytest.approx(vr_f - 2.0)

    def test_zero_radius_turn(self):
        """lin=0, ang≠0 → equal and opposite speeds (zero-radius pivot)."""
        vl, vr = tank_speeds(0.0, 1.0, 0.30, slip_factor=0.0)
        assert vl == pytest.approx(-vr)

    def test_zero_track_width_no_crash(self):
        vl, vr = tank_speeds(1.0, 2.0, 0.0, slip_factor=0.0)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)


# ─── skid_steer_speeds ───────────────────────────────────────────────────────

class TestSkidSteerSpeeds:

    def test_straight(self):
        vl, vr = skid_steer_speeds(1.0, 0.0, 0.30)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_point_turn(self):
        vl, vr = skid_steer_speeds(0.0, 1.0, 0.30)
        assert vl == pytest.approx(-0.15)
        assert vr == pytest.approx(0.15)

    def test_curve(self):
        vl, vr = skid_steer_speeds(1.0, 1.0, 0.30)
        assert vl == pytest.approx(1.0 - 0.15)
        assert vr == pytest.approx(1.0 + 0.15)


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

class TestSpeedToPwm:

    def test_zero_speed_neutral(self):
        assert speed_to_pwm(0.0, 1.5, 1500, 500) == 1500

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

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

    def test_half_forward(self):
        assert speed_to_pwm(0.75, 1.5, 1500, 500) == 1750

    def test_clamp_over(self):
        assert speed_to_pwm(10.0, 1.5, 1500, 500) == 2000

    def test_clamp_under(self):
        assert speed_to_pwm(-10.0, 1.5, 1500, 500) == 1000

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

    def test_returns_int(self):
        assert isinstance(speed_to_pwm(0.5, 1.5, 1500, 500), int)

    def test_rounding(self):
        # 1.0/1.5 = 0.6667 → 1500 + 0.6667*500 = 1833.33 → 1833
        pwm = speed_to_pwm(1.0, 1.5, 1500, 500)
        assert pwm == 1833


# ─── compute_track_speeds ────────────────────────────────────────────────────

class TestComputeTrackSpeeds:

    def test_returns_two_values(self):
        result = compute_track_speeds(1.0, 0.0, 0.30, 1.5)
        assert len(result) == 2

    def test_straight_tracked(self):
        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.3)
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_straight_2wd(self):
        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="2wd")
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_straight_4wd(self):
        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="4wd")
        assert vl == pytest.approx(1.0)
        assert vr == pytest.approx(1.0)

    def test_clipping_tracked(self):
        vl, vr = compute_track_speeds(1.5, 5.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.3)
        assert -1.5 <= vl <= 1.5
        assert -1.5 <= vr <= 1.5

    def test_clipping_2wd(self):
        vl, vr = compute_track_speeds(1.5, 5.0, 0.30, 1.5, drive_mode="2wd")
        assert -1.5 <= vl <= 1.5
        assert -1.5 <= vr <= 1.5

    def test_tracked_differs_from_2wd_on_turn(self):
        """With slip>0, tracked mode should produce a different differential."""
        vl_t, vr_t = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
                                           drive_mode="tracked", slip_factor=0.3)
        vl_2, vr_2 = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
                                           drive_mode="2wd", slip_factor=0.3)
        assert abs(vr_t - vl_t) > abs(vr_2 - vl_2)

    def test_unknown_mode_falls_back(self):
        """Unknown drive_mode: falls through to skid_steer (no slip comp)."""
        vl, vr = compute_track_speeds(1.0, 0.5, 0.30, 1.5, drive_mode="bogus")
        vl_ref, vr_ref = compute_track_speeds(1.0, 0.5, 0.30, 1.5, drive_mode="2wd")
        assert vl == pytest.approx(vl_ref)
        assert vr == pytest.approx(vr_ref)

    def test_zero_pivot_tracked(self):
        vl, vr = compute_track_speeds(0.0, 1.0, 0.30, 1.5, drive_mode="tracked", slip_factor=0.0)
        assert vl == pytest.approx(-vr)

    def test_reverse_straight(self):
        vl, vr = compute_track_speeds(-1.0, 0.0, 0.30, 1.5, drive_mode="tracked")
        assert vl == pytest.approx(-1.0)
        assert vr == pytest.approx(-1.0)


# ─── expand_to_4ch ───────────────────────────────────────────────────────────

class TestExpandTo4ch:

    def test_returns_four_values(self):
        result = expand_to_4ch(1.0, -1.0)
        assert len(result) == 4

    def test_left_pair(self):
        vfl, vlr, vrf, vrr = expand_to_4ch(0.5, 1.0)
        assert vfl == pytest.approx(0.5)
        assert vlr == pytest.approx(0.5)

    def test_right_pair(self):
        vfl, vlr, vrf, vrr = expand_to_4ch(0.5, 1.0)
        assert vrf == pytest.approx(1.0)
        assert vrr == pytest.approx(1.0)

    def test_zero_inputs(self):
        assert expand_to_4ch(0.0, 0.0) == (0.0, 0.0, 0.0, 0.0)

    def test_negative(self):
        vfl, vlr, vrf, vrr = expand_to_4ch(-0.5, -0.5)
        assert all(v == pytest.approx(-0.5) for v in (vfl, vlr, vrf, vrr))


# ─── odometry_from_track_speeds ──────────────────────────────────────────────

class TestOdometryFromTrackSpeeds:

    def test_straight_no_slip(self):
        lin, ang = odometry_from_track_speeds(1.0, 1.0, 0.30, slip_factor=0.0)
        assert lin == pytest.approx(1.0)
        assert ang == pytest.approx(0.0)

    def test_point_turn_ccw_no_slip(self):
        vl, vr = -0.15, 0.15
        lin, ang = odometry_from_track_speeds(vl, vr, 0.30, slip_factor=0.0)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(1.0)

    def test_slip_reduces_angular_estimate(self):
        """With slip, measured angular is scaled down."""
        lin0, ang0 = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.0)
        lin3, ang3 = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.3)
        assert abs(ang3) < abs(ang0)

    def test_known_slip_value(self):
        """slip=0.5 → angular halved."""
        lin, ang = odometry_from_track_speeds(-0.15, 0.15, 0.30, slip_factor=0.5)
        # (0.15 - -0.15) / 0.30 * (1 - 0.5) = 1.0 * 0.5
        assert ang == pytest.approx(0.5)

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

    def test_zero_inputs(self):
        lin, ang = odometry_from_track_speeds(0.0, 0.0, 0.30, slip_factor=0.0)
        assert lin == pytest.approx(0.0)
        assert ang == pytest.approx(0.0)

    def test_reverse_straight(self):
        lin, ang = odometry_from_track_speeds(-1.0, -1.0, 0.30, slip_factor=0.0)
        assert lin == pytest.approx(-1.0)
        assert ang == pytest.approx(0.0)


# ─── Integration: command ↔ odometry round-trip ──────────────────────────────

class TestRoundTrip:

    @pytest.mark.parametrize("slip", [0.0, 0.1, 0.3, 0.5])
    @pytest.mark.parametrize("lin,ang", [
        (1.0,  0.0),
        (0.0,  1.0),
        (0.5,  0.5),
        (-0.5, 0.3),
    ])
    def test_roundtrip_tracked(self, lin, ang, slip):
        """
        Commanding (lin, ang) then estimating back via odometry should recover
        (lin, ang) for all slip values, provided the speeds are unclipped.
        """
        track = 0.30
        max_speed = 5.0   # generous to avoid clipping in test
        vl, vr = compute_track_speeds(lin, ang, track, max_speed,
                                       drive_mode="tracked", slip_factor=slip)
        lin_out, ang_out = odometry_from_track_speeds(vl, vr, track, slip_factor=slip)
        assert lin_out == pytest.approx(lin, abs=1e-9)
        assert ang_out == pytest.approx(ang, abs=1e-9)

    def test_zero_pivot_pwm_symmetric(self):
        """Zero-radius pivot: left and right PWMs are symmetric around neutral."""
        vl, vr = compute_track_speeds(0.0, 1.0, 0.30, 1.5,
                                       drive_mode="tracked", slip_factor=0.0)
        pwm_l = speed_to_pwm(vl, 1.5, 1500, 500)
        pwm_r = speed_to_pwm(vr, 1.5, 1500, 500)
        assert pwm_l + pwm_r == 3000   # symmetric: 1500 - x + 1500 + x

    def test_all_modes_forward_equal(self):
        """Straight forward: all drive modes produce the same speeds."""
        for mode in DRIVE_MODES:
            vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode=mode)
            assert vl == pytest.approx(1.0)
            assert vr == pytest.approx(1.0)

    def test_4ch_expansion_pwm(self):
        """4-channel expansion: all channels driven during straight motion."""
        vl, vr = compute_track_speeds(1.0, 0.0, 0.30, 1.5, drive_mode="4wd")
        pwms = [speed_to_pwm(v, 1.5, 1500, 500) for v in expand_to_4ch(vl, vr)]
        assert all(p > 1500 for p in pwms)

    def test_pwm_neutral_when_stopped(self):
        vl, vr = compute_track_speeds(0.0, 0.0, 0.30, 1.5, drive_mode="tracked")
        for v in (vl, vr):
            assert speed_to_pwm(v, 1.5, 1500, 500) == 1500
