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

usage() {
  cat <<'EOF'
Usage:
  scripts/go2_start_mapping.sh [--rviz|--no-rviz]

Starts SLAM mapping on the wired Go2 interface.

Defaults:
  - RViz: auto; enabled only when DISPLAY/WAYLAND_DISPLAY exists

Examples:
  scripts/go2_start_mapping.sh
  scripts/go2_start_mapping.sh --rviz
  scripts/go2_start_mapping.sh --no-rviz
EOF
}

rviz_mode="${GO2_OS_USE_RVIZ:-auto}"
for arg in "$@"; do
  case "$arg" in
    --rviz) rviz_mode=true ;;
    --no-rviz|--headless) rviz_mode=false ;;
    -h|--help) usage; exit 0 ;;
    *) printf '[go2_start_mapping] unknown argument: %s
' "$arg" >&2; usage; exit 2 ;;
  esac
done

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

if pgrep -f 'ros2 launch go2_navigation2 go2_nav2.launch.py' >/dev/null; then
  printf '[go2_start_mapping] ERROR: Nav2 appears to be running. Stop navigation before mapping.
' >&2
  pgrep -af 'ros2 launch go2_navigation2 go2_nav2.launch.py' >&2 || true
  exit 3
fi

if pgrep -f 'ros2 launch go2_core go2_start.launch.py' >/dev/null; then
  printf '[go2_start_mapping] ERROR: mapping already appears to be running. Stop it first:
' >&2
  printf '  scripts/go2_stop_mapping.sh
' >&2
  pgrep -af 'ros2 launch go2_core go2_start.launch.py' >&2 || true
  exit 4
fi


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

case "$(printf '%s' "${rviz_mode}" | tr '[:upper:]' '[:lower:]')" in
  auto)
    if [[ -n "${DISPLAY:-}" || -n "${WAYLAND_DISPLAY:-}" ]]; then
      use_rviz=true
      rviz_reason="auto:display-detected"
    else
      use_rviz=false
      rviz_reason="auto:no-display-detected"
    fi
    ;;
  1|true|yes|on)
    use_rviz=true
    rviz_reason="forced:true"
    ;;
  0|false|no|off)
    use_rviz=false
    rviz_reason="forced:false"
    ;;
  *)
    printf '[go2_start_mapping] ERROR: invalid RViz mode: %s (expected auto/true/false)
' "${rviz_mode}" >&2
    exit 6
    ;;
esac

cd "${repo_root}"
printf '[go2_start_mapping] policy=%s iface=%s reason=%s rviz=%s (%s)
' "${GO2_TRANSPORT_POLICY:-mapping}" "${GO2_ROS_IFACE}" "${GO2_OS_SELECTED_IFACE_REASON:-n/a}" "${use_rviz}" "${rviz_reason}"
printf '[go2_start_mapping] starting SLAM mapping on wired iface %s
' "${GO2_ROS_IFACE}"
printf '[go2_start_mapping] keep this terminal open while mapping. Press Ctrl+C to stop after saving the map.
'

# 清除 DDS 缓存，避免历史地图残留
rm -rf /dev/shm/fastrtps_* /dev/shm/cyclonedds_* /tmp/cyclonedds_* 2>/dev/null || true
printf '[go2_start_mapping] DDS cache cleared — clean start\n'

exec ros2 launch go2_core go2_start.launch.py use_rviz:="${use_rviz}"