"""
test_dynamic_obstacles.py — Unit tests for KalmanTracker, TrackerManager,
and ObjectDetector.

Runs without ROS2 / hardware (no rclpy imports).
"""

from __future__ import annotations

import math
import sys
import os

import numpy as np
import pytest

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

from saltybot_dynamic_obstacles.kalman_tracker import KalmanTracker
from saltybot_dynamic_obstacles.tracker_manager import TrackerManager, TrackState
from saltybot_dynamic_obstacles.object_detector import ObjectDetector, Detection


# ── KalmanTracker ─────────────────────────────────────────────────────────────

class TestKalmanTracker:

    def test_initial_position(self):
        kt = KalmanTracker(3.0, 4.0)
        px, py = kt.position
        assert px == pytest.approx(3.0)
        assert py == pytest.approx(4.0)

    def test_initial_velocity_zero(self):
        kt = KalmanTracker(0.0, 0.0)
        vx, vy = kt.velocity
        assert vx == pytest.approx(0.0)
        assert vy == pytest.approx(0.0)

    def test_predict_moves_position(self):
        kt = KalmanTracker(0.0, 0.0)
        # Give it some velocity via update sequence
        kt.update(np.array([0.1, 0.0]))
        kt.update(np.array([0.2, 0.0]))
        kt.predict(0.1)
        px, _ = kt.position
        assert px > 0.0   # should have moved forward

    def test_pure_predict_constant_velocity(self):
        """After velocity is established, predict() should move linearly."""
        kt = KalmanTracker(0.0, 0.0)
        # Force velocity by repeated updates
        for i in range(10):
            kt.update(np.array([i * 0.1, 0.0]))
            kt.predict(0.1)
        vx, _ = kt.velocity
        px0, _ = kt.position
        kt.predict(1.0)
        px1, _ = kt.position
        # Should advance roughly vx * 1.0 metres
        assert px1 == pytest.approx(px0 + vx * 1.0, abs=0.3)

    def test_update_corrects_position(self):
        kt = KalmanTracker(0.0, 0.0)
        # Predict way off
        kt.predict(10.0)
        # Then update to ground truth
        kt.update(np.array([1.0, 2.0]))
        px, py = kt.position
        # Should move toward (1, 2)
        assert px == pytest.approx(1.0, abs=0.5)
        assert py == pytest.approx(2.0, abs=0.5)

    def test_predict_horizon_length(self):
        kt = KalmanTracker(0.0, 0.0)
        preds = kt.predict_horizon(horizon_s=2.5, step_s=0.5)
        assert len(preds) == 5   # 0.5, 1.0, 1.5, 2.0, 2.5

    def test_predict_horizon_times(self):
        kt = KalmanTracker(0.0, 0.0)
        preds = kt.predict_horizon(horizon_s=2.0, step_s=0.5)
        times = [t for _, _, t in preds]
        assert times == pytest.approx([0.5, 1.0, 1.5, 2.0], abs=0.01)

    def test_predict_horizon_does_not_mutate_state(self):
        kt = KalmanTracker(1.0, 2.0)
        kt.predict_horizon(horizon_s=3.0, step_s=0.5)
        px, py = kt.position
        assert px == pytest.approx(1.0)
        assert py == pytest.approx(2.0)

    def test_speed_zero_at_init(self):
        kt = KalmanTracker(5.0, 5.0)
        assert kt.speed == pytest.approx(0.0)

    def test_covariance_shape(self):
        kt = KalmanTracker(0.0, 0.0)
        cov = kt.covariance_2x2
        assert cov.shape == (2, 2)

    def test_covariance_positive_definite(self):
        kt = KalmanTracker(0.0, 0.0)
        for _ in range(5):
            kt.predict(0.1)
            kt.update(np.array([0.1, 0.0]))
        eigvals = np.linalg.eigvalsh(kt.covariance_2x2)
        assert np.all(eigvals > 0)

    def test_joseph_form_stays_symmetric(self):
        """Covariance should remain symmetric after many updates."""
        kt = KalmanTracker(0.0, 0.0)
        for i in range(50):
            kt.predict(0.1)
            kt.update(np.array([i * 0.01, 0.0]))
        P = kt._P
        assert np.allclose(P, P.T, atol=1e-10)


# ── TrackerManager ────────────────────────────────────────────────────────────

