"""gesture_node.py — Multi-camera hand + body-pose gesture recognition (Issue #140).

Subscribes to images from up to 4 CSI cameras, runs MediaPipe Hands and Pose
on each frame, classifies gestures via gesture_classifier.py, and publishes
/social/gestures (GestureArray).

Pipeline
--------
camera/*/image_raw → cv_bridge → MediaPipe Hands + Pose
    → gesture_classifier → correlate with PersonStateArray
    → publish /social/gestures

Multi-camera
------------
Up to n_cameras (default 4) camera streams are processed.
Topic names follow the saltybot_cameras convention:
  /camera/front/image_raw, /camera/left/image_raw,
  /camera/rear/image_raw,  /camera/right/image_raw

All cameras share a single round-robin processing queue so that GPU/CPU
resources are not over-subscribed.  At process_fps=10 Hz, each camera
receives approximately process_fps / n_cameras updates per second.

Person-ID correlation
---------------------
When a gesture is detected on camera N, the node inspects the cached
PersonStateArray.  A person with matching camera_id is preferred; if only
one person is tracked, they receive the gesture unconditionally.

ROS2 topics
-----------
Subscribe:
  /camera/front/image_raw   (sensor_msgs/Image)
  /camera/left/image_raw    (sensor_msgs/Image)
  /camera/rear/image_raw    (sensor_msgs/Image)
  /camera/right/image_raw   (sensor_msgs/Image)
  /social/persons           (saltybot_social_msgs/PersonStateArray)

Publish:
  /social/gestures          (saltybot_social_msgs/GestureArray)

Parameters
----------
n_cameras               int     4       Number of active camera streams
process_fps             float   10.0    Target processing rate (frames/sec)
min_confidence          float   0.60    Discard detections below this threshold
model_complexity        int     0       MediaPipe model (0=lite, 1=full, 2=heavy)
max_hands               int     2       Max hands per frame (MediaPipe param)
pose_enabled            bool    true    Enable MediaPipe Pose body-pose detection
hands_enabled           bool    true    Enable MediaPipe Hands
enabled_gestures        str     ""      Comma-separated allowlist; empty = all
camera_names            str     "front,left,rear,right"  Camera name list
"""

from __future__ import annotations

import threading
import time
from typing import Dict, List, Optional, Set

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

from sensor_msgs.msg import Image
from saltybot_social_msgs.msg import (
    GestureArray, Gesture, PersonStateArray,
)

from .gesture_classifier import (
    Landmark, ClassifiedGesture,
    classify_hand, classify_pose,
    WaveDetector,
)

# Optional imports — may not be present in CI / test environments
try:
    import cv2
    from cv_bridge import CvBridge
    _HAS_CV = True
except ImportError:
    _HAS_CV = False

try:
    import mediapipe as mp
    _HAS_MP = True
except ImportError:
    _HAS_MP = False


# ── Camera index map ──────────────────────────────────────────────────────────

_CAMERA_NAMES = ["front", "left", "rear", "right"]
_CAMERA_ID: Dict[str, int] = {name: i for i, name in enumerate(_CAMERA_NAMES)}


# ── Latest-frame buffer ───────────────────────────────────────────────────────


class _FrameBuffer:
    """Thread-safe single-slot buffer per camera — always keeps newest frame."""

    def __init__(self) -> None:
        self._frames: Dict[int, object] = {}   # camera_id → Image msg
        self._lock = threading.Lock()

    def put(self, camera_id: int, msg: object) -> None:
        with self._lock:
            self._frames[camera_id] = msg

    def drain(self) -> List[tuple]:
        with self._lock:
            result = list(self._frames.items())
            self._frames = {}
            return result


# ── MediaPipe wrapper ─────────────────────────────────────────────────────────


class _MediaPipeProcessor:
    """Wraps MediaPipe Hands + Pose with lazy initialisation."""

    def __init__(
        self,
        model_complexity: int,
        max_hands: int,
        hands_enabled: bool,
        pose_enabled: bool,
    ) -> None:
        self._complexity = model_complexity
        self._max_hands = max_hands
        self._hands_enabled = hands_enabled
        self._pose_enabled = pose_enabled
        self._hands = None
        self._pose = None
        self._ready = False

    def init(self) -> None:
        if not _HAS_MP:
            return
        mp_hands = mp.solutions.hands
        mp_pose = mp.solutions.pose
        if self._hands_enabled:
            self._hands = mp_hands.Hands(
                static_image_mode=False,
                max_num_hands=self._max_hands,
                min_detection_confidence=0.50,
                min_tracking_confidence=0.50,
                model_complexity=self._complexity,
            )
        if self._pose_enabled:
            self._pose = mp_pose.Pose(
                static_image_mode=False,
                min_detection_confidence=0.50,
                min_tracking_confidence=0.50,
                model_complexity=self._complexity,
            )
        self._ready = True

    def process(self, bgr_image) -> tuple:
        """Returns (hands_results, pose_results) from MediaPipe."""
        if not self._ready or not _HAS_MP:
            return None, None
        try:
            import numpy as np
            rgb = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB)
            rgb.flags.writeable = False
            h_res = self._hands.process(rgb) if self._hands else None
            p_res = self._pose.process(rgb) if self._pose else None
            return h_res, p_res
        except Exception:
            return None, None

    def close(self) -> None:
        if self._hands:
            self._hands.close()
        if self._pose:
            self._pose.close()


