"""
test_wheel_odom.py — Unit tests for wheel odometry kinematics (no ROS2 required).

Covers:
  unwrap_delta:
    - normal positive delta
    - normal negative delta
    - zero delta
    - positive rollover (counter crosses +2^31 boundary)
    - negative rollover (counter crosses -2^31 boundary)
    - near-zero after positive rollover
    - large forward motion (just under rollover threshold)

  ticks_to_meters:
    - zero ticks → 0.0 m
    - one full revolution → 2π * radius
    - half revolution → π * radius
    - negative ticks → negative displacement
    - ticks_per_rev = 0 → 0.0 (guard)
    - fractional result correctness

  normalize_angle:
    - 0 → 0
    - π → π (or −π, both valid; atan2 returns in (−π, π])
    - 2π → 0
    - −π → −π (or π)
    - 3π/2 → −π/2

  integrate_odom — straight line:
    - equal d_left == d_right → x increases, y unchanged, theta unchanged
    - moving backward (negative equal displacements) → x decreases

  integrate_odom — rotation:
    - d_right > d_left → theta increases (left turn)
    - d_left > d_right → theta decreases (right turn)
    - spin in place (d_left = −d_right) → x,y unchanged, theta = d_theta

  integrate_odom — circular motion:
    - four identical quarter-turns return near-original position
    - heading after 90° left turn ≈ π/2

  integrate_odom — starting from non-zero pose:
    - displacement is applied in the current heading direction

  quat_from_yaw:
    - yaw=0 → qw=1, qx=qy=qz=0
    - yaw=π → qw≈0, qz≈1
    - yaw=π/2 → qw=qz≈1/√2, qx=qy=0
    - unit quaternion: qx²+qy²+qz²+qw²=1 for arbitrary yaw
    - yaw=−π/2 → qz≈−1/√2

  WheelOdomState:
    - fields accessible by name
    - immutable (NamedTuple)
"""

import sys
import os
import math

import pytest

sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))

from saltybot_bringup._wheel_odom import (
    WheelOdomState,
    unwrap_delta,
    ticks_to_meters,
    normalize_angle,
    integrate_odom,
    quat_from_yaw,
)

# ── Constants ─────────────────────────────────────────────────────────────────

_R    = 0.034    # wheel radius (m)
_BASE = 0.160    # wheel base (m)
_TPR  = 1000     # ticks per rev


# ── WheelOdomState ────────────────────────────────────────────────────────────

class TestWheelOdomState:

    def test_fields_accessible(self):
        s = WheelOdomState(x=1.0, y=2.0, theta=0.5)
        assert s.x     == pytest.approx(1.0)
        assert s.y     == pytest.approx(2.0)
        assert s.theta == pytest.approx(0.5)

    def test_is_namedtuple(self):
        s = WheelOdomState(0.0, 0.0, 0.0)
        assert isinstance(s, tuple)


# ── unwrap_delta ──────────────────────────────────────────────────────────────

class TestUnwrapDelta:

    def test_normal_positive(self):
        assert unwrap_delta(100, 50) == 50

    def test_normal_negative(self):
        assert unwrap_delta(50, 100) == -50

    def test_zero(self):
        assert unwrap_delta(200, 200) == 0

    def test_positive_rollover(self):
        """Counter wraps from just below +2^31 to just above −2^31."""
        MAX = 2 ** 31
        prev    =  MAX - 10
        current = -MAX + 5   # wrapped: total advance = 15 ticks
        assert unwrap_delta(current, prev) == 15

    def test_negative_rollover(self):
        """Counter wraps from just above −2^31 to just below +2^31."""
        MAX = 2 ** 31
        prev    = -MAX + 10
        current =  MAX - 5   # wrapped backward: delta = −15
        assert unwrap_delta(current, prev) == -15

    def test_large_forward_no_rollover(self):
        """Delta just under the rollover threshold should not be treated as rollover."""
        half = 2 ** 30
        assert unwrap_delta(half - 1, 0) == half - 1

    def test_symmetry(self):
        assert unwrap_delta(300, 200) == -unwrap_delta(200, 300)


# ── ticks_to_meters ───────────────────────────────────────────────────────────

