"""
test_tracker.py — Unit tests for SimplePersonTracker.

Run with: pytest test/test_tracker.py -v
No ROS2 runtime required.
"""

import time
import pytest
from saltybot_perception.tracker import SimplePersonTracker, PersonTrack, _iou


# ── IoU helper ────────────────────────────────────────────────────────────────

class TestIoU:
    def test_identical_boxes(self):
        assert _iou((0, 0, 10, 10), (0, 0, 10, 10)) == pytest.approx(1.0)

    def test_no_overlap(self):
        assert _iou((0, 0, 5, 5), (10, 10, 15, 15)) == pytest.approx(0.0)

    def test_partial_overlap(self):
        iou = _iou((0, 0, 10, 10), (5, 5, 15, 15))
        # Intersection 5×5=25, each area=100, union=175
        assert iou == pytest.approx(25 / 175)

    def test_contained_box(self):
        # Inner box fully inside outer box
        outer = (0, 0, 10, 10)
        inner = (2, 2, 8, 8)
        # intersection = 6×6=36, area_outer=100, area_inner=36, union=100
        assert _iou(outer, inner) == pytest.approx(36 / 100)

    def test_touching_edges(self):
        # Boxes touch at a single edge — no area overlap
        assert _iou((0, 0, 5, 5), (5, 0, 10, 5)) == pytest.approx(0.0)


# ── Tracker ───────────────────────────────────────────────────────────────────

class TestSimplePersonTracker:

    def _make_det(self, x1=10, y1=10, x2=60, y2=160, depth=2.0, conf=0.85):
        return ((x1, y1, x2, y2), depth, conf)

    # ── Basic update ──────────────────────────────────────────────────────────

    def test_no_detections_returns_none(self):
        t = SimplePersonTracker()
        assert t.update([]) is None

    def test_single_detection_creates_track(self):
        t = SimplePersonTracker()
        track = t.update([self._make_det()])
        assert track is not None
        assert track.track_id == 1
        assert track.depth == pytest.approx(2.0)

    def test_track_id_increments(self):
        t = SimplePersonTracker(hold_duration=0.0)
        t.update([self._make_det()])
        t.update([])                            # lose track immediately
        track2 = t.update([self._make_det(x1=100, y1=100, x2=150, y2=250)])
        assert track2.track_id == 2

    # ── Closest-first selection ───────────────────────────────────────────────

    def test_picks_closest_person(self):
        t = SimplePersonTracker()
        dets = [
            ((10, 10, 60, 160), 4.0, 0.8),    # far
            ((70, 10, 120, 160), 1.5, 0.9),    # closest
            ((130, 10, 180, 160), 3.0, 0.75),  # mid
        ]
        track = t.update(dets)
        assert track.depth == pytest.approx(1.5)

    # ── Depth filtering ───────────────────────────────────────────────────────

    def test_rejects_beyond_max_depth(self):
        t = SimplePersonTracker(max_depth=5.0)
        assert t.update([self._make_det(depth=6.0)]) is None

    def test_rejects_below_min_depth(self):
        t = SimplePersonTracker(min_depth=0.3)
        assert t.update([self._make_det(depth=0.1)]) is None

    def test_accepts_within_depth_range(self):
        t = SimplePersonTracker(min_depth=0.3, max_depth=5.0)
        track = t.update([self._make_det(depth=2.5)])
        assert track is not None

    # ── Re-association ────────────────────────────────────────────────────────

    def test_iou_reassociation_keeps_same_id(self):
        t = SimplePersonTracker(iou_threshold=0.2)
        # Frame 1
        track1 = t.update([self._make_det(x1=10, y1=10, x2=60, y2=160)])
        id1 = track1.track_id
        # Frame 2 — slightly shifted (good IoU)
        track2 = t.update([self._make_det(x1=12, y1=12, x2=62, y2=162)])
        assert track2.track_id == id1

    def test_poor_iou_loses_track_after_hold(self):
        t = SimplePersonTracker(hold_duration=0.0, iou_threshold=0.5)
        # Frame 1 — track person at left
        t.update([self._make_det(x1=0, y1=0, x2=50, y2=150)])
        # Frame 2 — completely different position, bad IoU
        # hold_duration=0, so old track expires, new track started
        track = t.update([self._make_det(x1=500, y1=0, x2=550, y2=150)])
        assert track.track_id == 2   # new track

    # ── Hold duration ─────────────────────────────────────────────────────────

    def test_holds_last_known_within_duration(self):
        t = SimplePersonTracker(hold_duration=10.0)
        track = t.update([self._make_det()])
        track_id = track.track_id
        # No detections — should hold
        held = t.update([])
        assert held is not None
        assert held.track_id == track_id
        assert held.age >= 0

    def test_releases_track_after_hold_duration(self):
        t = SimplePersonTracker(hold_duration=0.0)
        t.update([self._make_det()])
        # Immediately lose
        result = t.update([])
        assert result is None

    # ── Reset ────────────────────────────────────────────────────────────────

    def test_reset_clears_track(self):
        t = SimplePersonTracker()
        t.update([self._make_det()])
        t.reset()
        assert t.active is False
        assert t.update([]) is None

    # ── PersonTrack properties ────────────────────────────────────────────────

    def test_track_center(self):
        track = PersonTrack((10, 20, 50, 100), 2.0, 0.9, 1)
        cx, cy = track.center
        assert cx == pytest.approx(30.0)
        assert cy == pytest.approx(60.0)

    def test_track_area(self):
        track = PersonTrack((10, 20, 50, 100), 2.0, 0.9, 1)
        assert track.area == pytest.approx(40 * 80)

    def test_track_age_increases(self):
        track = PersonTrack((0, 0, 50, 100), 2.0, 0.9, 1)
        time.sleep(0.05)
        assert track.age >= 0.04
