"""
map_manager_node.py — RTAB-Map persistence and map management node.

Responsibilities:
  1. Auto-save current.db every N minutes (configurable, default 5 min)
  2. Prune old autosaves (keep_autosaves_n, default 5)
  3. Map management services: list / save_as / load / delete
  4. Export services: 2D occupancy (PGM+YAML) / 3D point cloud (PLY/PCD)
  5. Publish /mapping/status (map size, last save time, keyframe count)

Map lifecycle with RTAB-Map:
  - RTAB-Map writes continuously to current.db (configured via database_path param)
  - On startup: if current.db exists and fresh_start=false → RTAB-Map relocates
  - This node periodically copies current.db → autosave_YYYYMMDD_HHMMSS.db
  - load_map copies a named .db → current.db then signals for restart

ROS2 services:
  /mapping/maps/list         (saltybot_mapping/ListMaps)
  /mapping/maps/save_as      (saltybot_mapping/SaveMap)
  /mapping/maps/load         (saltybot_mapping/LoadMap)
  /mapping/maps/delete       (saltybot_mapping/DeleteMap)
  /mapping/export/occupancy  (saltybot_mapping/ExportOccupancy)
  /mapping/export/pointcloud (saltybot_mapping/ExportPointCloud)

ROS2 topics published:
  /mapping/status            (std_msgs/String, JSON, 1 Hz)

ROS2 topics subscribed:
  /rtabmap/map               (nav_msgs/OccupancyGrid, latched) — for export
  /rtabmap/cloud_map         (sensor_msgs/PointCloud2, latched) — for export
  /rtabmap/info              (rtabmap_msgs/Info) — for keyframe count
"""

import json
import time
import os
from pathlib import Path
from datetime import datetime

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from std_msgs.msg import String
from nav_msgs.msg import OccupancyGrid
from sensor_msgs.msg import PointCloud2

from saltybot_mapping.srv import (
    ListMaps, SaveMap, LoadMap, DeleteMap,
    ExportOccupancy, ExportPointCloud,
)
from .map_database import MapDatabase
from .map_exporter import write_occupancy_export, write_pointcloud_export