class TestTicksToMeters:

    def test_zero_ticks(self):
        assert ticks_to_meters(0, _R, _TPR) == pytest.approx(0.0)

    def test_one_full_revolution(self):
        expected = 2.0 * math.pi * _R
        assert ticks_to_meters(_TPR, _R, _TPR) == pytest.approx(expected, rel=1e-9)

    def test_half_revolution(self):
        expected = math.pi * _R
        assert ticks_to_meters(_TPR // 2, _R, _TPR) == pytest.approx(expected, rel=1e-4)

    def test_negative_ticks(self):
        d = ticks_to_meters(-_TPR, _R, _TPR)
        assert d == pytest.approx(-(2.0 * math.pi * _R), rel=1e-9)

    def test_ticks_per_rev_zero_returns_zero(self):
        assert ticks_to_meters(100, _R, 0) == pytest.approx(0.0)

    def test_proportional(self):
        d1 = ticks_to_meters(100, _R, _TPR)
        d2 = ticks_to_meters(200, _R, _TPR)
        assert d2 == pytest.approx(2 * d1, rel=1e-9)


# ── normalize_angle ───────────────────────────────────────────────────────────

class TestNormalizeAngle:

    def test_zero(self):
        assert normalize_angle(0.0) == pytest.approx(0.0)

    def test_two_pi_to_zero(self):
        assert normalize_angle(2 * math.pi) == pytest.approx(0.0, abs=1e-10)

    def test_three_half_pi_to_neg_half_pi(self):
        assert normalize_angle(3 * math.pi / 2) == pytest.approx(-math.pi / 2, rel=1e-9)

    def test_minus_three_half_pi_to_half_pi(self):
        assert normalize_angle(-3 * math.pi / 2) == pytest.approx(math.pi / 2, rel=1e-9)

    def test_pi_stays_in_range(self):
        v = normalize_angle(math.pi)
        assert -math.pi <= v <= math.pi

    def test_large_angle_wraps(self):
        v = normalize_angle(100 * math.pi + 0.1)
        assert -math.pi < v <= math.pi
        assert v == pytest.approx(0.1, rel=1e-6)


# ── integrate_odom — straight line ───────────────────────────────────────────

class TestIntegrateOdomStraight:

    def test_straight_forward_x_increases(self):
        d = 0.1
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d, d, _BASE)
        assert x == pytest.approx(d, rel=1e-9)
        assert y == pytest.approx(0.0, abs=1e-12)
        assert theta == pytest.approx(0.0, abs=1e-12)

    def test_straight_backward_x_decreases(self):
        d = -0.05
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d, d, _BASE)
        assert x == pytest.approx(d, rel=1e-9)
        assert y == pytest.approx(0.0, abs=1e-12)

    def test_straight_along_y_axis(self):
        """Starting at theta=π/2, forward motion should increase y."""
        d = 0.1
        x, y, theta = integrate_odom(0.0, 0.0, math.pi / 2, d, d, _BASE)
        assert x == pytest.approx(0.0, abs=1e-10)
        assert y == pytest.approx(d, rel=1e-9)

    def test_straight_accumulates(self):
        """Multiple straight steps accumulate correctly."""
        x, y, theta = 0.0, 0.0, 0.0
        for _ in range(10):
            x, y, theta = integrate_odom(x, y, theta, 0.01, 0.01, _BASE)
        assert x == pytest.approx(0.10, rel=1e-6)
        assert y == pytest.approx(0.0,  abs=1e-10)


# ── integrate_odom — rotation ─────────────────────────────────────────────────

