"""multi_person_tracker_node.py — Multi-person tracking node for Issue #423.

Subscribes to person detections with re-ID embeddings and publishes:
  /saltybot/tracked_people (PersonArray) — all tracked people + active target
  /saltybot/follow_target (Person) — the current follow target

Implements:
- Up to 10 tracked people
- Target priority: wake-word speaker > closest > largest bbox
- Occlusion handoff (3s grace)
- Re-ID via face embedding + HSV
- Group detection
- Lost target: SEARCHING state
"""

from __future__ import annotations

import time
import numpy as np
from typing import Dict, List, Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy
from rclpy.duration import Duration

from std_msgs.msg import String
from geometry_msgs.msg import Twist
from saltybot_social_msgs.msg import Person, PersonArray

from .multi_person_tracker import MultiPersonTracker, TrackedPerson


class MultiPersonTrackerNode(Node):
    """ROS2 node for multi-person tracking."""

    def __init__(self) -> None:
        super().__init__("multi_person_tracker_node")

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter("max_people", 10)
        self.declare_parameter("occlusion_grace_s", 3.0)
        self.declare_parameter("embedding_threshold", 0.75)
        self.declare_parameter("hsv_threshold", 0.60)
        self.declare_parameter("publish_hz", 15.0)
        self.declare_parameter("enable_group_follow", True)
        self.declare_parameter("announce_state", True)

        max_people = self.get_parameter("max_people").value
        occlusion_grace = self.get_parameter("occlusion_grace_s").value
        embed_threshold = self.get_parameter("embedding_threshold").value
        hsv_threshold = self.get_parameter("hsv_threshold").value
        publish_hz = self.get_parameter("publish_hz").value
        self._enable_group = self.get_parameter("enable_group_follow").value
        self._announce = self.get_parameter("announce_state").value

        # ── Tracker instance ────────────────────────────────────────────────
        self._tracker = MultiPersonTracker(
            max_people=max_people,
            occlusion_grace_s=occlusion_grace,
            embedding_similarity_threshold=embed_threshold,
            hsv_similarity_threshold=hsv_threshold,
        )

        # ── Reliable QoS ────────────────────────────────────────────────────
        qos = QoSProfile(depth=5)
        qos.reliability = QoSReliabilityPolicy.RELIABLE

        self._tracked_pub = self.create_publisher(
            PersonArray,
            "/saltybot/tracked_people",
            qos,
        )

        self._target_pub = self.create_publisher(
            Person,
            "/saltybot/follow_target",
            qos,
        )

        self._state_pub = self.create_publisher(
            String,
            "/saltybot/tracker_state",
            qos,
        )

        self._cmd_vel_pub = self.create_publisher(
            Twist,
            "/saltybot/cmd_vel",
            qos,
        )

        # ── Placeholder: in real implementation, subscribe to detection topics
        # For now, we'll use a timer to demonstrate the node structure
        self._update_timer = self.create_timer(1.0 / publish_hz, self._update_tracker)

        self._last_state = "IDLE"
        self.get_logger().info(
            f"multi_person_tracker_node ready (max_people={max_people}, "
            f"occlusion_grace={occlusion_grace}s, publish_hz={publish_hz} Hz)"
        )

    def _update_tracker(self) -> None:
        """Update tracker and publish results."""
        # In real implementation, this would be triggered by detection callbacks
        # For now, we'll use the timer and publish empty/demo data

        # Get current tracked people and active target
        tracked_people = list(self._tracker.tracked_people.values())
        active_target_id = self._tracker.active_target_id

        # Build PersonArray message
        person_array_msg = PersonArray()
        person_array_msg.header.stamp = self.get_clock().now().to_msg()
        person_array_msg.header.frame_id = "base_link"
        person_array_msg.active_id = active_target_id if active_target_id is not None else -1

        for tracked in tracked_people:
            person_msg = Person()
            person_msg.header = person_array_msg.header
            person_msg.track_id = tracked.track_id
            person_msg.bearing_rad = tracked.bearing_rad
            person_msg.distance_m = tracked.distance_m
            person_msg.confidence = tracked.confidence
            person_msg.is_speaking = tracked.is_speaking
            person_msg.source = tracked.source
            person_array_msg.persons.append(person_msg)

        self._tracked_pub.publish(person_array_msg)

        # Publish active target
        if active_target_id is not None and active_target_id in self._tracker.tracked_people:
            target = self._tracker.tracked_people[active_target_id]
            target_msg = Person()
            target_msg.header = person_array_msg.header
            target_msg.track_id = target.track_id
            target_msg.bearing_rad = target.bearing_rad
            target_msg.distance_m = target.distance_m
            target_msg.confidence = target.confidence
            target_msg.is_speaking = target.is_speaking
            target_msg.source = target.source
            self._target_pub.publish(target_msg)

        # Publish tracker state
        state = "SEARCHING" if self._tracker.searching else "TRACKING"
        if state != self._last_state and self._announce:
            self.get_logger().info(f"State: {state}")
            self._last_state = state
            state_msg = String()
            state_msg.data = state
            self._state_pub.publish(state_msg)

            # Handle lost target
            if state == "SEARCHING":
                self._handle_lost_target()

    def _handle_lost_target(self) -> None:
        """Execute behavior when target is lost: stop + rotate."""
        # Publish stop command
        stop_twist = Twist()
        self._cmd_vel_pub.publish(stop_twist)

        # Slow rotating search
        search_twist = Twist()
        search_twist.angular.z = 0.5  # rad/s
        self._cmd_vel_pub.publish(search_twist)

        if self._announce:
            self.get_logger().warn("Lost target, searching...")


def main(args=None) -> None:
    rclpy.init(args=args)
    node = MultiPersonTrackerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
