"""test_slam_nav2.py — Integration tests for SLAM and Nav2 navigation stack.

Verifies:
  - RTAB-Map SLAM starts and publishes map + odometry (indoor mode)
  - Slam Toolbox (nav2_slam) node appears in graph
  - Nav2 lifecycle nodes are active (planner, controller, behavior_server)
  - Nav2 compute_path_to_pose service is available
  - Map manager services (/mapping/maps/list, /saltybot/save_map)
  - TF tree includes map -> odom -> base_link chain (required for Nav2)

Run with:
  pytest test_slam_nav2.py -v -s
  (Requires full stack launched in indoor mode)
"""

import time
import unittest

import pytest
import rclpy
from nav_msgs.msg import OccupancyGrid, Odometry, Path
from geometry_msgs.msg import PoseStamped

from saltybot_tests.test_helpers import NodeChecker, TFChecker, TopicMonitor


# ── Expected nodes in indoor mode ─────────────────────────────────────────────

SLAM_NODES = [
    "rtabmap",              # RTAB-Map SLAM node
    "map_server",           # Nav2 map server
    "planner_server",       # Nav2 global planner
    "controller_server",    # Nav2 local controller (DWB/MPPI)
    "bt_navigator",         # Nav2 behavior tree navigator
    "behavior_server",      # Nav2 recovery behaviors
]

SLAM_TOPICS = [
    "/rtabmap/map",         # SLAM occupancy grid
    "/rtabmap/odom",        # SLAM visual odometry
    "/map",                 # Nav2 active map
    "/odom",                # Wheel/fused odometry
]

NAV2_SERVICES = [
    "/compute_path_to_pose",
    "/navigate_to_pose",
    "/clear_global_costmap",
    "/clear_local_costmap",
]

MAPPING_SERVICES = [
    "/mapping/maps/list",
    "/saltybot/save_map",
    "/saltybot/load_map",
    "/saltybot/list_maps",
]

NODE_TIMEOUT_S = 45.0   # SLAM is slow to start (needs sensors)
SERVICE_TIMEOUT_S = 60.0  # Nav2 lifecycle takes ~30-40s


class TestSLAMStartup(unittest.TestCase):
    """Verify RTAB-Map SLAM launches and produces map data."""

    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("slam_test_probe")
        cls.node_checker = NodeChecker(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_rtabmap_node_alive(self):
        """RTAB-Map node must appear within 45s (slow startup due to sensor init)."""
        results = self.node_checker.wait_for_nodes(["rtabmap"], timeout_s=NODE_TIMEOUT_S)
        self.assertTrue(
            results.get("rtabmap", False),
            f"RTAB-Map SLAM node did not start within {NODE_TIMEOUT_S}s"
        )

    def test_02_slam_topics_advertised(self):
        """SLAM must advertise map and odometry topics."""
        self._spin(2.0)
        all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
        missing = [t for t in SLAM_TOPICS if t not in all_topics]
        self.assertEqual(
            missing, [],
            f"SLAM topics not advertised: {missing}\nSeen: {sorted(all_topics)}"
        )

    def test_03_rtabmap_map_publishes(self):
        """RTAB-Map must publish at least one occupancy grid within 60s."""
        received = []

        def on_map(msg):
            received.append(msg)

        sub = self.node.create_subscription(OccupancyGrid, "/rtabmap/map", on_map, 1)
        deadline = time.time() + 60.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,
            "RTAB-Map did not publish an occupancy grid within 60s"
        )
        # Validate basic map properties
        grid = received[0]
        self.assertGreater(grid.info.width, 0, "Map width must be > 0")
        self.assertGreater(grid.info.height, 0, "Map height must be > 0")
        self.assertGreater(grid.info.resolution, 0.0, "Map resolution must be > 0")

    def test_04_slam_odometry_publishes(self):
        """SLAM odometry (/rtabmap/odom) must publish continuously."""
        self.topic_monitor.subscribe_to_topic("/rtabmap/odom", Odometry)

        self._spin(3.0)

        count = self.topic_monitor.get_message_count("/rtabmap/odom")
        self.assertGreater(
            count, 0,
            "SLAM odometry (/rtabmap/odom) not publishing — RTAB-Map may have crashed"
        )

    def test_05_tf_map_to_odom_chain(self):
        """TF chain map -> odom -> base_link must be complete for Nav2."""
        tf_checker = TFChecker(self.node)
        is_complete, missing = tf_checker.wait_for_tf_tree(
            timeout_s=45.0,
            spin_func=self._spin,
        )
        self.assertTrue(
            is_complete,
            f"TF map->odom->base_link chain incomplete: {missing}"
        )


