#!/usr/bin/env bash
set -euo pipefail

usage() {
  cat <<'EOF'
Usage:
  scripts/go2_save_map.sh [map_name_or_prefix]

Examples:
  scripts/go2_save_map.sh
  scripts/go2_save_map.sh competition_map
  scripts/go2_save_map.sh ~/go2_maps/final_site

Saves the active /map as <prefix>.yaml and <prefix>.pgm.
EOF
}

if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then
  usage
  exit 0
fi

script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
export GO2_OS_ROLE=save_map
unset GO2_ROS_IFACE

# shellcheck disable=SC1091
source "${script_dir}/go2_env.sh"

map_arg="${1:-${GO2_OS_MAP_NAME}}"
map_arg="${map_arg%.yaml}"
map_arg="${map_arg%.pgm}"
if [[ "${map_arg}" == "~" ]]; then
  map_arg="${HOME}"
elif [[ "${map_arg}" == "~/"* ]]; then
  map_arg="${HOME}/${map_arg#"~/"}"
fi

if [[ "${map_arg}" == /* || "${map_arg}" == ~* || "${map_arg}" == *"/"* ]]; then
  map_prefix="${map_arg}"
else
  map_prefix="${GO2_OS_MAP_DIR}/${map_arg}"
fi

mkdir -p "$(dirname "${map_prefix}")"

printf '[go2_save_map] policy=%s iface=%s reason=%s\n' "${GO2_TRANSPORT_POLICY:-save_map}" "${GO2_ROS_IFACE}" "${GO2_OS_SELECTED_IFACE_REASON:-n/a}"
printf '[go2_save_map] saving /map from wired iface %s to %s.{yaml,pgm}\n' "${GO2_ROS_IFACE}" "${map_prefix}"
ros2 run nav2_map_server map_saver_cli -f "${map_prefix}"
printf '[go2_save_map] saved:\n  %s.yaml\n  %s.pgm\n' "${map_prefix}" "${map_prefix}"