"""
attention_node.py — Social attention controller for saltybot.

Rotates the robot to face the active speaker or most confident detected person.
Publishes rotation-only cmd_vel (linear.x = 0) so balance is not disturbed.

Subscribes
──────────
  /social/persons  (saltybot_social_msgs/PersonArray)
    Contains all tracked persons; active_id marks the current speaker.
    bearing_rad: signed bearing in base_link (+ve = left/CCW).

Publishes
─────────
  /cmd_vel  (geometry_msgs/Twist)
    angular.z = clamp(kp_angular * bearing, ±max_angular_vel)
    linear.x  = 0 always (social mode, not follow mode)

Control law
───────────
  1. Pick target: person with active_id; fall back to highest-confidence person.
  2. Dead zone: if |bearing| ≤ dead_zone_rad → angular_z = 0.
  3. Proportional: angular_z = kp_angular * bearing.
  4. Clamp to ±max_angular_vel.
  5. If no fresh person data for lost_timeout_s → publish zero and stay idle.

State machine
─────────────
  ATTENDING  — fresh target; publishing proportional rotation
  IDLE       — no target; publishing zero

Parameters
──────────
  kp_angular      (float)  1.0        proportional gain (rad/s per rad error)
  max_angular_vel (float)  0.8        hard cap (rad/s)
  dead_zone_rad   (float)  0.15       ~8.6° dead zone around robot heading
  control_rate    (float)  20.0       Hz
  lost_timeout_s  (float)  2.0        seconds before going idle
  attention_enabled (bool) True       runtime kill switch
"""

import math
import time

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

from saltybot_social_msgs.msg import PersonArray


# ── Pure helpers (used by tests) ──────────────────────────────────────────────

def clamp(v: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, v))


def select_target(persons, active_id: int):
    """
    Pick the attention target from a PersonArray's persons list.

    Priority:
      1. Person whose track_id == active_id (speaking / explicitly active).
      2. Person with highest confidence (fallback when no speaker declared).

    Returns the selected Person message, or None if persons is empty.
    """
    if not persons:
        return None

    # Prefer the declared active speaker
    for p in persons:
        if p.track_id == active_id:
            return p

    # Fall back to most confident detection
    return max(persons, key=lambda p: p.confidence)


def compute_attention_cmd(
    bearing_rad: float,
    dead_zone_rad: float,
    kp_angular: float,
    max_angular_vel: float,
) -> float:
    """
    Proportional rotation command toward bearing.

    Returns angular_z (rad/s).
    Zero inside dead zone; proportional + clamped outside.
    """
    if abs(bearing_rad) <= dead_zone_rad:
        return 0.0
    return clamp(kp_angular * bearing_rad, -max_angular_vel, max_angular_vel)


# ── ROS2 node ─────────────────────────────────────────────────────────────────

class AttentionNode(Node):

    def __init__(self):
        super().__init__("attention_node")

        self.declare_parameter("kp_angular",        1.0)
        self.declare_parameter("max_angular_vel",   0.8)
        self.declare_parameter("dead_zone_rad",     0.15)
        self.declare_parameter("control_rate",      20.0)
        self.declare_parameter("lost_timeout_s",    2.0)
        self.declare_parameter("attention_enabled", True)

        self._reload_params()

        # State
        self._last_persons_t = 0.0    # monotonic; 0 = never received
        self._persons        = []
        self._active_id      = -1
        self._state          = "idle"

        self.create_subscription(
            PersonArray, "/social/persons", self._persons_cb, 10
        )

        self._cmd_pub = self.create_publisher(Twist, "/cmd_vel", 10)

        rate = self._p["control_rate"]
        self._timer = self.create_timer(1.0 / rate, self._control_cb)

        self.get_logger().info(
            f"AttentionNode ready  kp={self._p['kp_angular']} "
            f"dead_zone={math.degrees(self._p['dead_zone_rad']):.1f}° "
            f"max_ω={self._p['max_angular_vel']} rad/s"
        )

    def _reload_params(self):
        self._p = {
            "kp_angular":        self.get_parameter("kp_angular").value,
            "max_angular_vel":   self.get_parameter("max_angular_vel").value,
            "dead_zone_rad":     self.get_parameter("dead_zone_rad").value,
            "control_rate":      self.get_parameter("control_rate").value,
            "lost_timeout_s":    self.get_parameter("lost_timeout_s").value,
            "attention_enabled": self.get_parameter("attention_enabled").value,
        }

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _persons_cb(self, msg: PersonArray):
        self._persons        = msg.persons
        self._active_id      = msg.active_id
        self._last_persons_t = time.monotonic()
        if self._state == "idle" and msg.persons:
            self._state = "attending"
            self.get_logger().info("Attention: person detected — attending")

    def _control_cb(self):
        self._reload_params()
        twist = Twist()

        if not self._p["attention_enabled"]:
            self._cmd_pub.publish(twist)
            return

        now = time.monotonic()
        fresh = (
            self._last_persons_t > 0.0
            and (now - self._last_persons_t) < self._p["lost_timeout_s"]
        )

        if not fresh:
            if self._state == "attending":
                self._state = "idle"
                self.get_logger().info("Attention: no person — idle")
            self._cmd_pub.publish(twist)
            return

        target = select_target(self._persons, self._active_id)
        if target is None:
            self._cmd_pub.publish(twist)
            return

        self._state = "attending"
        twist.angular.z = compute_attention_cmd(
            bearing_rad=target.bearing_rad,
            dead_zone_rad=self._p["dead_zone_rad"],
            kp_angular=self._p["kp_angular"],
            max_angular_vel=self._p["max_angular_vel"],
        )
        self._cmd_pub.publish(twist)


# ── Entry point ───────────────────────────────────────────────────────────────

def main(args=None):
    rclpy.init(args=args)
    node = AttentionNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()
