"""
pointing_node.py — Hand gesture pointing direction (Issue #221).

Converts a 'point' gesture into a precise 3-D ray using MediaPipe Hands
re-run on the D435i colour stream and the registered depth image.

Subscribes:
  /social/gestures                         saltybot_social_msgs/GestureArray
  /camera/color/image_raw                  sensor_msgs/Image   (D435i colour)
  /camera/depth/image_rect_raw             sensor_msgs/Image   (D435i depth, float32 m)
  /camera/color/camera_info                sensor_msgs/CameraInfo

Publishes:
  /saltybot/pointing_target                saltybot_social_msgs/PointingTarget  (5 Hz)

Algorithm
---------
1. Cache the most recent 'point' Gesture from /social/gestures.
2. Synchronise colour + depth frames (ApproximateTimeSynchronizer, 50 ms slop).
3. When a 'point' gesture was received within `gesture_timeout_s`, run
   MediaPipe Hands on the colour frame.
4. Locate the hand closest to the gesture anchor (hand_x, hand_y).
5. Call compute_pointing_ray(): unproject INDEX_MCP and INDEX_TIP using depth,
   form the unit direction vector, extend to `ref_distance_m`.
6. A 5 Hz timer publishes the latest PointingTarget (is_active=false if stale).

Parameters
----------
ref_distance_m    float  2.0   Default range when depth is valid (m)
gesture_timeout_s float  1.0   Max age of a cached 'point' gesture (s)
min_confidence    float  0.50  MediaPipe Hands min detection confidence
model_complexity  int    0     MediaPipe model complexity (0=lite)
publish_hz        float  5.0   Output publication rate (Hz)
"""

from __future__ import annotations

import math
import time
from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

import message_filters
import numpy as np
from cv_bridge import CvBridge

from sensor_msgs.msg import Image, CameraInfo
from saltybot_social_msgs.msg import GestureArray, PointingTarget

from ._pointing_ray import compute_pointing_ray

# Optional MediaPipe — absent in CI
try:
    import cv2
    import mediapipe as mp
    _HAS_MP = True
except ImportError:
    _HAS_MP = False


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=4,
)
_LATCHED_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
)


