"""
test_attention.py — Unit tests for attention_node helpers.

No ROS2 / serial / GPU dependencies — runs with plain pytest.
"""

import math
import pytest

from saltybot_social.attention_node import (
    clamp,
    compute_attention_cmd,
    select_target,
)


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

class FakePerson:
    def __init__(self, track_id, bearing_rad, confidence=0.9, is_speaking=False):
        self.track_id    = track_id
        self.bearing_rad = bearing_rad
        self.confidence  = confidence
        self.is_speaking = is_speaking


# ── clamp ─────────────────────────────────────────────────────────────────────

class TestClamp:
    def test_within_range(self):
        assert clamp(0.5, 0.0, 1.0) == 0.5

    def test_below_lo(self):
        assert clamp(-0.5, 0.0, 1.0) == 0.0

    def test_above_hi(self):
        assert clamp(1.5, 0.0, 1.0) == 1.0

    def test_at_boundary(self):
        assert clamp(0.0, 0.0, 1.0) == 0.0
        assert clamp(1.0, 0.0, 1.0) == 1.0


# ── select_target ─────────────────────────────────────────────────────────────

class TestSelectTarget:

    def test_empty_returns_none(self):
        assert select_target([], active_id=-1) is None

    def test_picks_active_id(self):
        persons = [FakePerson(1, 0.1, confidence=0.6),
                   FakePerson(2, 0.5, confidence=0.9)]
        result = select_target(persons, active_id=1)
        assert result.track_id == 1

    def test_falls_back_to_highest_confidence(self):
        persons = [FakePerson(1, 0.1, confidence=0.6),
                   FakePerson(2, 0.5, confidence=0.9),
                   FakePerson(3, 0.2, confidence=0.4)]
        result = select_target(persons, active_id=-1)
        assert result.track_id == 2

    def test_single_person_no_active(self):
        persons = [FakePerson(5, 0.3)]
        result = select_target(persons, active_id=-1)
        assert result.track_id == 5

    def test_active_id_not_in_list_falls_back(self):
        persons = [FakePerson(1, 0.0, confidence=0.5),
                   FakePerson(2, 0.2, confidence=0.95)]
        result = select_target(persons, active_id=99)
        assert result.track_id == 2


# ── compute_attention_cmd ─────────────────────────────────────────────────────

class TestComputeAttentionCmd:

    def test_inside_dead_zone_zero(self):
        cmd = compute_attention_cmd(
            bearing_rad=0.05,
            dead_zone_rad=0.15,
            kp_angular=1.0,
            max_angular_vel=0.8,
        )
        assert cmd == 0.0

    def test_exactly_at_dead_zone_boundary_zero(self):
        cmd = compute_attention_cmd(
            bearing_rad=0.15,
            dead_zone_rad=0.15,
            kp_angular=1.0,
            max_angular_vel=0.8,
        )
        assert cmd == 0.0

    def test_positive_bearing_gives_positive_angular_z(self):
        cmd = compute_attention_cmd(
            bearing_rad=0.4,
            dead_zone_rad=0.15,
            kp_angular=1.0,
            max_angular_vel=0.8,
        )
        assert cmd > 0.0

    def test_negative_bearing_gives_negative_angular_z(self):
        cmd = compute_attention_cmd(
            bearing_rad=-0.4,
            dead_zone_rad=0.15,
            kp_angular=1.0,
            max_angular_vel=0.8,
        )
        assert cmd < 0.0

    def test_clamps_to_max_angular_vel(self):
        cmd = compute_attention_cmd(
            bearing_rad=math.pi,  # 180° off
            dead_zone_rad=0.15,
            kp_angular=2.0,
            max_angular_vel=0.8,
        )
        assert abs(cmd) <= 0.8

    def test_proportional_scaling(self):
        """Double bearing → double cmd (within linear region)."""
        kp = 1.0
        dz = 0.05
        cmd1 = compute_attention_cmd(0.3, dz, kp, 10.0)
        cmd2 = compute_attention_cmd(0.6, dz, kp, 10.0)
        assert abs(cmd2 - 2.0 * cmd1) < 0.01

    def test_zero_bearing_zero_cmd(self):
        cmd = compute_attention_cmd(
            bearing_rad=0.0,
            dead_zone_rad=0.1,
            kp_angular=1.0,
            max_angular_vel=0.8,
        )
        assert cmd == 0.0
