"""test_launch.py - Launch integration test: full stack startup (Issue #504).

Uses launch_testing to verify all nodes launch, topics are advertised,
TF tree is complete, and system is stable for 30s.

Run with:
  ros2 launch launch_testing launch_test.py test/test_launch.py
"""
import os
import sys
import time
import unittest

import launch_testing
import launch_testing.actions
import launch_testing.markers
import pytest
import rclpy
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

sys.path.insert(0, os.path.dirname(os.path.dirname(__file__)))
from saltybot_tests.test_helpers import NodeChecker, TFChecker, TopicMonitor

REQUIRED_NODES = [
    "robot_description",
    "lidar_avoidance_node",
    "follower_node",
    "rosbridge_websocket",
]

REQUIRED_TOPICS = [
    "/scan",
    "/camera/color/image_raw",
    "/camera/depth/image_rect_raw",
    "/camera/imu",
    "/uwb/target",
    "/person/detections",
    "/saltybot/imu",
    "/odom",
    "/tf",
]

NODE_STARTUP_TIMEOUT_S = 30.0
STABILITY_CHECK_TIMEOUT_S = 10.0


@pytest.mark.launch_test
def generate_test_description():
    bringup_pkg = get_package_share_directory("saltybot_bringup")
    launch_file = os.path.join(bringup_pkg, "launch", "full_stack.launch.py")
    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(launch_file),
            launch_arguments={
                "mode": "follow",
                "enable_csi_cameras": "false",
                "enable_object_detection": "false",
                "enable_uwb": "true",
                "enable_perception": "true",
                "enable_follower": "true",
                "enable_bridge": "false",
                "enable_rosbridge": "true",
            }.items(),
        ),
        launch_testing.actions.ReadyToTest(),
    ])


class TestSaltyBotStackLaunch(unittest.TestCase):

    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("saltybot_stack_test_probe")
        cls.node_checker = NodeChecker(cls.node)
        cls.tf_checker = TFChecker(cls.node)
        cls.topic_monitor = TopicMonitor(cls.node)

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

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

    def test_01_critical_nodes_start(self):
        """All critical nodes must appear in graph within 30 seconds."""
        results = self.node_checker.wait_for_nodes(REQUIRED_NODES, timeout_s=NODE_STARTUP_TIMEOUT_S)
        missing = [n for n, found in results.items() if not found]
        self.assertEqual(missing, [], f"Nodes did not start: {missing}")

    def test_02_required_topics_advertised(self):
        """Key topics must be in topic graph."""
        self._spin(2.0)
        all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
        missing = [t for t in REQUIRED_TOPICS if t not in all_topics]
        self.assertEqual(missing, [], f"Topics not advertised: {missing}")

    def test_03_tf_tree_complete(self):
        """TF tree must have all critical links."""
        is_complete, missing = self.tf_checker.wait_for_tf_tree(
            timeout_s=NODE_STARTUP_TIMEOUT_S, spin_func=self._spin
        )
        self.assertTrue(is_complete, f"TF tree incomplete: {missing}")

    def test_04_odometry_publishing(self):
        """Odometry must publish within 5s."""
        from nav_msgs.msg import Odometry
        received = []
        sub = self.node.create_subscription(Odometry, "/odom", lambda m: received.append(m), 10)
        deadline = time.time() + 5.0
        while time.time() < deadline and not received:
            rclpy.spin_once(self.node, timeout_sec=0.2)
        self.node.destroy_subscription(sub)
        self.assertTrue(len(received) > 0, "Odometry (/odom) not publishing within 5s")

    def test_05_sensors_publishing(self):
        """Sensor topics must publish."""
        from sensor_msgs.msg import LaserScan, Image, Imu
        sensor_topics = {
            "/scan": (LaserScan, "RPLIDAR"),
            "/camera/color/image_raw": (Image, "RealSense RGB"),
            "/saltybot/imu": (Imu, "IMU"),
        }
        for topic_name, (msg_type, desc) in sensor_topics.items():
            received = []
            sub = self.node.create_subscription(msg_type, topic_name, lambda m: received.append(True), 10)
            deadline = time.time() + 5.0
            while time.time() < deadline and not received:
                rclpy.spin_once(self.node, timeout_sec=0.1)
            self.node.destroy_subscription(sub)
            self.assertTrue(len(received) > 0, f"{desc} ({topic_name}) not publishing within 5s")

    def test_06_no_immediate_crashes(self):
        """Nodes should still be alive 2 seconds after startup."""
        time.sleep(2.0)
        self._spin(1.0)
        results = self.node_checker.wait_for_nodes(REQUIRED_NODES, timeout_s=2.0)
        crashed = [n for n, found in results.items() if not found]
        self.assertEqual(crashed, [], f"Nodes crashed: {crashed}")

    def test_07_system_stability(self):
        """System must remain stable for 30 seconds."""
        print("[INFO] 30-second stability check...")
        deadline = time.time() + STABILITY_CHECK_TIMEOUT_S
        last_check = time.time()
        while time.time() < deadline:
            if time.time() - last_check >= 5.0:
                results = self.node_checker.wait_for_nodes(REQUIRED_NODES, timeout_s=2.0)
                crashed = [n for n, ok in results.items() if not ok]
                self.assertEqual(crashed, [], f"Nodes crashed during stability: {crashed}")
                last_check = time.time()
            rclpy.spin_once(self.node, timeout_sec=0.5)
        print("[INFO] Stability check passed")


@launch_testing.post_shutdown_test()
class TestSaltyBotShutdown(unittest.TestCase):
    def test_processes_exit_cleanly(self, proc_info):
        launch_testing.asserts.assertExitCodes(
            proc_info,
            allowable_exit_codes=[0, -15, launch_testing.asserts.EXIT_OK],
        )