# ── Gesture node ──────────────────────────────────────────────────────────────


class GestureNode(Node):
    """Multi-camera gesture recognition node."""

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

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter("n_cameras", 4)
        self.declare_parameter("process_fps", 10.0)
        self.declare_parameter("min_confidence", 0.60)
        self.declare_parameter("model_complexity", 0)
        self.declare_parameter("max_hands", 2)
        self.declare_parameter("pose_enabled", True)
        self.declare_parameter("hands_enabled", True)
        self.declare_parameter("enabled_gestures", "")
        self.declare_parameter("camera_names", "front,left,rear,right")

        self._n_cameras: int = self.get_parameter("n_cameras").value
        self._process_fps: float = self.get_parameter("process_fps").value
        self._min_conf: float = self.get_parameter("min_confidence").value
        self._complexity: int = self.get_parameter("model_complexity").value
        self._max_hands: int = self.get_parameter("max_hands").value
        self._pose_enabled: bool = self.get_parameter("pose_enabled").value
        self._hands_enabled: bool = self.get_parameter("hands_enabled").value
        _enabled_str: str = self.get_parameter("enabled_gestures").value
        camera_names_str: str = self.get_parameter("camera_names").value

        self._enabled: Optional[Set[str]] = (
            set(g.strip() for g in _enabled_str.split(",") if g.strip())
            if _enabled_str.strip() else None
        )
        self._camera_names: List[str] = [
            n.strip() for n in camera_names_str.split(",")
            if n.strip()
        ][:self._n_cameras]

        # ── QoS ─────────────────────────────────────────────────────────────
        img_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=1,
        )
        reliable_qos = QoSProfile(depth=10)

        # ── Publishers ───────────────────────────────────────────────────────
        self._gesture_pub = self.create_publisher(
            GestureArray, "/social/gestures", reliable_qos
        )

        # ── Subscribers ──────────────────────────────────────────────────────
        self._buf = _FrameBuffer()
        self._cam_subs = []
        for name in self._camera_names:
            topic = f"/camera/{name}/image_raw"
            cam_id = _CAMERA_ID.get(name, len(self._cam_subs))
            sub = self.create_subscription(
                Image, topic,
                lambda msg, cid=cam_id: self._buf.put(cid, msg),
                img_qos,
            )
            self._cam_subs.append(sub)
            self.get_logger().info(f"Subscribed to {topic} (camera_id={cam_id})")

        self._persons: Optional[PersonStateArray] = None
        self._persons_sub = self.create_subscription(
            PersonStateArray, "/social/persons",
            lambda msg: setattr(self, "_persons", msg),
            reliable_qos,
        )

        # ── Per-camera WaveDetector instances: (camera_id, hand_idx) → det ──
        self._wave_detectors: Dict[tuple, WaveDetector] = {}

        # ── MediaPipe (initialised in background thread) ─────────────────────
        self._mp = _MediaPipeProcessor(
            model_complexity=self._complexity,
            max_hands=self._max_hands,
            hands_enabled=self._hands_enabled,
            pose_enabled=self._pose_enabled,
        )
        self._bridge = CvBridge() if _HAS_CV else None
        self._mp_ready = False

        self._init_thread = threading.Thread(
            target=self._init_mediapipe, daemon=True
        )
        self._init_thread.start()

        # ── Processing timer ─────────────────────────────────────────────────
        self._timer = self.create_timer(
            1.0 / self._process_fps, self._process_tick
        )

        self.get_logger().info(
            f"gesture_node starting — cameras={self._camera_names}, "
            f"fps={self._process_fps}, min_conf={self._min_conf}, "
            f"complexity={self._complexity}"
        )

    # ── Initialisation ────────────────────────────────────────────────────────

    def _init_mediapipe(self) -> None:
        if not _HAS_MP:
            self.get_logger().warn(
                "mediapipe not installed — gesture_node will publish no gestures. "
                "Install with: pip install mediapipe"
            )
            return
        if not _HAS_CV:
            self.get_logger().error("opencv-python / cv_bridge not available")
            return
        self.get_logger().info("Initialising MediaPipe Hands + Pose…")
        t0 = time.time()
        self._mp.init()
        self._mp_ready = True
        self.get_logger().info(
            f"MediaPipe ready ({time.time() - t0:.1f}s) — "
            f"hands={'on' if self._hands_enabled else 'off'}, "
            f"pose={'on' if self._pose_enabled else 'off'}"
        )

    # ── Processing tick ───────────────────────────────────────────────────────

    def _process_tick(self) -> None:
        if not self._mp_ready:
            return
        frames = self._buf.drain()
        if not frames:
            return

        all_gestures: List[Gesture] = []
        for cam_id, img_msg in frames:
            gestures = self._process_frame(cam_id, img_msg)
            all_gestures.extend(gestures)

        if all_gestures:
            arr = GestureArray()
            arr.header.stamp = self.get_clock().now().to_msg()
            arr.gestures = all_gestures
            self._gesture_pub.publish(arr)

    def _process_frame(self, cam_id: int, img_msg) -> List[Gesture]:
        """Run MediaPipe on one camera frame and return gesture messages."""
        if self._bridge is None:
            return []
        try:
            bgr = self._bridge.imgmsg_to_cv2(img_msg, desired_encoding="bgr8")
        except Exception as e:
            self.get_logger().debug(f"cv_bridge error cam {cam_id}: {e}")
            return []

        h_results, p_results = self._mp.process(bgr)
        gestures: List[Gesture] = []

        # ── Hand gestures ─────────────────────────────────────────────────
        if h_results and h_results.multi_hand_landmarks:
            for hand_idx, (hand_lm, hand_info) in enumerate(
                zip(h_results.multi_hand_landmarks,
                    h_results.multi_handedness)
            ):
                is_right = hand_info.classification[0].label == "Right"
                lm = [
                    Landmark(p.x, p.y, p.z)
                    for p in hand_lm.landmark
                ]
                wave_key = (cam_id, hand_idx)
                wave_det = self._wave_detectors.get(wave_key)
                if wave_det is None:
                    wave_det = WaveDetector()
                    self._wave_detectors[wave_key] = wave_det

                result = classify_hand(
                    lm, is_right_hand=is_right,
                    wave_det=wave_det,
                    enabled=self._enabled,
                )
                if result and result.confidence >= self._min_conf:
                    gestures.append(
                        self._to_msg(result, cam_id)
                    )

        # ── Body pose gestures ────────────────────────────────────────────
        if p_results and p_results.pose_landmarks:
            lm = [
                Landmark(p.x, p.y, p.z, p.visibility)
                for p in p_results.pose_landmarks.landmark
            ]
            result = classify_pose(lm, enabled=self._enabled)
            if result and result.confidence >= self._min_conf:
                gestures.append(self._to_msg(result, cam_id))

        return gestures

    # ── Person correlation ────────────────────────────────────────────────────

    def _resolve_person_id(self, cam_id: int, hand_x: float) -> int:
        """Find person_id for a gesture on camera cam_id.  Returns -1 if unknown."""
        persons = self._persons
        if persons is None or not persons.persons:
            return -1
        # Filter persons seen on this camera
        on_cam = [p for p in persons.persons if p.camera_id == cam_id]
        if len(on_cam) == 1:
            return on_cam[0].person_id
        if len(on_cam) > 1:
            # Pick closest by horizontal image position (heuristic)
            return min(on_cam, key=lambda p: abs(p.bearing_deg / 90.0 - hand_x)).person_id
        # Fall back to any tracked person
        if persons.persons:
            return persons.persons[0].person_id
        return -1

    # ── Message construction ──────────────────────────────────────────────────

    def _to_msg(self, g: ClassifiedGesture, cam_id: int) -> Gesture:
        msg = Gesture()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.gesture_type = g.gesture_type
        msg.confidence = float(g.confidence)
        msg.camera_id = cam_id
        msg.hand_x = float(g.hand_x)
        msg.hand_y = float(g.hand_y)
        msg.is_right_hand = g.is_right_hand
        msg.direction = g.direction
        msg.source = g.source
        msg.person_id = self._resolve_person_id(cam_id, g.hand_x)
        return msg

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

    def destroy_node(self) -> None:
        self._mp.close()
        super().destroy_node()


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


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