"""Differential-drive kinematics and the (vx, vy) → wheel-speed map."""

import math

import pytest

from herding.world.diffdrive import (
    heading_speed_to_wheels, kinematics_step, velocity_to_wheels,
)


WHEEL_R = 0.038
WHEEL_B = 0.28
MAX_OMEGA = 70.0
MAX_LINEAR = WHEEL_R * MAX_OMEGA
DT = 0.016


def test_kinematics_zero_input_is_identity():
    x, y, h = kinematics_step(1.0, 2.0, 0.5, 0.0, 0.0, WHEEL_R, WHEEL_B, DT)
    assert (x, y, h) == (1.0, 2.0, 0.5)


def test_kinematics_forward_motion():
    # Equal wheel speeds → pure translation along the heading.
    x, y, h = kinematics_step(0.0, 0.0, 0.0, 10.0, 10.0, WHEEL_R, WHEEL_B, DT)
    assert h == 0.0
    assert math.isclose(x, 10.0 * WHEEL_R * DT)
    assert y == 0.0


def test_kinematics_pure_rotation():
    # Opposite wheel speeds → pure rotation, position unchanged.
    x, y, h = kinematics_step(0.0, 0.0, 0.0, -5.0, 5.0, WHEEL_R, WHEEL_B, DT)
    assert (x, y) == (0.0, 0.0)
    assert h > 0.0


def test_kinematics_heading_wrapped_to_pi():
    _, _, h = kinematics_step(0.0, 0.0, math.pi - 0.01, 100.0, -100.0,
                              WHEEL_R, WHEEL_B, DT)
    assert -math.pi <= h <= math.pi


def test_velocity_to_wheels_zero_velocity():
    left, right = velocity_to_wheels(0.0, 0.0, 0.0,
                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA)
    assert (left, right) == (0.0, 0.0)


def test_velocity_to_wheels_aligned_forward():
    # Target straight ahead → equal positive wheel speeds.
    left, right = velocity_to_wheels(1.0, 0.0, 0.0,
                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
    assert math.isclose(left, right, abs_tol=1e-6)
    assert left > 0.0


def test_velocity_to_wheels_perpendicular_target_spins():
    # Target 90° from heading → forward speed ≈ 0, wheels equal-and-opposite.
    left, right = velocity_to_wheels(0.0, 1.0, 0.0,
                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
    assert left + right == pytest.approx(0.0, abs=1e-6)
    assert right > 0.0  # turning CCW (left of heading is +y for h=0)


def test_velocity_to_wheels_clamped_to_max_omega():
    # Far overshoot — both wheel commands clamped at ±MAX_OMEGA.
    left, right = velocity_to_wheels(-1.0, 0.0, 0.0,
                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=20.0)
    assert -MAX_OMEGA <= left <= MAX_OMEGA
    assert -MAX_OMEGA <= right <= MAX_OMEGA


def test_heading_speed_to_wheels_aligned():
    left, right = heading_speed_to_wheels(0.0, 10.0, 0.0, MAX_OMEGA)
    assert math.isclose(left, right, abs_tol=1e-6)
    assert left > 0.0


def test_heading_speed_to_wheels_reverse_target_forwards_zero():
    left, right = heading_speed_to_wheels(math.pi, 10.0, 0.0, MAX_OMEGA)
    # cos(π) clamped at 0 → no forward; pure rotation.
    assert left + right == pytest.approx(0.0, abs=1e-6)