"""mesh_comms_node.py — Robot-to-robot social mesh communication.
Issue #171

Topics:
  Publish:   /social/mesh/announce   (MeshPeer, 1 Hz)  — presence heartbeat
             /social/mesh/handoff    (MeshHandoff)     — person context on LEAVING
             /social/mesh/peers      (String / JSON)   — peer list for UI
             /social/mesh/greeted    (String / JSON)   — mesh-wide greeted names
  Subscribe: /social/mesh/announce   (MeshPeer)        — remote peer announcements
             /social/mesh/handoff    (MeshHandoff)     — incoming handoffs
             /social/person_states   (PersonStateArray)
             /social/orchestrator/state (String / JSON)

Parameters:
  robot_id          (str, $ROBOT_ID or "saltybot_1") — unique robot identifier
  namespace         (str, $ROS_NAMESPACE or "")      — ROS2 namespace
  announce_rate_hz  (float, 1.0)   — heartbeat rate
  peer_timeout_s    (float, 5.0)   — drop peer after N seconds without announce
  handoff_distance_m (float, 3.0)  — max distance to trigger handoff (unused guard)
  greeted_memory_s  (float, 300.0) — forget a greeted name after N seconds
"""

from __future__ import annotations

import json
import os
import threading
import time
from typing import Dict, Set

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
from std_msgs.msg import String

from saltybot_social_msgs.msg import PersonStateArray, MeshPeer, MeshHandoff

_LEAVING  = 4  # PersonState.STATE_LEAVING
_ENGAGED  = 2  # PersonState.STATE_ENGAGED
_TALKING  = 3  # PersonState.STATE_TALKING