class TestIntegrateOdomRotation:

    def test_right_wheel_faster_turns_left(self):
        """d_right > d_left → robot turns left → theta increases."""
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, 0.0, 0.01, _BASE)
        assert theta > 0.0

    def test_left_wheel_faster_turns_right(self):
        """d_left > d_right → robot turns right → theta decreases."""
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, 0.01, 0.0, _BASE)
        assert theta < 0.0

    def test_spin_in_place_xy_unchanged(self):
        """d_left = −d_right → pure rotation, position unchanged."""
        d = 0.01
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, -d, d, _BASE)
        assert x == pytest.approx(0.0, abs=1e-10)
        assert y == pytest.approx(0.0, abs=1e-10)

    def test_spin_in_place_theta_correct(self):
        """Spinning one radian: d_theta = (d_right − d_left) / base."""
        d_theta_target = 1.0
        d = d_theta_target * _BASE / 2.0
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, -d, d, _BASE)
        assert theta == pytest.approx(d_theta_target, rel=1e-9)

    def test_90deg_left_turn_heading(self):
        """Quarter arc left turn → theta ≈ π/2."""
        # Arc length for 90° left turn on wheel base _BASE, radius R:
        # r = R + base/2 for outer (right) wheel; r - base/2 for inner (left)
        arc_radius = 0.5   # arbitrary turn radius
        d_theta = math.pi / 2
        d_right = (arc_radius + _BASE / 2) * d_theta
        d_left  = (arc_radius - _BASE / 2) * d_theta
        x, y, theta = integrate_odom(0.0, 0.0, 0.0, d_left, d_right, _BASE)
        assert theta == pytest.approx(math.pi / 2, rel=1e-9)


# ── integrate_odom — circular closure ────────────────────────────────────────

class TestIntegrateOdomClosure:

    def test_four_quarter_turns_return_to_origin(self):
        """
        Four 90° left arcs (same radius) should close into a full circle and
        return approximately to the origin.
        """
        arc_r   = 0.30        # turning radius (m)
        d_theta = math.pi / 2  # 90° per arc

        d_right = (arc_r + _BASE / 2) * d_theta
        d_left  = (arc_r - _BASE / 2) * d_theta

        x, y, theta = 0.0, 0.0, 0.0
        for _ in range(4):
            x, y, theta = integrate_odom(x, y, theta, d_left, d_right, _BASE)

        assert x     == pytest.approx(0.0, abs=1e-9)
        assert y     == pytest.approx(0.0, abs=1e-9)
        assert theta == pytest.approx(0.0, abs=1e-9)

    def test_full_spin_in_place(self):
        """Spinning 2π in small steps should return theta to 0."""
        d_theta_step = math.pi / 18  # 10° per step
        d            = d_theta_step * _BASE / 2.0
        x, y, theta  = 0.0, 0.0, 0.0
        for _ in range(36):          # 36 × 10° = 360°
            x, y, theta = integrate_odom(x, y, theta, -d, d, _BASE)
        assert theta == pytest.approx(0.0, abs=1e-9)


# ── quat_from_yaw ─────────────────────────────────────────────────────────────

class TestQuatFromYaw:

    def test_zero_yaw(self):
        qx, qy, qz, qw = quat_from_yaw(0.0)
        assert qx == pytest.approx(0.0, abs=1e-12)
        assert qy == pytest.approx(0.0, abs=1e-12)
        assert qz == pytest.approx(0.0, abs=1e-12)
        assert qw == pytest.approx(1.0)

    def test_pi_yaw(self):
        qx, qy, qz, qw = quat_from_yaw(math.pi)
        assert qx == pytest.approx(0.0, abs=1e-10)
        assert qy == pytest.approx(0.0, abs=1e-10)
        assert abs(qz) == pytest.approx(1.0, rel=1e-9)
        assert abs(qw) == pytest.approx(0.0, abs=1e-10)

    def test_half_pi_yaw(self):
        qx, qy, qz, qw = quat_from_yaw(math.pi / 2)
        s = 1.0 / math.sqrt(2)
        assert qx == pytest.approx(0.0, abs=1e-12)
        assert qy == pytest.approx(0.0, abs=1e-12)
        assert qz == pytest.approx(s, rel=1e-9)
        assert qw == pytest.approx(s, rel=1e-9)

    def test_neg_half_pi_yaw(self):
        qx, qy, qz, qw = quat_from_yaw(-math.pi / 2)
        s = 1.0 / math.sqrt(2)
        assert qz == pytest.approx(-s, rel=1e-9)
        assert qw == pytest.approx(s,  rel=1e-9)

    @pytest.mark.parametrize('yaw', [0.0, 0.1, 0.5, math.pi, -math.pi / 3, 2.7])
    def test_unit_quaternion(self, yaw):
        qx, qy, qz, qw = quat_from_yaw(yaw)
        norm_sq = qx**2 + qy**2 + qz**2 + qw**2
        assert norm_sq == pytest.approx(1.0, rel=1e-9)


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