"""greeting_trigger_node.py -- Proximity-based greeting trigger.
Issue #270

Monitors face detections and person states.  When a new face_id is seen
within ``proximity_m`` metres (default 2 m) and has not been greeted within
``cooldown_s`` seconds, publishes a JSON greeting trigger on
/saltybot/greeting_trigger.

Distance is looked up from the /social/person_states topic which carries a
face_id → distance mapping.  When no state is available for a face the node
applies a configurable default distance so it can still fire on face-only
pipelines.

Subscriptions:
  /social/faces/detected    saltybot_social_msgs/FaceDetectionArray
  /social/person_states     saltybot_social_msgs/PersonStateArray

Publication:
  /saltybot/greeting_trigger  std_msgs/String  (JSON)
    {"face_id": <int>, "person_name": <str>, "distance_m": <float>,
     "ts": <float unix epoch>}

Parameters:
  proximity_m       (float, 2.0)     -- trigger when distance <= this
  cooldown_s        (float, 300.0)   -- suppress re-greeting same face_id
  unknown_distance  (float, 0.0)     -- distance assumed when PersonState
                                        is not yet available (0.0 → always
                                        trigger for unknown faces)
  faces_topic       (str, "/social/faces/detected")
  states_topic      (str, "/social/person_states")
"""

from __future__ import annotations

import json
import time
import threading
from typing import Dict

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import String

# Custom messages — imported at runtime so offline tests can stub them
try:
    from saltybot_social_msgs.msg import FaceDetectionArray, PersonStateArray
    _MSGS = True
except ImportError:
    _MSGS = False


class GreetingTriggerNode(Node):
    """Publishes greeting trigger when a person enters proximity."""

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

        self.declare_parameter("proximity_m",      2.0)
        self.declare_parameter("cooldown_s",       300.0)
        self.declare_parameter("unknown_distance",  0.0)
        self.declare_parameter("faces_topic",      "/social/faces/detected")
        self.declare_parameter("states_topic",     "/social/person_states")

        self._proximity    = self.get_parameter("proximity_m").value
        self._cooldown     = self.get_parameter("cooldown_s").value
        self._unknown_dist = self.get_parameter("unknown_distance").value
        faces_topic        = self.get_parameter("faces_topic").value
        states_topic       = self.get_parameter("states_topic").value

        # face_id → last known distance (m); updated from PersonStateArray
        self._distance_cache: Dict[int, float] = {}
        # face_id → unix timestamp of last greeting
        self._last_greeted: Dict[int, float] = {}
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._pub = self.create_publisher(String, "/saltybot/greeting_trigger", qos)

        if _MSGS:
            self._states_sub = self.create_subscription(
                PersonStateArray, states_topic, self._on_person_states, qos
            )
            self._faces_sub = self.create_subscription(
                FaceDetectionArray, faces_topic, self._on_faces, qos
            )
        else:
            self.get_logger().warn(
                "saltybot_social_msgs not available — node is passive (no subscriptions)"
            )

        self.get_logger().info(
            f"GreetingTriggerNode ready "
            f"(proximity={self._proximity}m, cooldown={self._cooldown}s)"
        )

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

    def _on_person_states(self, msg: "PersonStateArray") -> None:
        """Cache face_id → distance from incoming PersonState array."""
        with self._lock:
            for ps in msg.persons:
                if ps.face_id >= 0:
                    self._distance_cache[ps.face_id] = float(ps.distance)

    def _on_faces(self, msg: "FaceDetectionArray") -> None:
        """Evaluate each detected face; fire greeting if conditions met."""
        now = time.monotonic()
        with self._lock:
            for face in msg.faces:
                fid = int(face.face_id)
                dist = self._distance_cache.get(fid, self._unknown_dist)

                if dist > self._proximity:
                    continue  # too far

                last = self._last_greeted.get(fid, 0.0)
                if now - last < self._cooldown:
                    continue  # still in cooldown

                # Fire!
                self._last_greeted[fid] = now
                self._fire(fid, str(face.person_name), dist)

    def _fire(self, face_id: int, person_name: str, distance_m: float) -> None:
        payload = {
            "face_id":     face_id,
            "person_name": person_name,
            "distance_m":  round(distance_m, 3),
            "ts":          time.time(),
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._pub.publish(msg)
        self.get_logger().info(
            f"Greeting trigger: face_id={face_id} name={person_name!r} "
            f"dist={distance_m:.2f}m"
        )


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