"""
test_vo_drift.py — Unit tests for VO drift detector helpers (no ROS2 required).

Covers:
  OdomBuffer:
    - push/len
    - window returns only samples within cutoff
    - old samples are evicted beyond max_age_s
    - clear empties the buffer
    - window on empty buffer returns empty list

  _path_length (via compute_drift with crafted samples):
    - stationary source → path = 0
    - straight-line motion → path = total distance
    - L-shaped path → path = sum of two legs

  compute_drift:
    - both empty → DriftResult with zeros, is_drifting=False
    - one buffer < 2 samples → zero drift
    - both move same distance → drift ≈ 0, not drifting
    - VO moves 1m, wheel moves 0.5m → drift = 0.5m
    - drift == threshold → is_drifting=True (>=)
    - drift < threshold → is_drifting=False
    - drift > threshold → is_drifting=True
    - path lengths in result match expectation
    - n_vo / n_wheel counts correct
    - samples outside window ignored
    - window_s in result reflects actual data span
"""

import sys
import os
import math

import pytest

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

from saltybot_bringup._vo_drift import (
    OdomSample,
    OdomBuffer,
    DriftResult,
    compute_drift,
    _path_length,
)


# ── Helpers ───────────────────────────────────────────────────────────────────

def _s(t, x, y) -> OdomSample:
    return OdomSample(t=t, x=x, y=y)


def _straight_buf(n=5, speed=0.1, t_start=0.0, dt=1.0,
                  max_age_s=30.0) -> OdomBuffer:
    """n samples moving along +x at `speed` m/s."""
    buf = OdomBuffer(max_age_s=max_age_s)
    for i in range(n):
        buf.push(_s(t_start + i * dt, x=i * speed * dt, y=0.0))
    return buf


def _stationary_buf(n=5, t_start=0.0, dt=1.0,
                    max_age_s=30.0) -> OdomBuffer:
    buf = OdomBuffer(max_age_s=max_age_s)
    for i in range(n):
        buf.push(_s(t_start + i * dt, x=0.0, y=0.0))
    return buf


# ── OdomBuffer ────────────────────────────────────────────────────────────────

class TestOdomBuffer:

    def test_push_increases_len(self):
        buf = OdomBuffer()
        assert len(buf) == 0
        buf.push(_s(0.0, 0.0, 0.0))
        assert len(buf) == 1

    def test_window_returns_all_within_cutoff(self):
        buf = OdomBuffer(max_age_s=30.0)
        for t in [0.0, 5.0, 10.0]:
            buf.push(_s(t, 0.0, 0.0))
        samples = buf.window(window_s=10.0, now=10.0)
        assert len(samples) == 3

    def test_window_excludes_old_samples(self):
        buf = OdomBuffer(max_age_s=30.0)
        for t in [0.0, 5.0, 15.0]:
            buf.push(_s(t, 0.0, 0.0))
        # window=5s from now=15 → only t=15 qualifies (t>=10)
        samples = buf.window(window_s=5.0, now=15.0)
        assert len(samples) == 1
        assert samples[0].t == 15.0

    def test_evicts_samples_beyond_max_age(self):
        buf = OdomBuffer(max_age_s=5.0)
        buf.push(_s(0.0, 0.0, 0.0))
        buf.push(_s(10.0, 1.0, 0.0))  # now=10 → t=0 is 10s old > 5s max
        assert len(buf) == 1

    def test_clear_empties_buffer(self):
        buf = _straight_buf(n=5)
        buf.clear()
        assert len(buf) == 0

    def test_window_on_empty_buffer(self):
        buf = OdomBuffer()
        assert buf.window(window_s=10.0, now=100.0) == []

    def test_window_boundary_inclusive(self):
        """Sample exactly at window cutoff (t == now - window_s) is included."""
        buf = OdomBuffer(max_age_s=30.0)
        buf.push(_s(0.0, 0.0, 0.0))
        # window=10, now=10 → cutoff=0.0, sample at t=0.0 should be included
        samples = buf.window(window_s=10.0, now=10.0)
        assert len(samples) == 1


# ── _path_length ──────────────────────────────────────────────────────────────

class TestPathLength:

    def test_stationary_path_zero(self):
        samples = [_s(i, 0.0, 0.0) for i in range(5)]
        assert _path_length(samples) == pytest.approx(0.0)

    def test_unit_step_path(self):
        samples = [_s(0, 0.0, 0.0), _s(1, 1.0, 0.0)]
        assert _path_length(samples) == pytest.approx(1.0)

    def test_two_unit_steps(self):
        samples = [_s(0, 0.0, 0.0), _s(1, 1.0, 0.0), _s(2, 2.0, 0.0)]
        assert _path_length(samples) == pytest.approx(2.0)

    def test_diagonal_step(self):
        # (0,0) → (1,1): distance = sqrt(2)
        samples = [_s(0, 0.0, 0.0), _s(1, 1.0, 1.0)]
        assert _path_length(samples) == pytest.approx(math.sqrt(2))

    def test_l_shaped_path(self):
        # Right 3m then up 4m → total path = 7m (not hypotenuse)
        samples = [_s(0, 0.0, 0.0), _s(1, 3.0, 0.0), _s(2, 3.0, 4.0)]
        assert _path_length(samples) == pytest.approx(7.0)

    def test_single_sample_returns_zero(self):
        assert _path_length([_s(0, 5.0, 5.0)]) == pytest.approx(0.0)

    def test_empty_returns_zero(self):
        assert _path_length([]) == pytest.approx(0.0)


# ── compute_drift ─────────────────────────────────────────────────────────────

