"""Unit tests for differential drive odometry kinematics (Issue #646).

Tests run without any ROS2 installation — DiffDriveOdometry has no ROS deps.
"""

import math
import sys
import os

import pytest

# Allow import from package source without installing
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))

from saltybot_nav2_slam.diff_drive_odom import DiffDriveOdometry


def make_odom(**kwargs) -> DiffDriveOdometry:
    defaults = dict(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7, invert_right=False)
    defaults.update(kwargs)
    return DiffDriveOdometry(**defaults)


# ── Construction & reset ──────────────────────────────────────────────────────

def test_initial_state_zero():
    odom = make_odom()
    assert odom.x == 0.0
    assert odom.y == 0.0
    assert odom.theta == 0.0
    assert odom.v_lin == 0.0
    assert odom.v_ang == 0.0


def test_reset_sets_pose():
    odom = make_odom()
    odom.reset(x=1.0, y=2.0, theta=math.pi / 4)
    assert odom.x == 1.0
    assert odom.y == 2.0
    assert abs(odom.theta - math.pi / 4) < 1e-12


def test_reset_clears_velocity():
    odom = make_odom()
    # Drive forward then reset
    odom.update(erpm_left=4200, erpm_right=4200, dt=0.1)
    assert odom.v_lin != 0.0
    odom.reset()
    assert odom.v_lin == 0.0
    assert odom.v_ang == 0.0


# ── eRPM → velocity conversion ────────────────────────────────────────────────

def test_erpm_to_wheel_vel_zero():
    odom = make_odom(wheel_radius=0.10, motor_poles=7)
    assert odom.erpm_to_wheel_vel(0.0) == 0.0


def test_erpm_to_wheel_vel_positive():
    odom = make_odom(wheel_radius=0.10, motor_poles=7)
    # 4200 eRPM / 7 poles = 600 mech RPM = 10 rev/s = 10 × 2π × 0.1 m/s ≈ 6.283 m/s
    expected = (4200 / 7 / 60.0) * (2.0 * math.pi) * 0.10
    assert abs(odom.erpm_to_wheel_vel(4200) - expected) < 1e-9


def test_erpm_to_wheel_vel_negative():
    odom = make_odom(wheel_radius=0.10, motor_poles=7)
    pos = odom.erpm_to_wheel_vel(4200)
    neg = odom.erpm_to_wheel_vel(-4200)
    assert abs(pos + neg) < 1e-9


# ── Straight-line motion ──────────────────────────────────────────────────────

def test_straight_forward():
    """Both wheels same eRPM → straight forward motion."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=4200, dt=1.0)

    assert v_ang == pytest.approx(0.0, abs=1e-9)
    assert v_lin > 0.0
    assert odom.x > 0.0
    assert abs(odom.y) < 1e-9
    assert abs(odom.theta) < 1e-9


def test_straight_backward():
    """Both wheels negative same eRPM → straight backward motion."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
    v_lin, v_ang = odom.update(erpm_left=-4200, erpm_right=-4200, dt=1.0)

    assert v_ang == pytest.approx(0.0, abs=1e-9)
    assert v_lin < 0.0
    assert odom.x < 0.0
    assert abs(odom.y) < 1e-9


def test_straight_distance():
    """Verify computed distance matches kinematics formula."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
    # 600 eRPM / 1 pole = 600 mech RPM = 10 rev/s = 10×2π×0.10 = 6.2832 m/s
    expected_v = (600.0 / 60.0) * (2.0 * math.pi) * 0.10
    dt = 2.0
    odom.update(erpm_left=600, erpm_right=600, dt=dt)
    assert abs(odom.x - expected_v * dt) < 1e-9
    assert abs(odom.y) < 1e-9


# ── Zero-radius (point) rotation ──────────────────────────────────────────────

def test_spin_in_place_cw():
    """Right faster than left → rotate clockwise (negative omega)."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
    # right > left → v_ang = (v_right - v_left) / sep > 0 → CCW in ROS convention
    v_lin, v_ang = odom.update(erpm_left=-4200, erpm_right=4200, dt=1.0)
    assert v_ang > 0.0                          # CCW
    assert abs(v_lin) < 1e-9                    # no net translation
    assert abs(odom.x)  < 1e-6
    assert abs(odom.y)  < 1e-6


def test_spin_in_place_ccw():
    """Left faster than right → clockwise (negative omega)."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=-4200, dt=1.0)
    assert v_ang < 0.0                          # CW
    assert abs(v_lin) < 1e-9
    assert abs(odom.x)  < 1e-6
    assert abs(odom.y)  < 1e-6


def test_full_rotation():
    """Spin in place for exactly 2π, ending back at (0, 0, 0)."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
    # v_right = +1 m/s, v_left = -1 m/s → v_ang = 2/0.35 rad/s
    # t_full = 2π / v_ang  → drive for exactly that long
    v_ang_expected = 2.0 / 0.35
    t_full = (2.0 * math.pi) / v_ang_expected
    # eRPM to produce 1 m/s: 1 = (eRPM/60)×2π×0.10 → eRPM = 600/2π×60 ≈ 95.49
    erpm = (1.0 / (2.0 * math.pi * 0.10)) * 60.0
    odom.update(erpm_left=-erpm, erpm_right=erpm, dt=t_full)
    assert abs(odom.x)     < 1e-6
    assert abs(odom.y)     < 1e-6
    assert abs(odom.theta) < 1e-6


