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

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

Examples:
  scripts/go2_start_nav.sh
  scripts/go2_start_nav.sh competition_map
  scripts/go2_start_nav.sh ~/go2_maps/final_site.yaml

Starts Nav2 with the specified map. In RViz, set 2D Pose Estimate first, then
send a Nav2 Goal.
GO2_OS_USE_RVIZ=auto|true|false overrides RViz launch (default: auto).
EOF
}

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

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

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

if pgrep -f 'ros2 launch go2_navigation2 go2_nav2.launch.py' >/dev/null; then
  printf '[go2_start_nav] ERROR: Nav2 already appears to be running.\n' >&2
  pgrep -af 'ros2 launch go2_navigation2 go2_nav2.launch.py' >&2 || true
  exit 4
fi

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

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

if [[ "${map_arg}" == *.yaml ]]; then
  map_yaml="${map_arg}"
elif [[ "${map_arg}" == /* || "${map_arg}" == ~* || "${map_arg}" == *"/"* ]]; then
  map_yaml="${map_arg}.yaml"
else
  map_yaml="${GO2_OS_MAP_DIR}/${map_arg}.yaml"
fi

if [[ ! -f "${map_yaml}" ]]; then
  printf '[go2_start_nav] ERROR: map yaml not found: %s\n' "${map_yaml}" >&2
  exit 5
fi

rviz_mode="${GO2_OS_USE_RVIZ:-auto}"
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_nav] ERROR: invalid GO2_OS_USE_RVIZ=%s (expected auto/true/false)\n' "${GO2_OS_USE_RVIZ:-}" >&2
    exit 6
    ;;
esac

cd "${repo_root}"
printf '[go2_start_nav] policy=%s iface=%s reason=%s rviz=%s (%s)\n' "${GO2_TRANSPORT_POLICY:-nav}" "${GO2_ROS_IFACE}" "${GO2_OS_SELECTED_IFACE_REASON:-n/a}" "${use_rviz}" "${rviz_reason}"
if [[ "${GO2_OS_FALLBACK_ACTIVE:-0}" == "1" ]]; then
  printf '[go2_start_nav] fallback active: wired backup in use\n'
fi
printf '[go2_start_nav] starting Nav2 on LAN iface %s with map: %s\n' "${GO2_ROS_IFACE}" "${map_yaml}"
exec ros2 launch go2_navigation2 go2_nav2.launch.py map:="${map_yaml}" use_rviz:="${use_rviz}"