class TestNav2Stack(unittest.TestCase):
    """Verify Nav2 navigation stack starts correctly."""

    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("nav2_test_probe")
        cls.node_checker = NodeChecker(cls.node)

    @classmethod
    def tearDownClass(cls):
        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_nav2_lifecycle_nodes_active(self):
        """Nav2 lifecycle nodes (planner, controller, bt_navigator) must be alive."""
        results = self.node_checker.wait_for_nodes(SLAM_NODES, timeout_s=SERVICE_TIMEOUT_S)
        missing = [n for n, found in results.items() if not found]
        self.assertEqual(
            missing, [],
            f"Nav2 lifecycle nodes not active within {SERVICE_TIMEOUT_S}s: {missing}"
        )

    def test_02_nav2_services_available(self):
        """Nav2 compute_path_to_pose and navigate_to_pose services must exist."""
        self._spin(2.0)
        all_services = {name for name, _ in self.node.get_service_names_and_types()}
        # Check at least compute_path_to_pose is available (most basic Nav2 service)
        self.assertIn(
            "/compute_path_to_pose",
            all_services,
            "Nav2 compute_path_to_pose service not available — Nav2 stack may not be active"
        )

    def test_03_costmap_topics_advertised(self):
        """Nav2 costmaps (global + local) must be publishing."""
        self._spin(3.0)
        all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
        costmap_topics = [
            "/global_costmap/costmap",
            "/local_costmap/costmap",
        ]
        missing = [t for t in costmap_topics if t not in all_topics]
        self.assertEqual(
            missing, [],
            f"Nav2 costmap topics not advertised: {missing}"
        )

    def test_04_nav2_goal_pose_topic_exists(self):
        """Nav2 must expose /goal_pose for manual navigation."""
        self._spin(1.0)
        all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
        self.assertIn(
            "/goal_pose",
            all_topics,
            "Nav2 goal_pose topic not advertised"
        )

    def test_05_nav2_path_plan_topic(self):
        """Nav2 must advertise /plan for path visualization."""
        self._spin(2.0)
        all_topics = {name for name, _ in self.node.get_topic_names_and_types()}
        self.assertIn(
            "/plan",
            all_topics,
            "Nav2 global planner /plan topic not advertised"
        )


class TestMapManagerServices(unittest.TestCase):
    """Verify map manager persistence services are available."""

    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("map_manager_test_probe")

    @classmethod
    def tearDownClass(cls):
        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_mapping_services_available(self):
        """Map manager services must be available (/mapping/maps/list, /saltybot/save_map)."""
        self._spin(3.0)
        all_services = {name for name, _ in self.node.get_service_names_and_types()}
        # Require at least the save/load map services
        key_services = ["/saltybot/save_map", "/saltybot/list_maps"]
        missing = [s for s in key_services if s not in all_services]
        self.assertEqual(
            missing, [],
            f"Map manager services not available: {missing}"
        )

    def test_02_slam_toolbox_services(self):
        """Slam Toolbox must expose its serialize/deserialize map services."""
        self._spin(3.0)
        all_services = {name for name, _ in self.node.get_service_names_and_types()}
        # Slam Toolbox core service (used for map persistence)
        slam_services = [
            "/slam_toolbox/serialize_map",
            "/slam_toolbox/deserialize_map",
        ]
        missing = [s for s in slam_services if s not in all_services]
        if missing:
            pytest.skip(f"Slam Toolbox services not available (using RTAB-Map instead): {missing}")


if __name__ == "__main__":
    pytest.main([__file__, "-v", "--tb=short"])
