import time

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

from std_msgs.msg import Int32, String
from geometry_msgs.msg import PoseArray, PoseStamped, Point
from sensor_msgs.msg import Image
from builtin_interfaces.msg import Time as TimeMsg

from saltybot_social_msgs.msg import (
    FaceDetection,
    FaceDetectionArray,
    PersonState,
    PersonStateArray,
)
from saltybot_social.identity_fuser import IdentityFuser
from saltybot_social.person_state import State


class PersonStateTrackerNode(Node):
    """Main ROS2 node for multi-modal person tracking."""

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

        # Parameters
        self.declare_parameter('engagement_distance', 2.0)
        self.declare_parameter('absent_timeout', 5.0)
        self.declare_parameter('prune_timeout', 30.0)
        self.declare_parameter('update_rate', 10.0)
        self.declare_parameter('n_cameras', 4)
        self.declare_parameter('uwb_enabled', False)

        self._engagement_distance = self.get_parameter('engagement_distance').value
        self._absent_timeout = self.get_parameter('absent_timeout').value
        self._prune_timeout = self.get_parameter('prune_timeout').value
        update_rate = self.get_parameter('update_rate').value
        n_cameras = self.get_parameter('n_cameras').value
        uwb_enabled = self.get_parameter('uwb_enabled').value

        self._fuser = IdentityFuser()

        # QoS profiles
        best_effort_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )
        reliable_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )

        # Subscriptions
        self.create_subscription(
            FaceDetectionArray,
            '/social/faces/detections',
            self._on_face_detections,
            best_effort_qos
        )
        self.create_subscription(
            String,
            '/social/speech/speaker_id',
            self._on_speaker_id,
            10
        )
        if uwb_enabled:
            self.create_subscription(
                PoseArray,
                '/uwb/positions',
                self._on_uwb,
                10
            )

        # Camera subscriptions (monitor topic existence)
        self.create_subscription(
            Image, '/camera/color/image_raw',
            self._on_camera_image, best_effort_qos)
        for i in range(n_cameras):
            self.create_subscription(
                Image, f'/surround/cam{i}/image_raw',
                self._on_camera_image, best_effort_qos)

        # Existing person tracker position
        self.create_subscription(
            PoseStamped,
            '/person/target',
            self._on_person_target,
            10
        )

        # Publishers
        self._persons_pub = self.create_publisher(
            PersonStateArray, '/social/persons', best_effort_qos)
        self._attention_pub = self.create_publisher(
            Int32, '/social/attention/target_id', reliable_qos)

        # Timer
        timer_period = 1.0 / update_rate
        self.create_timer(timer_period, self._on_timer)

        self.get_logger().info(
            f'PersonStateTracker started: engagement={self._engagement_distance}m, '
            f'absent_timeout={self._absent_timeout}s, rate={update_rate}Hz, '
            f'cameras={n_cameras}, uwb={uwb_enabled}')

    def _on_face_detections(self, msg: FaceDetectionArray):
        now = time.monotonic()
        for face in msg.faces:
            position_3d = None
            # Use bbox center as rough position estimate if depth is available
            # Real 3D position would come from depth camera projection
            if face.bbox_x > 0 or face.bbox_y > 0:
                # Approximate: use bbox center as bearing proxy, distance from bbox size
                # This is a placeholder; real impl would use depth image lookup
                position_3d = (
                    max(0.5, 1.0 / max(face.bbox_w, 0.01)),  # rough depth from bbox width
                    face.bbox_x + face.bbox_w / 2.0 - 0.5,   # x offset from center
                    face.bbox_y + face.bbox_h / 2.0 - 0.5    # y offset from center
                )

            camera_id = -1
            if hasattr(face.header, 'frame_id') and face.header.frame_id:
                frame = face.header.frame_id
                if 'cam' in frame:
                    try:
                        camera_id = int(frame.split('cam')[-1].split('_')[0])
                    except ValueError:
                        pass

            self._fuser.update_face(
                face_id=face.face_id,
                name=face.person_name,
                position_3d=position_3d,
                camera_id=camera_id,
                now=now
            )

    def _on_speaker_id(self, msg: String):
        now = time.monotonic()
        speaker_id = msg.data
        # Parse "speaker_<id>" format or raw id
        if speaker_id.startswith('speaker_'):
            speaker_id = speaker_id[len('speaker_'):]
        self._fuser.update_speaker(speaker_id, now)

    def _on_uwb(self, msg: PoseArray):
        now = time.monotonic()
        for i, pose in enumerate(msg.poses):
            position = (pose.position.x, pose.position.y, pose.position.z)
            anchor_id = f'uwb_{i}'
            if hasattr(msg, 'header') and msg.header.frame_id:
                anchor_id = f'{msg.header.frame_id}_{i}'
            self._fuser.update_uwb(anchor_id, position, now)

    def _on_camera_image(self, msg: Image):
        # Monitor only -- no processing here
        pass

    def _on_person_target(self, msg: PoseStamped):
        now = time.monotonic()
        position_3d = (
            msg.pose.position.x,
            msg.pose.position.y,
            msg.pose.position.z
        )
        # Update position for unidentified person (from YOLOv8 tracker)
        self._fuser.update_face(
            face_id=-1,
            name='unknown',
            position_3d=position_3d,
            camera_id=-1,
            now=now
        )

    def _on_timer(self):
        now = time.monotonic()
        tracks = self._fuser.get_all_tracks()

        # Update state machine for all tracks
        for track in tracks:
            track.update_state(
                now,
                engagement_distance=self._engagement_distance,
                absent_timeout=self._absent_timeout
            )

        # Compute engagement scores
        for track in tracks:
            if track.state == State.ABSENT:
                track.engagement_score = 0.0
            elif track.state == State.TALKING:
                track.engagement_score = 1.0
            elif track.state == State.ENGAGED:
                track.engagement_score = max(0.0, 1.0 - track.distance / self._engagement_distance)
            elif track.state == State.APPROACHING:
                track.engagement_score = 0.3
            elif track.state == State.LEAVING:
                track.engagement_score = 0.1
            else:
                track.engagement_score = 0.0

        # Prune long-absent tracks
        self._fuser.prune_absent(self._prune_timeout)

        # Compute attention target
        attention_id = IdentityFuser.compute_attention(tracks)

        # Build and publish PersonStateArray
        msg = PersonStateArray()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'
        msg.primary_attention_id = attention_id

        for track in tracks:
            ps = PersonState()
            ps.header.stamp = msg.header.stamp
            ps.header.frame_id = 'base_link'
            ps.person_id = track.person_id
            ps.person_name = track.name
            ps.face_id = track.face_id
            ps.speaker_id = track.speaker_id
            ps.uwb_anchor_id = track.uwb_anchor_id
            if track.position is not None:
                ps.position = Point(
                    x=float(track.position[0]),
                    y=float(track.position[1]),
                    z=float(track.position[2]))
            ps.distance = float(track.distance)
            ps.bearing_deg = float(track.bearing_deg)
            ps.state = int(track.state)
            ps.engagement_score = float(track.engagement_score)
            ps.last_seen = self._mono_to_ros_time(track.last_seen)
            ps.camera_id = track.camera_id
            msg.persons.append(ps)

        self._persons_pub.publish(msg)

        # Publish attention target
        att_msg = Int32()
        att_msg.data = attention_id
        self._attention_pub.publish(att_msg)

    def _mono_to_ros_time(self, mono: float) -> TimeMsg:
        """Convert monotonic timestamp to approximate ROS time."""
        # Offset from monotonic to wall clock
        offset = time.time() - time.monotonic()
        wall = mono + offset
        sec = int(wall)
        nsec = int((wall - sec) * 1e9)
        t = TimeMsg()
        t.sec = sec
        t.nanosec = nsec
        return t


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


if __name__ == '__main__':
    main()
