#!/usr/bin/env python3
"""
export_map.py — CLI tool to export RTAB-Map data to standard formats.

Calls the running map_manager_node services to trigger export.

Usage:
    # Export 2D occupancy grid (PGM + YAML)
    ros2 run saltybot_mapping export_map -- --occupancy --name home --output /tmp

    # Export 3D point cloud (PLY)
    ros2 run saltybot_mapping export_map -- --pointcloud --name home --format ply --output /tmp

    # Both
    ros2 run saltybot_mapping export_map -- --occupancy --pointcloud --name home
"""

import argparse
import sys
import rclpy
from rclpy.node import Node
from saltybot_mapping.srv import ExportOccupancy, ExportPointCloud


class ExportCLI(Node):
    def __init__(self):
        super().__init__('export_map_cli')
        self._occ_client   = self.create_client(ExportOccupancy,  '/mapping/export/occupancy')
        self._cloud_client = self.create_client(ExportPointCloud, '/mapping/export/pointcloud')

    def export_occupancy(self, output_dir: str, map_name: str) -> bool:
        if not self._occ_client.wait_for_service(timeout_sec=5.0):
            print('[ERROR] /mapping/export/occupancy service not available')
            return False
        req = ExportOccupancy.Request()
        req.output_dir = output_dir
        req.map_name   = map_name
        fut = self._occ_client.call_async(req)
        rclpy.spin_until_future_complete(self, fut, timeout_sec=30.0)
        resp = fut.result()
        if resp and resp.success:
            print(f'[OK] Occupancy: {resp.pgm_path}')
            print(f'[OK]     YAML: {resp.yaml_path}')
            return True
        print(f'[FAIL] {resp.message if resp else "No response"}')
        return False

    def export_pointcloud(self, output_dir: str, map_name: str, fmt: str) -> bool:
        if not self._cloud_client.wait_for_service(timeout_sec=5.0):
            print('[ERROR] /mapping/export/pointcloud service not available')
            return False
        req = ExportPointCloud.Request()
        req.output_dir = output_dir
        req.map_name   = map_name
        req.format     = fmt
        fut = self._cloud_client.call_async(req)
        rclpy.spin_until_future_complete(self, fut, timeout_sec=60.0)
        resp = fut.result()
        if resp and resp.success:
            print(f'[OK] Point cloud: {resp.file_path}')
            return True
        print(f'[FAIL] {resp.message if resp else "No response"}')
        return False


def main():
    parser = argparse.ArgumentParser(description='Export RTAB-Map data')
    parser.add_argument('--occupancy',   action='store_true', help='Export 2D occupancy (PGM+YAML)')
    parser.add_argument('--pointcloud',  action='store_true', help='Export 3D point cloud')
    parser.add_argument('--name',        default='',          help='Base filename (default: auto timestamp)')
    parser.add_argument('--output',      default='',          help='Output directory (default: maps/exports)')
    parser.add_argument('--format',      default='ply',       choices=['ply', 'pcd'], help='Point cloud format')
    args, ros_args = parser.parse_known_args()

    if not args.occupancy and not args.pointcloud:
        parser.error('Specify --occupancy and/or --pointcloud')

    rclpy.init(args=ros_args)
    node = ExportCLI()
    ok = True
    if args.occupancy:
        ok &= node.export_occupancy(args.output, args.name)
    if args.pointcloud:
        ok &= node.export_pointcloud(args.output, args.name, args.format)
    node.destroy_node()
    rclpy.shutdown()
    sys.exit(0 if ok else 1)


if __name__ == '__main__':
    main()