class MeshCommsNode(Node):
    """Robot mesh comms — peer announce, person handoff, greeting deduplication."""

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

        self.declare_parameter("robot_id", os.environ.get("ROBOT_ID", "saltybot_1"))
        self.declare_parameter("namespace", os.environ.get("ROS_NAMESPACE", ""))
        self.declare_parameter("announce_rate_hz", 1.0)
        self.declare_parameter("peer_timeout_s", 5.0)
        self.declare_parameter("handoff_distance_m", 3.0)
        self.declare_parameter("greeted_memory_s", 300.0)

        self._robot_id     = self.get_parameter("robot_id").value
        self._ns           = self.get_parameter("namespace").value
        self._peer_timeout = self.get_parameter("peer_timeout_s").value
        self._handoff_dist = self.get_parameter("handoff_distance_m").value
        self._greeted_mem  = self.get_parameter("greeted_memory_s").value

        qos = QoSProfile(depth=10)
        mesh_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )

        self._announce_pub = self.create_publisher(MeshPeer,    "/social/mesh/announce", mesh_qos)
        self._handoff_pub  = self.create_publisher(MeshHandoff, "/social/mesh/handoff",  qos)
        self._peers_pub    = self.create_publisher(String,      "/social/mesh/peers",    qos)
        self._greeted_pub  = self.create_publisher(String,      "/social/mesh/greeted",  qos)

        self._announce_sub = self.create_subscription(
            MeshPeer, "/social/mesh/announce", self._on_announce, mesh_qos)
        self._handoff_sub  = self.create_subscription(
            MeshHandoff, "/social/mesh/handoff", self._on_handoff, qos)
        self._persons_sub  = self.create_subscription(
            PersonStateArray, "/social/person_states", self._on_person_states, qos)
        self._orch_sub     = self.create_subscription(
            String, "/social/orchestrator/state", self._on_orch_state, qos)

        self._lock                         = threading.Lock()
        self._peers: Dict[str, dict]       = {}   # robot_id → {last_seen, msg}
        self._person_states: dict          = {}   # person_id → PersonState
        self._social_state                 = "idle"
        self._greeted: Dict[str, float]    = {}   # person_name → timestamp
        self._handoff_sent: Set[str]       = set()  # str(person_id) already handed off

        rate = self.get_parameter("announce_rate_hz").value
        self._announce_timer = self.create_timer(1.0 / rate, self._publish_announce)
        self._cleanup_timer  = self.create_timer(2.0, self._cleanup)

        self.get_logger().info(
            f"MeshCommsNode ready (robot_id={self._robot_id}, "
            f"peer_timeout={self._peer_timeout}s, handoff_dist={self._handoff_dist}m)"
        )

    # ── Subscriber callbacks ──────────────────────────────────────────────────

    def _on_orch_state(self, msg: String) -> None:
        try:
            d = json.loads(msg.data)
            with self._lock:
                self._social_state = d.get("state", "idle")
        except Exception:
            pass

    def _on_person_states(self, msg: PersonStateArray) -> None:
        with self._lock:
            new_states = {p.person_id: p for p in msg.persons}
            for pid, ps in new_states.items():
                prev = self._person_states.get(pid)
                if (prev is not None
                        and prev.state != _LEAVING
                        and ps.state == _LEAVING
                        and str(pid) not in self._handoff_sent):
                    self._trigger_handoff(ps)
            self._person_states = new_states

    def _on_announce(self, msg: MeshPeer) -> None:
        if msg.robot_id == self._robot_id:
            return
        with self._lock:
            self._peers[msg.robot_id] = {"last_seen": time.time(), "msg": msg}
            for name in msg.greeted_person_names:
                if name and name not in self._greeted:
                    self._greeted[name] = time.time()
        self._publish_greeted()

    def _on_handoff(self, msg: MeshHandoff) -> None:
        if msg.from_robot_id == self._robot_id:
            return
        self.get_logger().info(
            f"Handoff received from [{msg.from_robot_id}]: "
            f"person='{msg.person_name or msg.person_id}' "
            f"lang={msg.language or '?'} "
            f"bearing={msg.bearing_to_target_deg:.0f}° dist={msg.distance_m:.1f}m"
        )

    # ── Publish helpers ───────────────────────────────────────────────────────

    def _trigger_handoff(self, ps) -> None:
        """Build and publish a MeshHandoff for a LEAVING person. Must hold self._lock."""
        msg = MeshHandoff()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.from_robot_id             = self._robot_id
        msg.to_robot_id               = ""
        msg.person_id                 = str(ps.person_id)
        msg.person_name               = ps.person_name
        msg.face_id                   = ps.face_id
        msg.speaker_id                = ps.speaker_id
        msg.bearing_to_target_deg     = ps.bearing_deg
        msg.distance_m                = ps.distance
        msg.engagement_score          = ps.engagement_score
        msg.conversation_summary      = ""
        msg.last_utterance            = ""
        msg.last_response             = ""
        msg.language                  = ""
        self._handoff_sent.add(str(ps.person_id))
        self._handoff_pub.publish(msg)
        self.get_logger().info(
            f"Handoff → '{ps.person_name or ps.person_id}' "
            f"bearing={ps.bearing_deg:.0f}° dist={ps.distance:.1f}m"
        )

    def _publish_announce(self) -> None:
        with self._lock:
            active_ids = [
                pid for pid, ps in self._person_states.items()
                if ps.state in (_ENGAGED, _TALKING)
            ]
            greeted_names = [
                name for name, t in self._greeted.items()
                if time.time() - t < self._greeted_mem
            ]
            state = self._social_state
        msg = MeshPeer()
        msg.header.stamp          = self.get_clock().now().to_msg()
        msg.robot_id              = self._robot_id
        msg.ros_namespace         = self._ns
        msg.social_state          = state
        msg.active_person_ids     = active_ids
        msg.greeted_person_names  = greeted_names
        msg.battery_pct           = -1.0
        self._announce_pub.publish(msg)
        self._publish_peers_status()

    def _publish_peers_status(self) -> None:
        with self._lock:
            entries = [
                {
                    "robot_id":       rid,
                    "social_state":   d["msg"].social_state,
                    "active_persons": len(d["msg"].active_person_ids),
                    "age_s":          round(time.time() - d["last_seen"], 1),
                }
                for rid, d in self._peers.items()
            ]
        msg = String()
        msg.data = json.dumps({"peers": entries, "ts": time.time()})
        self._peers_pub.publish(msg)

    def _publish_greeted(self) -> None:
        with self._lock:
            names = [n for n, t in self._greeted.items()
                     if time.time() - t < self._greeted_mem]
        msg = String()
        msg.data = json.dumps({"greeted": names, "ts": time.time()})
        self._greeted_pub.publish(msg)

    # ── Cleanup ───────────────────────────────────────────────────────────────

    def _cleanup(self) -> None:
        now = time.time()
        with self._lock:
            dead = [rid for rid, d in self._peers.items()
                    if now - d["last_seen"] > self._peer_timeout]
            for rid in dead:
                self.get_logger().info(f"Peer lost: {rid}")
                del self._peers[rid]
            self._greeted = {n: t for n, t in self._greeted.items()
                             if now - t < self._greeted_mem}
            active_pids = {str(p) for p in self._person_states}
            self._handoff_sent &= active_pids


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