"""enrollment_node.py -- ROS2 person enrollment node for saltybot social.

Coordinates person enrollment:
- Forwards /social/enroll to face_recognizer's service
- Owns persistent SQLite gallery (PersonDB)
- Voice-triggered enrollment via "remember me my name is X"
- Gallery management services (list/delete/update)
- Syncs DB from /social/faces/embeddings topic
"""

import threading
import numpy as np

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 (
    FaceDetectionArray,
    FaceEmbedding,
    FaceEmbeddingArray,
)
from saltybot_social_msgs.srv import (
    EnrollPerson,
    ListPersons,
    DeletePerson,
    UpdatePerson,
)

from saltybot_social_enrollment.person_db import PersonDB


class EnrollmentNode(Node):
    def __init__(self):
        super().__init__('enrollment_node')

        # Parameters
        self.declare_parameter('db_path', '/mnt/nvme/saltybot/gallery/persons.db')
        self.declare_parameter('voice_samples_dir', '/mnt/nvme/saltybot/gallery/voice')
        self.declare_parameter('auto_enroll_phrase', 'remember me my name is')
        self.declare_parameter('n_samples_default', 10)

        db_path = self.get_parameter('db_path').value
        self._voice_dir = self.get_parameter('voice_samples_dir').value
        self._phrase = self.get_parameter('auto_enroll_phrase').value
        self._n_samples = self.get_parameter('n_samples_default').value

        self._db = PersonDB(db_path)
        self.get_logger().info(f'PersonDB initialized at {db_path}')

        # Client to face_recognizer's enroll service
        self._enroll_client = self.create_client(
            EnrollPerson, '/social/face_recognizer/enroll'
        )

        # QoS profiles
        best_effort_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )
        reliable_qos = QoSProfile(
            depth=1,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
        )
        status_qos = QoSProfile(
            depth=1,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )

        # Subscriptions
        self.create_subscription(
            FaceDetectionArray, '/social/faces/detections',
            self._on_detections, best_effort_qos
        )
        self.create_subscription(
            FaceEmbeddingArray, '/social/faces/embeddings',
            self._on_embeddings, reliable_qos
        )
        self.create_subscription(
            String, '/social/speech/transcript',
            self._on_transcript, best_effort_qos
        )
        self.create_subscription(
            String, '/social/speech/command',
            self._on_command, best_effort_qos
        )

        # Services
        self.create_service(EnrollPerson, '/social/enroll', self._handle_enroll)
        self.create_service(ListPersons, '/social/persons/list', self._handle_list)
        self.create_service(DeletePerson, '/social/persons/delete', self._handle_delete)
        self.create_service(UpdatePerson, '/social/persons/update', self._handle_update)

        # Publishers
        self._pub_embeddings = self.create_publisher(
            FaceEmbeddingArray, '/social/faces/embeddings', reliable_qos
        )
        self._pub_status = self.create_publisher(
            String, '/social/enrollment/status', status_qos
        )

        self.get_logger().info('EnrollmentNode ready')

    # ---- Voice-triggered enrollment ----

    def _on_transcript(self, msg: String):
        text = msg.data.lower()
        phrase = self._phrase.lower()
        if phrase in text:
            idx = text.index(phrase) + len(phrase)
            name = text[idx:].strip()
            # Clean up: take first 3 words max as the name
            words = name.split()
            if words:
                name = ' '.join(words[:3]).title()
                self.get_logger().info(f'Voice enrollment triggered: "{name}"')
                self._trigger_voice_enroll(name)

    def _on_command(self, msg: String):
        # Reserved for explicit voice commands (e.g., "enroll Alice")
        pass

    def _trigger_voice_enroll(self, name: str):
        if not self._enroll_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().warn(
                'face_recognizer enroll service not available'
            )
            self._publish_status(f'Enrollment failed: face_recognizer unavailable')
            return

        req = EnrollPerson.Request()
        req.name = name
        req.mode = 'face'
        req.n_samples = self._n_samples
        future = self._enroll_client.call_async(req)
        future.add_done_callback(
            lambda f: self._on_enroll_done(f, name)
        )
        self._publish_status(f'Enrolling "{name}"... look at the camera')

    def _on_enroll_done(self, future, name: str):
        try:
            resp = future.result()
            if resp.success:
                status = f'Enrolled "{name}" (id={resp.person_id})'
                self.get_logger().info(status)
            else:
                status = f'Enrollment failed for "{name}": {resp.message}'
                self.get_logger().warn(status)
            self._publish_status(status)
        except Exception as e:
            self.get_logger().error(f'Enroll call failed: {e}')
            self._publish_status(f'Enrollment error: {e}')

    # ---- Face detection callback (during enrollment) ----

    def _on_detections(self, msg: FaceDetectionArray):
        # Reserved for future direct enrollment (without face_recognizer)
        pass

    # ---- Embeddings sync from face_recognizer ----

    def _on_embeddings(self, msg: FaceEmbeddingArray):
        for emb in msg.embeddings:
            existing = self._db.get_person(emb.person_id)
            if existing is None:
                arr = np.array(emb.embedding, dtype=np.float32)
                self._db.add_person(
                    emb.person_name, arr, emb.sample_count
                )
                self.get_logger().info(
                    f'Synced new person from face_recognizer: '
                    f'{emb.person_name} (id={emb.person_id})'
                )

    # ---- Service handlers ----

    def _handle_enroll(self, request, response):
        """Forward enrollment to face_recognizer service."""
        if not self._enroll_client.wait_for_service(timeout_sec=2.0):
            response.success = False
            response.message = 'face_recognizer service unavailable'
            return response

        # Use threading.Event to bridge async call in service callback
        event = threading.Event()
        result_holder = {}

        req = EnrollPerson.Request()
        req.name = request.name
        req.mode = request.mode
        req.n_samples = request.n_samples

        future = self._enroll_client.call_async(req)

        def _done(f):
            try:
                result_holder['resp'] = f.result()
            except Exception as e:
                result_holder['err'] = str(e)
            event.set()

        future.add_done_callback(_done)
        success = event.wait(timeout=35.0)

        if not success:
            response.success = False
            response.message = 'Enrollment timed out'
        elif 'resp' in result_holder:
            resp = result_holder['resp']
            response.success = resp.success
            response.message = resp.message
            response.person_id = resp.person_id
        else:
            response.success = False
            response.message = result_holder.get('err', 'Unknown error')

        return response

    def _handle_list(self, request, response):
        persons = self._db.get_all()
        response.persons = []
        for p in persons:
            fe = FaceEmbedding()
            fe.person_id = p['id']
            fe.person_name = p['name']
            fe.sample_count = p['sample_count']
            fe.enrolled_at.sec = int(p['enrolled_at'])
            fe.enrolled_at.nanosec = int(
                (p['enrolled_at'] - int(p['enrolled_at'])) * 1e9
            )
            if p['embedding'] is not None:
                fe.embedding = p['embedding'].tolist()
            response.persons.append(fe)
        return response

    def _handle_delete(self, request, response):
        success = self._db.delete_person(request.person_id)
        response.success = success
        if success:
            response.message = f'Deleted person {request.person_id}'
            self.get_logger().info(response.message)
            self._publish_embeddings_from_db()
        else:
            response.message = f'Person {request.person_id} not found'
        return response

    def _handle_update(self, request, response):
        success = self._db.update_name(request.person_id, request.new_name)
        response.success = success
        if success:
            response.message = f'Updated person {request.person_id} name to "{request.new_name}"'
            self.get_logger().info(response.message)
            self._publish_embeddings_from_db()
        else:
            response.message = f'Person {request.person_id} not found'
        return response

    # ---- Helpers ----

    def _publish_status(self, text: str):
        msg = String()
        msg.data = text
        self._pub_status.publish(msg)

    def _publish_embeddings_from_db(self):
        persons = self._db.get_all()
        arr = FaceEmbeddingArray()
        arr.header.stamp = self.get_clock().now().to_msg()
        for p in persons:
            if p['embedding'] is not None:
                fe = FaceEmbedding()
                fe.person_id = p['id']
                fe.person_name = p['name']
                fe.sample_count = p['sample_count']
                fe.enrolled_at.sec = int(p['enrolled_at'])
                fe.enrolled_at.nanosec = int(
                    (p['enrolled_at'] - int(p['enrolled_at'])) * 1e9
                )
                fe.embedding = p['embedding'].tolist()
                arr.embeddings.append(fe)
        self._pub_embeddings.publish(arr)


def main(args=None):
    rclpy.init(args=args)
    node = EnrollmentNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == '__main__':
    main()
