"""personal_space_node.py — Personal space respector.
Issue #310

Monitors the distance to detected faces.  When the closest person
enters within ``personal_space_m`` metres (default 0.8 m) the robot
slowly backs up (``Twist.linear.x = -backup_speed``) and latches
/saltybot/too_close to True.  Once the person retreats beyond
``personal_space_m + hysteresis_m`` the robot stops and the latch clears.

Distance is read from /social/person_states (PersonStateArray, face_id →
distance).  When a face has no matching PersonState entry the distance is
treated as ``unknown_distance_m`` (default: very large → safe, no backup).

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

Publications
────────────
  /cmd_vel               geometry_msgs/Twist  — linear.x only; angular.z = 0
  /saltybot/too_close    std_msgs/Bool        — True while backing up

State machine
─────────────
  CLEAR     — no face within personal_space_m; publish zero Twist + False
  TOO_CLOSE — face within personal_space_m; publish backup Twist + True
              clears only when distance > personal_space_m + hysteresis_m

Parameters
──────────
  personal_space_m    (float, 0.8)   minimum comfortable distance (m)
  backup_speed        (float, 0.1)   retreat linear speed (m/s, applied as -x)
  hysteresis_m        (float, 0.1)   hysteresis band above threshold (m)
  unknown_distance_m  (float, 99.0)  distance assumed for faces without state
  lost_timeout_s      (float, 1.5)   face freshness window (s)
  control_rate        (float, 10.0)  publish rate (Hz)
  faces_topic         (str, "/social/faces/detected")
  states_topic        (str, "/social/person_states")
"""

from __future__ import annotations

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 Bool
from geometry_msgs.msg import Twist

try:
    from saltybot_social_msgs.msg import FaceDetectionArray, PersonStateArray
    _MSGS = True
except ImportError:
    _MSGS = False


# ── Pure helpers ───────────────────────────────────────────────────────────────

def closest_face_distance(face_ids, distance_cache: Dict[int, float],
                          unknown_distance_m: float) -> float:
    """Return the minimum distance across all visible face_ids.

    Falls back to *unknown_distance_m* for any face_id not in the cache.
    Returns *unknown_distance_m* when *face_ids* is empty.
    """
    if not face_ids:
        return unknown_distance_m
    return min(distance_cache.get(fid, unknown_distance_m) for fid in face_ids)


def should_backup(distance: float, personal_space_m: float) -> bool:
    return distance <= personal_space_m


def should_clear(distance: float, personal_space_m: float,
                 hysteresis_m: float) -> bool:
    return distance > personal_space_m + hysteresis_m


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

class PersonalSpaceNode(Node):
    """Backs the robot away when a person enters personal space."""

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

        self.declare_parameter("personal_space_m",   0.8)
        self.declare_parameter("backup_speed",        0.1)
        self.declare_parameter("hysteresis_m",        0.1)
        self.declare_parameter("unknown_distance_m",  99.0)
        self.declare_parameter("lost_timeout_s",      1.5)
        self.declare_parameter("control_rate",        10.0)
        self.declare_parameter("faces_topic",         "/social/faces/detected")
        self.declare_parameter("states_topic",        "/social/person_states")

        self._space        = self.get_parameter("personal_space_m").value
        self._backup_speed = self.get_parameter("backup_speed").value
        self._hysteresis   = self.get_parameter("hysteresis_m").value
        self._unknown_dist = self.get_parameter("unknown_distance_m").value
        self._lost_t       = self.get_parameter("lost_timeout_s").value
        rate               = self.get_parameter("control_rate").value
        faces_topic        = self.get_parameter("faces_topic").value
        states_topic       = self.get_parameter("states_topic").value

        # State
        self._too_close: bool = False
        self._distance_cache: Dict[int, float] = {}   # face_id → distance (m)
        self._active_face_ids: list = []               # face_ids in last msg
        self._last_face_t: float = 0.0
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._cmd_pub       = self.create_publisher(Twist, "/cmd_vel",             qos)
        self._too_close_pub = self.create_publisher(Bool,  "/saltybot/too_close",  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 passive"
            )

        self._timer = self.create_timer(1.0 / rate, self._control_cb)

        self.get_logger().info(
            f"PersonalSpaceNode ready "
            f"(space={self._space}m, backup={self._backup_speed}m/s, "
            f"hysteresis={self._hysteresis}m)"
        )

    # ── Subscription callbacks ─────────────────────────────────────────────

    def _on_person_states(self, msg) -> None:
        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) -> None:
        face_ids = [int(f.face_id) for f in msg.faces]
        with self._lock:
            self._active_face_ids = face_ids
            if face_ids:
                self._last_face_t = time.monotonic()

    # ── Control loop ───────────────────────────────────────────────────────

    def _control_cb(self) -> None:
        now = time.monotonic()
        with self._lock:
            face_ids    = list(self._active_face_ids)
            last_face_t = self._last_face_t
            cache       = dict(self._distance_cache)

        face_fresh = last_face_t > 0.0 and (now - last_face_t) < self._lost_t

        if not face_fresh:
            # No fresh face data → clear state, stop
            if self._too_close:
                self._too_close = False
                self.get_logger().info("PersonalSpace: face lost — clearing")
            self._publish(backup=False)
            return

        dist = closest_face_distance(face_ids, cache, self._unknown_dist)

        if not self._too_close and should_backup(dist, self._space):
            self._too_close = True
            self.get_logger().warn(
                f"PersonalSpace: TOO CLOSE ({dist:.2f}m <= {self._space}m) — backing up"
            )
        elif self._too_close and should_clear(dist, self._space, self._hysteresis):
            self._too_close = False
            self.get_logger().info(
                f"PersonalSpace: CLEAR ({dist:.2f}m > {self._space + self._hysteresis:.2f}m)"
            )

        self._publish(backup=self._too_close)

    def _publish(self, backup: bool) -> None:
        twist = Twist()
        if backup:
            twist.linear.x = -self._backup_speed

        bool_msg = Bool()
        bool_msg.data = backup

        self._cmd_pub.publish(twist)
        self._too_close_pub.publish(bool_msg)


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