class PointingNode(Node):

    def __init__(self):
        super().__init__('pointing_node')

        self.declare_parameter('ref_distance_m',    2.0)
        self.declare_parameter('gesture_timeout_s', 1.0)
        self.declare_parameter('min_confidence',    0.50)
        self.declare_parameter('model_complexity',  0)
        self.declare_parameter('publish_hz',        5.0)

        self._ref_dist      = self.get_parameter('ref_distance_m').value
        self._gesture_tmo   = self.get_parameter('gesture_timeout_s').value
        self._min_conf      = self.get_parameter('min_confidence').value
        self._complexity    = self.get_parameter('model_complexity').value
        publish_hz          = self.get_parameter('publish_hz').value

        self._bridge = CvBridge()
        self._camera_info: Optional[CameraInfo] = None

        # Cached state
        self._last_gesture = None          # most recent Gesture with type='point'
        self._last_gesture_t = 0.0         # time.monotonic() stamp
        self._last_result: Optional[dict] = None

        # MediaPipe Hands (lazy init)
        self._hands = None
        if _HAS_MP:
            self._hands = mp.solutions.hands.Hands(
                static_image_mode=False,
                max_num_hands=2,
                min_detection_confidence=self._min_conf,
                min_tracking_confidence=0.5,
                model_complexity=self._complexity,
            )

        # Subscribers
        self.create_subscription(
            GestureArray, '/social/gestures', self._on_gestures,
            QoSProfile(depth=10),
        )
        self.create_subscription(
            CameraInfo, '/camera/color/camera_info', self._on_camera_info,
            _LATCHED_QOS,
        )

        color_sub = message_filters.Subscriber(
            self, Image, '/camera/color/image_raw', qos_profile=_SENSOR_QOS)
        depth_sub = message_filters.Subscriber(
            self, Image, '/camera/depth/image_rect_raw', qos_profile=_SENSOR_QOS)
        self._sync = message_filters.ApproximateTimeSynchronizer(
            [color_sub, depth_sub], queue_size=4, slop=0.05)
        self._sync.registerCallback(self._on_frame)

        # Publisher + timer
        self._pub = self.create_publisher(PointingTarget, '/saltybot/pointing_target', 10)
        self.create_timer(1.0 / publish_hz, self._tick)

        self.get_logger().info(
            f'pointing_node ready — mediapipe={"on" if _HAS_MP else "off"} '
            f'ref={self._ref_dist}m timeout={self._gesture_tmo}s'
        )

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

    def _on_gestures(self, msg: GestureArray) -> None:
        for g in msg.gestures:
            if g.gesture_type == 'point':
                self._last_gesture   = g
                self._last_gesture_t = time.monotonic()
                break   # first point gesture wins

    def _on_camera_info(self, msg: CameraInfo) -> None:
        self._camera_info = msg

    def _on_frame(self, color_msg: Image, depth_msg: Image) -> None:
        if self._camera_info is None:
            return
        age = time.monotonic() - self._last_gesture_t
        if age > self._gesture_tmo:
            return   # no recent point gesture — skip expensive MediaPipe call

        try:
            bgr   = self._bridge.imgmsg_to_cv2(color_msg, 'bgr8')
            depth = self._bridge.imgmsg_to_cv2(depth_msg, 'passthrough').astype(np.float32)
        except Exception as exc:
            self.get_logger().error(f'cv_bridge: {exc}', throttle_duration_sec=5.0)
            return

        if depth.max() > 100.0:        # uint16 mm → float32 m
            depth /= 1000.0

        lms = self._run_mediapipe(bgr)
        if lms is None:
            return

        # Select hand closest to gesture anchor
        g = self._last_gesture
        best_lm  = self._pick_hand(lms, g.hand_x, g.hand_y)
        if best_lm is None:
            return

        h, w = bgr.shape[:2]
        K = np.array(self._camera_info.k).reshape(3, 3)
        fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2]

        result = compute_pointing_ray(
            landmarks     = best_lm,
            depth_img     = depth,
            fx=fx, fy=fy, cx=cx, cy=cy,
            img_w=w, img_h=h,
            ref_distance  = self._ref_dist,
        )
        if result is not None:
            self._last_result = result

    # ── MediaPipe helpers ──────────────────────────────────────────────────────

    def _run_mediapipe(self, bgr: np.ndarray):
        """Run MediaPipe Hands on a BGR frame; returns list of landmark lists or None."""
        if self._hands is None:
            return None
        import cv2
        rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)
        rgb.flags.writeable = False
        results = self._hands.process(rgb)
        if not results.multi_hand_landmarks:
            return None
        return [
            [(lm.x, lm.y, lm.z) for lm in hand.landmark]
            for hand in results.multi_hand_landmarks
        ]

    def _pick_hand(self, landmark_sets, anchor_x: float, anchor_y: float):
        """Return the landmark set whose INDEX_TIP is closest to (anchor_x, anchor_y)."""
        best, best_dist = None, float('inf')
        for lms in landmark_sets:
            tip = lms[8]   # INDEX_TIP = 8
            dist = math.hypot(tip[0] - anchor_x, tip[1] - anchor_y)
            if dist < best_dist:
                best, best_dist = lms, dist
        return best

    # ── 5 Hz publish tick ─────────────────────────────────────────────────────

    def _tick(self) -> None:
        msg = PointingTarget()
        msg.header.stamp    = self.get_clock().now().to_msg()
        msg.header.frame_id = 'camera_color_optical_frame'

        age = time.monotonic() - self._last_gesture_t
        msg.is_active = age <= self._gesture_tmo

        if self._last_gesture is not None:
            msg.person_id        = self._last_gesture.person_id
            msg.confidence       = self._last_gesture.confidence
            msg.coarse_direction = self._last_gesture.direction

        if msg.is_active and self._last_result is not None:
            ox, oy, oz = self._last_result['origin_3d']
            dx, dy, dz = self._last_result['direction_3d']
            tx, ty, tz = self._last_result['target_3d']

            msg.origin.x    = ox;   msg.origin.y    = oy;   msg.origin.z    = oz
            msg.direction.x = dx;   msg.direction.y = dy;   msg.direction.z = dz
            msg.target.x    = tx;   msg.target.y    = ty;   msg.target.z    = tz
            msg.range_m     = self._last_result['range_m']

        self._pub.publish(msg)

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


def main(args=None):
    rclpy.init(args=args)
    node = PointingNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