class TestTrackerManager:

    def _det(self, x, y):
        return Detection(x=x, y=y, size_m2=0.1, range_m=math.hypot(x, y))

    def test_empty_detections_no_tracks(self):
        tm = TrackerManager()
        confirmed = tm.update([], 0.1)
        assert confirmed == []

    def test_track_created_on_detection(self):
        tm = TrackerManager(confirm_frames=1)
        confirmed = tm.update([self._det(1.0, 0.0)], 0.1)
        assert len(confirmed) == 1

    def test_track_tentative_before_confirm(self):
        tm = TrackerManager(confirm_frames=3)
        tm.update([self._det(1.0, 0.0)], 0.1)
        # Only 1 hit — should still be tentative
        assert tm.tentative_count == 1

    def test_track_confirmed_after_N_hits(self):
        tm = TrackerManager(confirm_frames=3, assoc_dist_m=2.0)
        for _ in range(4):
            confirmed = tm.update([self._det(1.0, 0.0)], 0.1)
        assert len(confirmed) == 1

    def test_track_deleted_after_max_missed(self):
        tm = TrackerManager(confirm_frames=1, max_missed=3, assoc_dist_m=2.0)
        tm.update([self._det(1.0, 0.0)], 0.1)   # create
        for _ in range(5):
            tm.update([], 0.1)   # no detections → missed++
        assert len(tm.all_tracks) == 0

    def test_max_tracks_cap(self):
        tm = TrackerManager(max_tracks=5, confirm_frames=1)
        dets = [self._det(float(i), 0.0) for i in range(10)]
        tm.update(dets, 0.1)
        assert len(tm.all_tracks) <= 5

    def test_consistent_track_id(self):
        tm = TrackerManager(confirm_frames=3, assoc_dist_m=1.5)
        for i in range(5):
            confirmed = tm.update([self._det(1.0 + i * 0.01, 0.0)], 0.1)
        assert len(confirmed) == 1
        track_id = confirmed[0].track_id
        # One more tick — ID should be stable
        confirmed2 = tm.update([self._det(1.06, 0.0)], 0.1)
        assert confirmed2[0].track_id == track_id

    def test_two_independent_tracks(self):
        tm = TrackerManager(confirm_frames=3, assoc_dist_m=0.8)
        for _ in range(5):
            confirmed = tm.update([self._det(1.0, 0.0), self._det(5.0, 0.0)], 0.1)
        assert len(confirmed) == 2

    def test_reset_clears_all(self):
        tm = TrackerManager(confirm_frames=1)
        tm.update([self._det(1.0, 0.0)], 0.1)
        tm.reset()
        assert len(tm.all_tracks) == 0

    def test_far_detection_not_assigned(self):
        tm = TrackerManager(confirm_frames=1, assoc_dist_m=0.5)
        tm.update([self._det(1.0, 0.0)], 0.1)   # create track at (1,0)
        # Detection 3 m away → new track, not update
        tm.update([self._det(4.0, 0.0)], 0.1)
        assert len(tm.all_tracks) == 2


# ── ObjectDetector ────────────────────────────────────────────────────────────

class TestObjectDetector:

    def _empty_scan(self, n=360, rmax=8.0) -> tuple:
        """All readings at max range (static background)."""
        ranges = np.full(n, rmax - 0.1, dtype=np.float32)
        return ranges, -math.pi, 2 * math.pi / n, rmax

    def _scan_with_blob(self, blob_r=2.0, blob_theta=0.0, n=360, rmax=8.0) -> tuple:
        """Background scan + a short-range cluster at blob_theta."""
        ranges = np.full(n, rmax - 0.1, dtype=np.float32)
        angle_inc = 2 * math.pi / n
        angle_min = -math.pi
        # Put a cluster of ~10 beams at blob_r
        center_idx = int((blob_theta - angle_min) / angle_inc) % n
        for di in range(-5, 6):
            idx = (center_idx + di) % n
            ranges[idx] = blob_r
        return ranges, angle_min, angle_inc, rmax

    def test_empty_scan_no_detections_after_warmup(self):
        od = ObjectDetector()
        r, a_min, a_inc, rmax = self._empty_scan()
        od.update(r, a_min, a_inc, rmax)   # init background
        for _ in range(3):
            dets = od.update(r, a_min, a_inc, rmax)
        assert len(dets) == 0

    def test_moving_blob_detected(self):
        od = ObjectDetector()
        bg_r, a_min, a_inc, rmax = self._empty_scan()
        od.update(bg_r, a_min, a_inc, rmax)   # seed background
        for _ in range(5):
            od.update(bg_r, a_min, a_inc, rmax)
        # Now inject a foreground blob
        fg_r, _, _, _ = self._scan_with_blob(blob_r=2.0, blob_theta=0.0)
        dets = od.update(fg_r, a_min, a_inc, rmax)
        assert len(dets) >= 1

    def test_detection_centroid_approximate(self):
        od = ObjectDetector()
        bg_r, a_min, a_inc, rmax = self._empty_scan()
        for _ in range(8):
            od.update(bg_r, a_min, a_inc, rmax)
        fg_r, _, _, _ = self._scan_with_blob(blob_r=3.0, blob_theta=0.0)
        dets = od.update(fg_r, a_min, a_inc, rmax)
        assert len(dets) >= 1
        # Blob is at ~3 m along x-axis (theta=0)
        cx = dets[0].x
        cy = dets[0].y
        assert abs(cx - 3.0) < 0.5
        assert abs(cy) < 0.5

    def test_reset_clears_background(self):
        od = ObjectDetector()
        bg_r, a_min, a_inc, rmax = self._empty_scan()
        for _ in range(5):
            od.update(bg_r, a_min, a_inc, rmax)
        od.reset()
        assert not od._initialized

    def test_no_inf_nan_ranges(self):
        od = ObjectDetector()
        r = np.array([np.inf, np.nan, 5.0, -1.0, 0.0] * 72, dtype=np.float32)
        a_min = -math.pi
        a_inc = 2 * math.pi / len(r)
        od.update(r, a_min, a_inc, 8.0)   # should not raise


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