class MapManagerNode(Node):

    def __init__(self):
        super().__init__('map_manager')

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter('maps_dir',           '/mnt/nvme/saltybot/maps')
        self.declare_parameter('exports_dir',        '/mnt/nvme/saltybot/maps/exports')
        self.declare_parameter('autosave_interval',  300.0)  # seconds (5 min)
        self.declare_parameter('keep_autosaves_n',   5)
        self.declare_parameter('max_db_size_mb',     2048)   # warn if current.db > 2GB

        maps_dir       = self.get_parameter('maps_dir').value
        self._exports_dir = self.get_parameter('exports_dir').value
        self._autosave_interval = self.get_parameter('autosave_interval').value
        self._keep_autosaves  = self.get_parameter('keep_autosaves_n').value
        self._max_db_mb       = self.get_parameter('max_db_size_mb').value

        self._db = MapDatabase(maps_dir)
        self._last_autosave: float = time.monotonic()
        self._last_occ_msg: OccupancyGrid | None = None
        self._last_cloud_msg: PointCloud2 | None = None
        self._keyframe_count: int = 0

        # ── Subscriptions ────────────────────────────────────────────────────
        transient_local = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )
        best_effort = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        self.create_subscription(
            OccupancyGrid, '/rtabmap/map',
            self._on_occupancy_grid, transient_local)
        self.create_subscription(
            PointCloud2, '/rtabmap/cloud_map',
            self._on_cloud_map, best_effort)

        # ── Publishers ───────────────────────────────────────────────────────
        self._pub_status = self.create_publisher(String, '/mapping/status', 10)

        # ── Services ─────────────────────────────────────────────────────────
        self.create_service(ListMaps,        '/mapping/maps/list',         self._handle_list)
        self.create_service(SaveMap,         '/mapping/maps/save_as',      self._handle_save_as)
        self.create_service(LoadMap,         '/mapping/maps/load',         self._handle_load)
        self.create_service(DeleteMap,       '/mapping/maps/delete',       self._handle_delete)
        self.create_service(ExportOccupancy, '/mapping/export/occupancy',  self._handle_export_occ)
        self.create_service(ExportPointCloud,'/mapping/export/pointcloud', self._handle_export_cloud)

        # ── Timers ───────────────────────────────────────────────────────────
        self.create_timer(1.0, self._status_tick)
        self.create_timer(self._autosave_interval, self._autosave_tick)

        self.get_logger().info(
            f'map_manager ready. maps_dir={maps_dir}, '
            f'autosave every {self._autosave_interval:.0f}s'
        )

    # ── Subscription callbacks ─────────────────────────────────────────────

    def _on_occupancy_grid(self, msg: OccupancyGrid) -> None:
        self._last_occ_msg = msg

    def _on_cloud_map(self, msg: PointCloud2) -> None:
        self._last_cloud_msg = msg

    # ── Status tick ───────────────────────────────────────────────────────

    def _status_tick(self) -> None:
        current_db = Path(self._db.current_db_path())
        db_size_mb = (current_db.stat().st_size / 1024 / 1024
                      if current_db.exists() else 0.0)

        if db_size_mb > self._max_db_mb:
            self.get_logger().warn(
                f'current.db size {db_size_mb:.0f} MB exceeds limit '
                f'{self._max_db_mb} MB — consider pruning keyframes.',
                throttle_duration_sec=60.0,
            )

        status = {
            'db_size_mb':       round(db_size_mb, 1),
            'maps_count':       len(self._db.list_maps()),
            'has_occupancy':    self._last_occ_msg is not None,
            'has_cloud':        self._last_cloud_msg is not None,
            'next_autosave_s':  max(0, round(
                self._autosave_interval - (time.monotonic() - self._last_autosave))),
            'timestamp':        datetime.now().isoformat(),
        }
        msg = String()
        msg.data = json.dumps(status)
        self._pub_status.publish(msg)

    # ── Autosave tick ─────────────────────────────────────────────────────

    def _autosave_tick(self) -> None:
        self._last_autosave = time.monotonic()
        name = self._db.auto_timestamped_name()
        ok, msg, path = self._db.save_as(name, overwrite=False)
        if ok:
            deleted = self._db.prune_autosaves(self._keep_autosaves)
            self.get_logger().info(
                f'Autosave: {path} '
                f'(pruned {len(deleted)} old autosave(s))'
            )
        else:
            self.get_logger().warn(f'Autosave skipped: {msg}')

    # ── Service handlers ──────────────────────────────────────────────────

    def _handle_list(self, request: ListMaps.Request,
                     response: ListMaps.Response) -> ListMaps.Response:
        maps = self._db.list_maps()
        response.map_names     = [m['name']          for m in maps]
        response.map_paths     = [m['path']          for m in maps]
        response.sizes_bytes   = [m['size_bytes']    for m in maps]
        response.modified_times= [m['modified_time'] for m in maps]
        response.count         = len(maps)
        return response

    def _handle_save_as(self, request: SaveMap.Request,
                        response: SaveMap.Response) -> SaveMap.Response:
        if not request.map_name:
            request.map_name = self._db.auto_timestamped_name()
        ok, msg, path = self._db.save_as(request.map_name, request.overwrite)
        response.success    = ok
        response.message    = msg
        response.saved_path = path
        if ok:
            self.get_logger().info(f'Map saved: {path}')
        else:
            self.get_logger().warn(f'Save failed: {msg}')
        return response

    def _handle_load(self, request: LoadMap.Request,
                     response: LoadMap.Response) -> LoadMap.Response:
        ok, msg, path = self._db.load_map(request.map_name)
        response.success     = ok
        response.message     = msg
        response.loaded_path = path
        if ok:
            self.get_logger().info(f'Map loaded: {path}. Restart rtabmap to activate.')
        else:
            self.get_logger().warn(f'Load failed: {msg}')
        return response

    def _handle_delete(self, request: DeleteMap.Request,
                       response: DeleteMap.Response) -> DeleteMap.Response:
        ok, msg = self._db.delete_map(request.map_name)
        response.success = ok
        response.message = msg
        if ok:
            self.get_logger().info(f'Map deleted: {request.map_name}')
        else:
            self.get_logger().warn(f'Delete failed: {msg}')
        return response

    def _handle_export_occ(self, request: ExportOccupancy.Request,
                           response: ExportOccupancy.Response) -> ExportOccupancy.Response:
        if self._last_occ_msg is None:
            response.success = False
            response.message = 'No occupancy grid received yet from /rtabmap/map'
            return response

        out_dir = request.output_dir or self._exports_dir
        map_name = request.map_name or ''
        try:
            pgm, yaml = write_occupancy_export(
                self._last_occ_msg, out_dir, map_name)
            response.success  = True
            response.pgm_path = pgm
            response.yaml_path= yaml
            response.message  = f'Exported {pgm} ({Path(pgm).stat().st_size // 1024} KB)'
            self.get_logger().info(response.message)
        except Exception as e:
            response.success = False
            response.message = f'Export failed: {e}'
            self.get_logger().error(response.message)
        return response

    def _handle_export_cloud(self, request: ExportPointCloud.Request,
                             response: ExportPointCloud.Response) -> ExportPointCloud.Response:
        if self._last_cloud_msg is None:
            response.success = False
            response.message = 'No point cloud received yet from /rtabmap/cloud_map'
            return response

        out_dir = request.output_dir or self._exports_dir
        fmt = request.format if request.format in ('ply', 'pcd') else 'ply'
        map_name = request.map_name or ''
        try:
            path = write_pointcloud_export(
                self._last_cloud_msg, out_dir, map_name, fmt)
            response.success   = True
            response.file_path = path
            response.message   = f'Exported {path} ({Path(path).stat().st_size // 1024} KB)'
            self.get_logger().info(response.message)
        except Exception as e:
            response.success = False
            response.message = f'Export failed: {e}'
            self.get_logger().error(response.message)
        return response


# ── Entry point ───────────────────────────────────────────────────────────────

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


if __name__ == '__main__':
    main()
