"""
face_emotion_node.py — Facial emotion classifier node (Issue #359).

Subscribes to the raw colour camera stream, runs MediaPipe FaceMesh to
detect facial landmarks, applies geometric emotion rules, and publishes
the result at the camera frame rate (≤ param max_fps).

Subscribes
----------
  /camera/color/image_raw   sensor_msgs/Image   (BEST_EFFORT)

Publishes
---------
  /saltybot/face_emotions   saltybot_scene_msgs/FaceEmotionArray

Parameters
----------
  max_fps           float   15.0   Maximum classification rate (Hz)
  max_faces         int     4      MediaPipe max_num_faces
  min_detection_conf float  0.5    MediaPipe min detection confidence
  min_tracking_conf  float  0.5    MediaPipe min tracking confidence

Notes
-----
  * MediaPipe FaceMesh (mediapipe.solutions.face_mesh) is initialised lazily
    in a background thread to avoid blocking the ROS2 executor.
  * If mediapipe is not installed the node still starts but logs a warning
    and publishes empty arrays.
  * Landmark coordinates are always normalised to [0..1] image space.
"""

from __future__ import annotations

import threading
import time
from typing import Optional

import numpy as np

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

from sensor_msgs.msg import Image
from saltybot_scene_msgs.msg import FaceEmotion, FaceEmotionArray
from ._face_emotion import FaceLandmarks, detect_emotion, from_mediapipe


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
)


class _MPFaceMesh:
    """Lazy wrapper around mediapipe.solutions.face_mesh.FaceMesh."""

    def __init__(
        self,
        max_faces: int = 4,
        min_detection_conf: float = 0.5,
        min_tracking_conf:  float = 0.5,
    ) -> None:
        self._max_faces         = max_faces
        self._min_det_conf      = min_detection_conf
        self._min_track_conf    = min_tracking_conf
        self._mesh              = None
        self._available         = False
        self._ready             = threading.Event()
        threading.Thread(target=self._init, daemon=True).start()

    def _init(self) -> None:
        try:
            import mediapipe as mp
            self._mesh = mp.solutions.face_mesh.FaceMesh(
                static_image_mode        = False,
                max_num_faces            = self._max_faces,
                refine_landmarks         = False,
                min_detection_confidence = self._min_det_conf,
                min_tracking_confidence  = self._min_track_conf,
            )
            self._available = True
        except Exception:
            self._available = False
        finally:
            self._ready.set()

    def process(self, bgr: np.ndarray):
        """Run FaceMesh on a BGR uint8 frame.  Returns mp results or None."""
        if not self._ready.wait(timeout=5.0) or not self._available:
            return None
        import cv2
        rgb = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB)
        return self._mesh.process(rgb)


class FaceEmotionNode(Node):

    def __init__(self) -> None:
        super().__init__('face_emotion_node')

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter('max_fps',            15.0)
        self.declare_parameter('max_faces',          4)
        self.declare_parameter('min_detection_conf', 0.5)
        self.declare_parameter('min_tracking_conf',  0.5)

        p = self.get_parameter
        self._min_period  = 1.0 / max(float(p('max_fps').value),  0.1)
        max_faces         = int(p('max_faces').value)
        min_det           = float(p('min_detection_conf').value)
        min_trk           = float(p('min_tracking_conf').value)

        # Lazy-initialised MediaPipe
        self._mp = _MPFaceMesh(
            max_faces          = max_faces,
            min_detection_conf = min_det,
            min_tracking_conf  = min_trk,
        )

        self._last_proc: float = 0.0

        # ── Subscriber / publisher ───────────────────────────────────────────
        self._sub = self.create_subscription(
            Image, '/camera/color/image_raw',
            self._on_image, _SENSOR_QOS,
        )
        self._pub = self.create_publisher(
            FaceEmotionArray, '/saltybot/face_emotions', 10
        )

        self.get_logger().info(
            f'face_emotion_node ready — '
            f'max_fps={1.0/self._min_period:.1f}, max_faces={max_faces}'
        )

    # ── Callback ──────────────────────────────────────────────────────────────

    def _on_image(self, msg: Image) -> None:
        now = time.monotonic()
        if now - self._last_proc < self._min_period:
            return
        self._last_proc = now

        enc = msg.encoding.lower()
        if enc in ('bgr8', 'rgb8'):
            data = np.frombuffer(msg.data, dtype=np.uint8)
            try:
                frame = data.reshape((msg.height, msg.width, 3))
            except ValueError:
                return
            if enc == 'rgb8':
                import cv2
                frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
        else:
            self.get_logger().warn(
                f'Unsupported image encoding {msg.encoding!r}',
                throttle_duration_sec=10.0,
            )
            return

        results = self._mp.process(frame)

        out = FaceEmotionArray()
        out.header.stamp    = msg.header.stamp
        out.header.frame_id = msg.header.frame_id or 'camera_color_optical_frame'

        if results and results.multi_face_landmarks:
            for face_id, lms in enumerate(results.multi_face_landmarks):
                try:
                    fl  = from_mediapipe(lms)
                    res = detect_emotion(fl)
                except Exception as exc:
                    self.get_logger().warn(
                        f'Emotion detection failed for face {face_id}: {exc}',
                        throttle_duration_sec=5.0,
                    )
                    continue

                fe = FaceEmotion()
                fe.header     = out.header
                fe.face_id    = face_id
                fe.emotion    = res.emotion
                fe.confidence = float(res.confidence)
                fe.mouth_open = float(res.features.mouth_open)
                fe.smile      = float(res.features.smile)
                fe.brow_raise = float(res.features.brow_raise)
                fe.eye_open   = float(res.features.eye_open)
                out.faces.append(fe)

        out.face_count = len(out.faces)
        self._pub.publish(out)


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


if __name__ == '__main__':
    main()