# ── Arc motion ────────────────────────────────────────────────────────────────

def test_quarter_circle_arc():
    """Drive a quarter circle: verify end position matches arc geometry."""
    radius = 1.0   # m — desired turn radius
    sep    = 0.35  # m — wheel separation
    r_left  = radius - sep / 2.0
    r_right = radius + sep / 2.0

    # v_lin = (v_r + v_l)/2 = radius × omega
    # v_ang = (v_r - v_l)/sep = omega
    # choose omega = 1 rad/s → t_quarter = π/2 s
    omega    = 1.0   # rad/s
    v_left   = r_left  * omega
    v_right  = r_right * omega
    t_quarter = math.pi / 2.0

    # Convert velocities to eRPM with motor_poles=1
    def vel_to_erpm(v, r=0.10): return (v / (2.0 * math.pi * r)) * 60.0

    odom = make_odom(wheel_radius=0.10, wheel_separation=sep, motor_poles=1)
    odom.update(
        erpm_left=vel_to_erpm(v_left),
        erpm_right=vel_to_erpm(v_right),
        dt=t_quarter,
    )

    # After a CCW quarter circle of radius `radius`:
    #   x = radius * sin(π/2) = radius
    #   y = radius * (1 - cos(π/2)) = radius
    #   theta = π/2
    assert abs(odom.x - radius) < 1e-3, f"x={odom.x:.4f} expected≈{radius}"
    assert abs(odom.y - radius) < 1e-3, f"y={odom.y:.4f} expected≈{radius}"
    assert abs(odom.theta - math.pi / 2.0) < 1e-6


# ── invert_right flag ────────────────────────────────────────────────────────

def test_invert_right_reverses_direction():
    """With invert_right=True, same positive eRPM on both should still go forward."""
    odom_normal = make_odom(invert_right=False)
    odom_invert = make_odom(invert_right=True)

    # Without inversion: left=positive, right=positive → both forward → straight forward
    odom_normal.update(erpm_left=4200, erpm_right=4200, dt=1.0)
    # With inversion: right eRPM is negated internally → left=+, right=- → net forward ?
    odom_invert.update(erpm_left=4200, erpm_right=-4200, dt=1.0)
    # Both should yield same result (inversion compensates the physical mirror)
    assert abs(odom_normal.x - odom_invert.x) < 1e-9
    assert abs(odom_normal.y - odom_invert.y) < 1e-9
    assert abs(odom_normal.theta - odom_invert.theta) < 1e-9


# ── Heading wrap ──────────────────────────────────────────────────────────────

def test_heading_wraps_at_pi():
    """Heading should always stay within (-π, π]."""
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=1)
    erpm = (1.0 / (2.0 * math.pi * 0.10)) * 60.0
    for _ in range(20):
        odom.update(erpm_left=-erpm, erpm_right=erpm, dt=0.5)
        assert -math.pi < odom.theta <= math.pi, f"theta={odom.theta} out of range"


# ── Guard conditions ──────────────────────────────────────────────────────────

def test_zero_dt_no_change():
    """dt=0 should not change state."""
    odom = make_odom()
    odom.update(erpm_left=4200, erpm_right=4200, dt=0.0)
    assert odom.x == 0.0
    assert odom.y == 0.0
    assert odom.theta == 0.0


def test_negative_dt_no_change():
    """Negative dt should not change state."""
    odom = make_odom()
    odom.update(erpm_left=4200, erpm_right=4200, dt=-1.0)
    assert odom.x == 0.0


def test_zero_erpm_no_motion():
    """Zero RPM on both motors → no motion."""
    odom = make_odom()
    odom.update(erpm_left=0, erpm_right=0, dt=1.0)
    assert odom.x == 0.0
    assert odom.y == 0.0
    assert odom.theta == 0.0
    assert odom.v_lin == 0.0
    assert odom.v_ang == 0.0


# ── Velocity output ───────────────────────────────────────────────────────────

def test_update_returns_velocities():
    odom = make_odom(wheel_radius=0.10, wheel_separation=0.35, motor_poles=7)
    v_lin, v_ang = odom.update(erpm_left=4200, erpm_right=4200, dt=0.02)
    assert v_lin == odom.v_lin
    assert v_ang == odom.v_ang


def test_covariance_fields():
    """Verify covariance arrays have correct length for nav_msgs/Odometry."""
    # 6×6 = 36 elements
    assert 36 == 36  # placeholder — actual covariance set in ROS node


if __name__ == "__main__":
    pytest.main([__file__, "-v"])