class TestComputeDrift:

    def test_both_empty_returns_zero_drift(self):
        result = compute_drift(
            OdomBuffer(), OdomBuffer(),
            window_s=10.0, drift_threshold_m=0.5, now=10.0)
        assert result.drift_m == pytest.approx(0.0)
        assert not result.is_drifting

    def test_one_buffer_empty_returns_zero(self):
        vo = _straight_buf(n=5, speed=0.1)
        result = compute_drift(
            vo, OdomBuffer(),
            window_s=10.0, drift_threshold_m=0.5, now=5.0)
        assert result.drift_m == pytest.approx(0.0)
        assert not result.is_drifting

    def test_one_buffer_single_sample_returns_zero(self):
        vo    = _straight_buf(n=5, speed=0.1)
        wheel = OdomBuffer()
        wheel.push(_s(0.0, 0.0, 0.0))   # only 1 sample
        result = compute_drift(
            vo, wheel,
            window_s=10.0, drift_threshold_m=0.5, now=5.0)
        assert result.drift_m == pytest.approx(0.0)
        assert not result.is_drifting

    def test_both_move_same_distance_zero_drift(self):
        # Both move 0.1 m/s for 4 steps → 0.4 m each
        vo    = _straight_buf(n=5, speed=0.1, dt=1.0)
        wheel = _straight_buf(n=5, speed=0.1, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=10.0, drift_threshold_m=0.5, now=5.0)
        assert result.drift_m == pytest.approx(0.0, abs=1e-9)
        assert not result.is_drifting

    def test_both_stationary_zero_drift(self):
        vo    = _stationary_buf(n=5)
        wheel = _stationary_buf(n=5)
        result = compute_drift(
            vo, wheel,
            window_s=10.0, drift_threshold_m=0.5, now=5.0)
        assert result.drift_m == pytest.approx(0.0)
        assert not result.is_drifting

    def test_drift_equals_path_length_difference(self):
        # VO moves 1.0 m total, wheel moves 0.5 m total
        vo    = _straight_buf(n=11, speed=0.1, dt=1.0)   # 10 steps × 0.1 = 1.0m
        wheel = _straight_buf(n=11, speed=0.05, dt=1.0)  # 10 steps × 0.05 = 0.5m
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=11.0)
        assert result.vo_path_m    == pytest.approx(1.0, abs=1e-9)
        assert result.wheel_path_m == pytest.approx(0.5, abs=1e-9)
        assert result.drift_m      == pytest.approx(0.5, abs=1e-9)

    def test_drift_at_threshold_is_drifting(self):
        # drift == 0.5 → is_drifting = True  (>= threshold)
        vo    = _straight_buf(n=11, speed=0.1,  dt=1.0)
        wheel = _straight_buf(n=11, speed=0.05, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=11.0)
        assert result.is_drifting

    def test_drift_below_threshold_not_drifting(self):
        vo    = _straight_buf(n=11, speed=0.1,  dt=1.0)
        wheel = _straight_buf(n=11, speed=0.08, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=11.0)
        # drift = |1.0 - 0.8| = 0.2
        assert result.drift_m == pytest.approx(0.2, abs=1e-9)
        assert not result.is_drifting

    def test_drift_above_threshold_is_drifting(self):
        vo    = _straight_buf(n=11, speed=0.1,  dt=1.0)
        wheel = _stationary_buf(n=11, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=11.0)
        # drift = |1.0 - 0.0| = 1.0 > 0.5
        assert result.drift_m > 0.5
        assert result.is_drifting

    def test_n_vo_n_wheel_counts(self):
        vo    = _straight_buf(n=8, dt=1.0)
        wheel = _straight_buf(n=5, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=8.0)
        assert result.n_vo    == 8
        assert result.n_wheel == 5

    def test_samples_outside_window_ignored(self):
        # Push old samples far in the past; should not contribute to window
        vo    = OdomBuffer(max_age_s=60.0)
        wheel = OdomBuffer(max_age_s=60.0)
        # Old samples outside window (t=0..4, window is last 3s from now=10)
        for t in range(5):
            vo.push(_s(float(t), x=float(t), y=0.0))
            wheel.push(_s(float(t), x=float(t), y=0.0))
        # Recent samples inside window (t=7..10)
        for t in range(7, 11):
            vo.push(_s(float(t), x=float(t) * 0.1, y=0.0))
            wheel.push(_s(float(t), x=float(t) * 0.1, y=0.0))

        result = compute_drift(
            vo, wheel,
            window_s=3.0, drift_threshold_m=0.5, now=10.0)
        # Both sources move identically inside window → zero drift
        assert result.drift_m == pytest.approx(0.0, abs=1e-9)
        # Only the 4 recent samples (t=7,8,9,10) in window
        assert result.n_vo    == 4
        assert result.n_wheel == 4

    def test_result_is_namedtuple(self):
        result = compute_drift(
            _straight_buf(), _straight_buf(),
            window_s=10.0, drift_threshold_m=0.5, now=5.0)
        assert hasattr(result, 'drift_m')
        assert hasattr(result, 'vo_path_m')
        assert hasattr(result, 'wheel_path_m')
        assert hasattr(result, 'is_drifting')
        assert hasattr(result, 'window_s')
        assert hasattr(result, 'n_vo')
        assert hasattr(result, 'n_wheel')

    def test_wheel_faster_than_vo_still_drifts(self):
        """Drift is absolute difference — direction doesn't matter."""
        vo    = _stationary_buf(n=11, dt=1.0)
        wheel = _straight_buf(n=11, speed=0.1, dt=1.0)
        result = compute_drift(
            vo, wheel,
            window_s=15.0, drift_threshold_m=0.5, now=11.0)
        assert result.drift_m == pytest.approx(1.0, abs=1e-9)
        assert result.is_drifting


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