"""Tests for multi-person tracker (Issue #423)."""

import numpy as np
from saltybot_multi_person_tracker.multi_person_tracker import (
    MultiPersonTracker,
    TrackedPerson,
)


def test_create_new_tracks():
    """Test creating new tracks for detections."""
    tracker = MultiPersonTracker(max_people=10)

    detections = [
        {"bearing_rad": 0.1, "distance_m": 1.5, "confidence": 0.9},
        {"bearing_rad": -0.2, "distance_m": 2.0, "confidence": 0.85},
    ]

    tracked, target_id = tracker.update(detections)

    assert len(tracked) == 2
    assert tracked[0].track_id == 0
    assert tracked[1].track_id == 1


def test_match_existing_tracks():
    """Test matching detections to existing tracks."""
    tracker = MultiPersonTracker(max_people=10)

    # Create initial tracks
    detections1 = [
        {"bearing_rad": 0.1, "distance_m": 1.5, "confidence": 0.9},
        {"bearing_rad": -0.2, "distance_m": 2.0, "confidence": 0.85},
    ]
    tracked1, _ = tracker.update(detections1)

    # Update with similar positions
    detections2 = [
        {"bearing_rad": 0.12, "distance_m": 1.48, "confidence": 0.88},
        {"bearing_rad": -0.22, "distance_m": 2.05, "confidence": 0.84},
    ]
    tracked2, _ = tracker.update(detections2)

    # Should have same IDs (matched)
    assert len(tracked2) == 2
    ids_after = {p.track_id for p in tracked2}
    assert ids_after == {0, 1}


def test_max_people_limit():
    """Test that tracker respects max_people limit."""
    tracker = MultiPersonTracker(max_people=3)

    detections = [
        {"bearing_rad": 0.1 * i, "distance_m": 1.5 + i * 0.2, "confidence": 0.9}
        for i in range(5)
    ]

    tracked, _ = tracker.update(detections)
    assert len(tracked) <= 3


def test_wake_word_speaker_priority():
    """Test that wake-word speaker gets highest priority."""
    tracker = MultiPersonTracker(max_people=10)

    detections = [
        {"bearing_rad": 0.1, "distance_m": 5.0, "confidence": 0.9},  # Far
        {"bearing_rad": -0.2, "distance_m": 1.0, "confidence": 0.85},  # Closest
        {"bearing_rad": 0.3, "distance_m": 2.0, "confidence": 0.8},    # Farthest
    ]

    tracked, _ = tracker.update(detections)

    # Without speaker: should pick closest
    assert tracker.active_target_id == 1  # Closest person

    # With speaker at ID 0: should pick speaker
    tracked, target = tracker.update(detections, speaking_person_id=0)
    assert target == 0


def test_group_centroid():
    """Test group centroid calculation."""
    tracker = MultiPersonTracker(max_people=10)

    detections = [
        {"bearing_rad": 0.0, "distance_m": 1.0, "confidence": 0.9},
        {"bearing_rad": 0.2, "distance_m": 2.0, "confidence": 0.85},
        {"bearing_rad": -0.2, "distance_m": 1.5, "confidence": 0.8},
    ]

    tracked, _ = tracker.update(detections)

    centroid = tracker.get_group_centroid()
    assert centroid is not None
    mean_bearing, mean_distance = centroid
    # Mean bearing should be close to 0
    assert abs(mean_bearing) < 0.1
    # Mean distance should be between 1.0 and 2.0
    assert 1.0 < mean_distance < 2.0


def test_empty_tracker():
    """Test tracker with no detections."""
    tracker = MultiPersonTracker()

    tracked, target = tracker.update([])

    assert len(tracked) == 0
    assert target is None
    assert tracker.searching is True


if __name__ == "__main__":
    test_create_new_tracks()
    test_match_existing_tracks()
    test_max_people_limit()
    test_wake_word_speaker_priority()
    test_group_centroid()
    test_empty_tracker()
    print("All tests passed!")
