"""mock_sensors.py — Mock sensor publishers for CI integration tests.

Publishes minimal valid data on all topics that social-bot nodes subscribe to,
so the pipeline can start and produce output without real hardware.

Topics published:
  /camera/color/image_raw          — blank 640×480 RGB8 image at 15Hz
  /social/speech/transcript        — canned SpeechTranscript at 0.5Hz
  /social/faces/detections         — empty FaceDetectionArray at 15Hz
  /social/tracking/fused_target    — synthetic FusedTarget at 10Hz
  /social/persons                  — single synthetic PersonStateArray at 5Hz
  /uwb/target                      — synthetic PoseStamped at 10Hz

Usage (standalone):
  ros2 run saltybot_social_tests mock_sensor_pub

Usage (programmatic):
  from saltybot_social_tests.mock_sensors import MockSensorPublisher
  pub = MockSensorPublisher(node)
  pub.start()
  ...
  pub.stop()
"""

from __future__ import annotations

import threading
import time
import struct
from typing import Optional

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

from std_msgs.msg import Header, String
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3


# ── Helpers ───────────────────────────────────────────────────────────────────

def _now_header(node: Node, frame_id: str = "camera_color_optical_frame") -> Header:
    h = Header()
    h.stamp = node.get_clock().now().to_msg()
    h.frame_id = frame_id
    return h


def _blank_rgb_image(node: Node, width: int = 640, height: int = 480) -> Image:
    img = Image()
    img.header = _now_header(node)
    img.width = width
    img.height = height
    img.encoding = "rgb8"
    img.step = width * 3
    img.data = bytes(width * height * 3)  # all-black frame
    return img


# ── Mock publisher class ──────────────────────────────────────────────────────

class MockSensorPublisher:
    """Publishes mock sensor data for all social-bot pipeline inputs."""

    def __init__(self, node: Node) -> None:
        self._node = node
        self._running = False
        self._threads: list = []

        be_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT, depth=5
        )
        rel_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE, depth=10
        )

        # Camera image (required by face detection + segmentation)
        self._img_pub = node.create_publisher(Image, "/camera/color/image_raw", be_qos)

        # Fused tracking target (required by social_nav, person_follower)
        try:
            from saltybot_social_msgs.msg import FusedTarget, FaceDetectionArray, PersonStateArray
            self._fused_pub = node.create_publisher(
                FusedTarget, "/social/tracking/fused_target", rel_qos
            )
            self._faces_pub = node.create_publisher(
                FaceDetectionArray, "/social/faces/detections", be_qos
            )
            self._persons_pub = node.create_publisher(
                PersonStateArray, "/social/persons", rel_qos
            )
            self._has_social_msgs = True
        except ImportError:
            self._has_social_msgs = False
            node.get_logger().warn("saltybot_social_msgs not found — limited mock data")

        try:
            from saltybot_social_msgs.msg import SpeechTranscript
            self._transcript_pub = node.create_publisher(
                SpeechTranscript, "/social/speech/transcript", rel_qos
            )
            self._has_speech_msgs = True
        except ImportError:
            self._has_speech_msgs = False

        # UWB target (required by person_follower fusion)
        self._uwb_pub = node.create_publisher(PoseStamped, "/uwb/target", rel_qos)

        self._canned_phrases = [
            "Hello Salty, how are you?",
            "What time is it?",
            "Follow me please.",
            "Tell me a joke.",
        ]
        self._phrase_idx = 0

    def start(self) -> None:
        """Start all mock publisher threads."""
        self._running = True
        specs = [
            ("camera",    self._pub_camera,    1.0 / 15.0),
            ("faces",     self._pub_faces,      1.0 / 15.0),
            ("fused",     self._pub_fused,      1.0 / 10.0),
            ("persons",   self._pub_persons,    1.0 / 5.0),
            ("uwb",       self._pub_uwb,        1.0 / 10.0),
            ("transcript",self._pub_transcript, 2.0),
        ]
        for name, fn, interval in specs:
            t = threading.Thread(
                target=self._loop, args=(fn, interval),
                name=f"mock_{name}", daemon=True
            )
            t.start()
            self._threads.append(t)

    def stop(self) -> None:
        self._running = False

    def _loop(self, publish_fn, interval_s: float) -> None:
        while self._running:
            try:
                publish_fn()
            except Exception:
                pass
            time.sleep(interval_s)

    def _pub_camera(self) -> None:
        self._img_pub.publish(_blank_rgb_image(self._node))

    def _pub_faces(self) -> None:
        if not self._has_social_msgs:
            return
        from saltybot_social_msgs.msg import FaceDetectionArray
        msg = FaceDetectionArray()
        msg.header = _now_header(self._node)
        # Empty array — camera is "working" but no face in frame
        self._faces_pub.publish(msg)

    def _pub_fused(self) -> None:
        if not self._has_social_msgs:
            return
        from saltybot_social_msgs.msg import FusedTarget
        msg = FusedTarget()
        msg.header = _now_header(self._node, "base_link")
        msg.position.x = 1.5   # 1.5m ahead
        msg.position.y = 0.0
        msg.velocity.x = 0.0
        msg.track_id = 1
        msg.confidence = 0.85
        self._fused_pub.publish(msg)

    def _pub_persons(self) -> None:
        if not self._has_social_msgs:
            return
        from saltybot_social_msgs.msg import PersonStateArray, PersonState
        msg = PersonStateArray()
        msg.header = _now_header(self._node, "base_link")
        p = PersonState()
        p.header = msg.header
        p.person_id = 1
        p.person_name = "TestPerson"
        p.state = PersonState.STATE_ENGAGED
        p.distance = 1.5
        p.bearing_deg = 0.0
        p.engagement_score = 0.8
        msg.persons = [p]
        self._persons_pub.publish(msg)

    def _pub_uwb(self) -> None:
        msg = PoseStamped()
        msg.header = _now_header(self._node, "base_link")
        msg.pose.position.x = 1.5
        msg.pose.position.y = 0.1
        msg.pose.orientation.w = 1.0
        self._uwb_pub.publish(msg)

    def _pub_transcript(self) -> None:
        if not self._has_speech_msgs:
            return
        from saltybot_social_msgs.msg import SpeechTranscript
        msg = SpeechTranscript()
        msg.header = _now_header(self._node)
        msg.text = self._canned_phrases[self._phrase_idx % len(self._canned_phrases)]
        msg.speaker_id = "test_person"
        msg.confidence = 0.95
        msg.audio_duration = 1.2
        msg.is_partial = False
        self._transcript_pub.publish(msg)
        self._phrase_idx += 1


# ── Standalone entry point ────────────────────────────────────────────────────

def main(args=None) -> None:
    rclpy.init(args=args)
    node = rclpy.create_node("mock_sensor_pub")
    pub = MockSensorPublisher(node)
    pub.start()
    node.get_logger().info("Mock sensor publisher running — Ctrl+C to stop")
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        pub.stop()
        node.destroy_node()
        rclpy.shutdown()
