#!/usr/bin/env python3
"""
map_saver_node.py — Map persistence helper for SLAM → AMCL hand-off (Issue #696).

Responsibilities:
  1. On startup: report whether a saved map exists at map_dir/map_name.yaml
     (the path that nav2_amcl_bringup.launch.py will load on next boot).
  2. Provide /saltybot/save_map (std_srvs/Trigger) — triggers map_saver_cli
     to save the current /map topic to map_dir/map_name.{yaml,pgm}.
  3. On shutdown: if auto_save_on_shutdown is true, save the final map.

Parameters:
  map_dir              (str)  default /mnt/nvme/saltybot/maps
  map_name             (str)  default saltybot_map
  auto_save_on_shutdown (bool) default true

The saved map is loaded automatically by nav2_amcl_bringup.launch.py on the
next startup when the file is present at map_dir/map_name.yaml.
"""

import os
import subprocess

import rclpy
from rclpy.node import Node
from std_srvs.srv import Trigger


class MapSaverNode(Node):
    """Helper node for SLAM map persistence and AMCL pre-load."""

    def __init__(self) -> None:
        super().__init__("saltybot_map_saver")

        self.declare_parameter("map_dir",              "/mnt/nvme/saltybot/maps")
        self.declare_parameter("map_name",             "saltybot_map")
        self.declare_parameter("auto_save_on_shutdown", True)

        self._map_dir   = self.get_parameter("map_dir").value
        self._map_name  = self.get_parameter("map_name").value
        self._auto_save = self.get_parameter("auto_save_on_shutdown").value

        self._map_prefix = os.path.join(self._map_dir, self._map_name)
        self._map_yaml   = self._map_prefix + ".yaml"

        # ── Ensure map directory exists ────────────────────────────────────────
        os.makedirs(self._map_dir, exist_ok=True)

        # ── Report saved map status ────────────────────────────────────────────
        if os.path.isfile(self._map_yaml):
            self.get_logger().info(
                f"[map_saver] Saved map found: {self._map_yaml} — "
                "AMCL will load this map on next startup."
            )
        else:
            self.get_logger().warn(
                f"[map_saver] No saved map at {self._map_yaml} — "
                "AMCL will use the placeholder map. "
                "Run SLAM, then call /saltybot/save_map to persist."
            )

        # ── Service: /saltybot/save_map ────────────────────────────────────────
        self._save_srv = self.create_service(
            Trigger,
            "/saltybot/save_map",
            self._on_save_map,
        )

        self.get_logger().info(
            f"[map_saver] Ready. map_dir={self._map_dir} map_name={self._map_name} "
            f"auto_save_on_shutdown={self._auto_save}"
        )

    # ── Service callback ───────────────────────────────────────────────────────

    def _on_save_map(
        self, _request: Trigger.Request, response: Trigger.Response
    ) -> Trigger.Response:
        """Save current /map to map_dir/map_name.{yaml,pgm} via map_saver_cli."""
        success, message = self._save_map()
        response.success = success
        response.message = message
        return response

    # ── Map save helper ────────────────────────────────────────────────────────

    def _save_map(self) -> tuple[bool, str]:
        """
        Call nav2_map_server map_saver_cli to write the current /map.

        Returns (success, message).
        """
        cmd = [
            "ros2", "run", "nav2_map_server", "map_saver_cli",
            "-f", self._map_prefix,
            "--map_subscribe_transient_local",
        ]
        self.get_logger().info(f"[map_saver] Saving map to {self._map_prefix}.*")
        try:
            result = subprocess.run(
                cmd,
                timeout=10,
                capture_output=True,
                text=True,
            )
            if result.returncode == 0:
                msg = f"Map saved to {self._map_yaml}"
                self.get_logger().info(f"[map_saver] {msg}")
                return True, msg
            else:
                msg = f"map_saver_cli failed (rc={result.returncode}): {result.stderr.strip()}"
                self.get_logger().error(f"[map_saver] {msg}")
                return False, msg
        except subprocess.TimeoutExpired:
            msg = "map_saver_cli timed out after 10 s (is /map published?)"
            self.get_logger().error(f"[map_saver] {msg}")
            return False, msg
        except Exception as exc:  # noqa: BLE001
            msg = f"map_saver_cli exception: {exc}"
            self.get_logger().error(f"[map_saver] {msg}")
            return False, msg

    # ── Lifecycle ──────────────────────────────────────────────────────────────

    def destroy_node(self) -> None:
        if self._auto_save:
            self.get_logger().info("[map_saver] Shutdown — auto-saving map...")
            self._save_map()
        super().destroy_node()


def main(args=None) -> None:
    rclpy.init(args=args)
    node = MapSaverNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
