"""test_subsystems.py - Subsystem integration tests (sensors, perception, nav, comms)."""
import time
import unittest
import pytest
import rclpy
from sensor_msgs.msg import LaserScan, Image, Imu
from nav_msgs.msg import Odometry
from saltybot_tests.test_helpers import NodeChecker, TopicMonitor


class TestSensorHealth(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("sensor_health_test")
        cls.topic_monitor = TopicMonitor(cls.node)

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

    def test_lidar_health(self):
        """LIDAR must publish at ~10 Hz."""
        self.topic_monitor.subscribe_to_topic("/scan", LaserScan)
        deadline = time.time() + 2.0
        while time.time() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.1)
        count = self.topic_monitor.get_message_count("/scan")
        self.assertGreater(count, 10, f"LIDAR published {count} msgs in 2s (expected ~20)")

    def test_realsense_rgb_publishing(self):
        """RealSense RGB must publish frames."""
        self.topic_monitor.subscribe_to_topic("/camera/color/image_raw", Image)
        deadline = time.time() + 3.0
        while time.time() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.1)
        count = self.topic_monitor.get_message_count("/camera/color/image_raw")
        self.assertGreater(count, 0, "RealSense RGB not publishing")

    def test_imu_publishing(self):
        """IMU must publish continuously."""
        self.topic_monitor.subscribe_to_topic("/saltybot/imu", Imu)
        deadline = time.time() + 2.0
        while time.time() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.1)
        count = self.topic_monitor.get_message_count("/saltybot/imu")
        self.assertGreater(count, 0, "IMU not publishing")


class TestPerceptionPipeline(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("perception_test")

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

    def test_perception_node_alive(self):
        """YOLOv8 node must be alive."""
        node_names = [n for n, _ in self.node.get_node_names()]
        self.assertIn("yolo_node", node_names, "YOLOv8 perception node not found")

    def test_detection_topic_advertised(self):
        """Person detection topic must be advertised."""
        topics = {name for name, _ in self.node.get_topic_names_and_types()}
        self.assertIn("/person/detections", topics, "Person detection topic not advertised")


class TestNavigationStack(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("nav_test")
        cls.topic_monitor = TopicMonitor(cls.node)

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

    def test_odometry_continuity(self):
        """Odometry must publish continuously."""
        self.topic_monitor.subscribe_to_topic("/odom", Odometry)
        deadline = time.time() + 3.0
        while time.time() < deadline:
            rclpy.spin_once(self.node, timeout_sec=0.1)
        count = self.topic_monitor.get_message_count("/odom")
        self.assertGreater(count, 0, "Odometry not publishing")

    def test_tf_broadcasts(self):
        """TF must be active."""
        topics = {name for name, _ in self.node.get_topic_names_and_types()}
        self.assertIn("/tf", topics, "TF broadcaster not active")


class TestCommunication(unittest.TestCase):
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("comm_test")

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

    def test_rosbridge_node_alive(self):
        """Rosbridge WebSocket server must be running."""
        node_names = [n for n, _ in self.node.get_node_names()]
        self.assertIn("rosbridge_websocket", node_names, "Rosbridge not running")

    def test_key_topics_bridged(self):
        """Key topics must be available for bridging."""
        topics = {name for name, _ in self.node.get_topic_names_and_types()}
        missing = [t for t in ["/odom", "/scan", "/camera/color/image_raw", "/saltybot/imu"]
                   if t not in topics]
        self.assertEqual(missing, [], f"Topics not available for bridging: {missing}")
