"""test_topic_rates.py — Verify social-bot topics publish at expected rates.

Tests measure actual Hz over a 3-second window and assert:
  /social/faces/detections    >= 10 Hz   (face detector, camera@15Hz input)
  /social/tracking/fused_target >= 8 Hz  (tracking fusion)
  /social/orchestrator/state   >= 1 Hz   (orchestrator heartbeat)
  /camera/color/image_raw     >= 12 Hz   (mock sensor publisher)
  /uwb/target                 >= 8 Hz    (mock UWB)
  /social/persons             >= 3 Hz    (person state tracker)

Run as part of a live stack (requires social_test.launch.py to be running):
  pytest test_topic_rates.py -v --stack-running

Or run standalone with mock sensor pub only for camera/uwb tests.
"""

from __future__ import annotations

import os
import sys
import time
import unittest

import pytest
import rclpy
from rclpy.node import Node

sys.path.insert(0, os.path.dirname(os.path.dirname(__file__)))
from saltybot_social_tests.test_helpers import TopicRateChecker, poll_until


# ── Rate requirements ─────────────────────────────────────────────────────────
# (topic, msg_type_path, min_hz, warm_up_s)
RATE_SPECS = [
    # Mock-published topics — should always pass
    ("camera_image",       "/camera/color/image_raw",         "sensor_msgs.msg.Image",         12.0, 2.0),
    ("uwb_target",         "/uwb/target",                     "geometry_msgs.msg.PoseStamped",  8.0, 2.0),
    # Social-bot topics
    ("faces_detections",   "/social/faces/detections",        "saltybot_social_msgs.msg.FaceDetectionArray", 10.0, 5.0),
    ("fused_target",       "/social/tracking/fused_target",   "saltybot_social_msgs.msg.FusedTarget",        8.0,  5.0),
    ("orchestrator_state", "/social/orchestrator/state",      "std_msgs.msg.String",            1.0,  3.0),
    ("persons",            "/social/persons",                 "saltybot_social_msgs.msg.PersonStateArray",  3.0,  5.0),
]

MEASUREMENT_WINDOW_S = 3.0


def _import_msg_type(path: str):
    """Dynamically import a message class from dotted path like 'std_msgs.msg.String'."""
    parts = path.rsplit(".", 1)
    module = __import__(parts[0], fromlist=[parts[1]])
    return getattr(module, parts[1])


class TestTopicRates(unittest.TestCase):
    """Verify all social-bot topics publish at ≥ minimum required rate."""

    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("topic_rate_test_probe")
        cls.checkers: dict = {}

        # Create all rate checkers
        for spec_id, topic, msg_path, min_hz, warm_up in RATE_SPECS:
            try:
                msg_type = _import_msg_type(msg_path)
                cls.checkers[spec_id] = TopicRateChecker(
                    cls.node, topic, msg_type, window_s=MEASUREMENT_WINDOW_S
                )
            except Exception as e:
                print(f"[WARN] Could not create checker for {spec_id}: {e}")

    @classmethod
    def tearDownClass(cls):
        cls.node.destroy_node()
        rclpy.shutdown()

    def _spin_for(self, duration_s: float) -> None:
        deadline = time.time() + duration_s
        while time.time() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.05)

    def _check_rate(self, spec_id: str, min_hz: float, warm_up_s: float) -> None:
        """Generic rate check: warm up, measure, assert."""
        checker = self.checkers.get(spec_id)
        if checker is None:
            self.skipTest(f"Checker for {spec_id} not available (missing msg type)")

        # Warm-up period
        self._spin_for(warm_up_s)

        # Measurement window
        self._spin_for(MEASUREMENT_WINDOW_S + 0.5)

        hz = checker.measured_hz()
        count = checker.message_count()

        self.assertGreater(
            hz, min_hz * 0.7,  # 30% tolerance for scheduling jitter
            f"{spec_id}: measured {hz:.2f} Hz < {min_hz:.1f} Hz minimum "
            f"(received {count} messages total)"
        )

    # ── Individual test methods (one per topic) ────────────────────────────────

    def test_camera_image_rate(self):
        """Mock camera must publish at ≥12Hz."""
        self._check_rate("camera_image", 12.0, warm_up_s=2.0)

    def test_uwb_target_rate(self):
        """Mock UWB must publish at ≥8Hz."""
        self._check_rate("uwb_target", 8.0, warm_up_s=2.0)

    def test_face_detection_rate(self):
        """Face detection must publish at ≥10Hz given 15Hz camera input."""
        self._check_rate("faces_detections", 10.0, warm_up_s=5.0)

    def test_tracking_fusion_rate(self):
        """Tracking fusion must publish fused_target at ≥8Hz."""
        self._check_rate("fused_target", 8.0, warm_up_s=5.0)

    def test_orchestrator_state_rate(self):
        """Orchestrator must publish state at ≥1Hz (config: 2Hz)."""
        self._check_rate("orchestrator_state", 1.0, warm_up_s=3.0)

    def test_persons_rate(self):
        """Person state tracker must publish at ≥3Hz."""
        self._check_rate("persons", 3.0, warm_up_s=5.0)


class TestTopicPresence(unittest.TestCase):
    """Verify all expected topics exist in the topic graph (not rate)."""

    EXPECTED_TOPICS = [
        "/social/orchestrator/state",
        "/social/faces/detections",
        "/social/faces/embeddings",
        "/social/tracking/fused_target",
        "/social/persons",
        "/social/speech/vad_state",
        "/social/speech/transcript",
        "/social/conversation/response",
        "/social/nav/mode",
        "/camera/color/image_raw",
        "/uwb/target",
        "/cmd_vel",
    ]

    @classmethod
    def setUpClass(cls):
        if not rclpy.ok():
            rclpy.init()
        cls.node = rclpy.create_node("topic_presence_test_probe")
        # Wait briefly for topics to register
        deadline = time.time() + 10.0
        while time.time() < deadline:
            rclpy.spin_once(cls.node, timeout_sec=0.2)
            known = {n for n, _ in cls.node.get_topic_names_and_types()}
            if "/social/orchestrator/state" in known:
                break

    @classmethod
    def tearDownClass(cls):
        cls.node.destroy_node()

    def test_all_expected_topics_advertised(self):
        """Every expected topic must appear in the topic graph."""
        known = {n for n, _ in self.node.get_topic_names_and_types()}
        missing = [t for t in self.EXPECTED_TOPICS if t not in known]
        self.assertEqual(
            missing, [],
            f"Topics not advertised: {missing}"
        )
