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

# One-command runner for:
#   Terminal A: Gazebo Sim + bridge (+ optional SLAM + RViz)
#   Terminal B: Nav2 stack
#
# Modes:
#   mapping       -> online SLAM mapping (Localization in RViz is expected to stay inactive)
#   localization  -> map-based localization/navigation (Localization becomes active)
#
# Examples:
#   ./run_ab.sh mapping
#   NAV2_MAP_PATH=/abs/path/office_map.yaml ./run_ab.sh localization

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
MODE="${1:-mapping}"
ROBOT_MODEL="${ROBOT_MODEL:-waffle}"
GZ_PARTITION="${GZ_PARTITION:-tb3_office_gz}"
USE_SIM_TIME="${USE_SIM_TIME:-true}"
SLEEP_NAV2="${SLEEP_NAV2:-18}"
WORLD_PATH="${WORLD_PATH:-${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/worlds/office_gz_dartsim.sdf}"
NAV2_MAP_PATH="${NAV2_MAP_PATH:-${ROOT}/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml}"

set +u
source /opt/ros/humble/setup.bash
source "${ROOT}/install/setup.bash"
set -u
export TURTLEBOT3_MODEL="${ROBOT_MODEL}"

declare -a CMD_A
declare -a CMD_B

if [[ "${MODE}" == "mapping" ]]; then
  CMD_A=(
    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py
    "robot_model:=${ROBOT_MODEL}"
    "use_sim_time:=${USE_SIM_TIME}"
    start_slam:=true
    show_rviz:=true
    "gz_partition:=${GZ_PARTITION}"
    gz_world_name:=default
    "world:=${WORLD_PATH}"
  )
  CMD_B=(
    ros2 launch nav2_bringup navigation_launch.py
    "use_sim_time:=${USE_SIM_TIME}"
  )
elif [[ "${MODE}" == "localization" ]]; then
  if [[ ! -f "${NAV2_MAP_PATH}" ]]; then
    echo "[run_ab] ERROR: map yaml not found: ${NAV2_MAP_PATH}" >&2
    echo "[run_ab] Hint: save map first with map_saver_cli." >&2
    exit 1
  fi
  CMD_A=(
    ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py
    "robot_model:=${ROBOT_MODEL}"
    "use_sim_time:=${USE_SIM_TIME}"
    start_slam:=false
    show_rviz:=true
    "gz_partition:=${GZ_PARTITION}"
    gz_world_name:=default
    "world:=${WORLD_PATH}"
  )
  CMD_B=(
    ros2 launch nav2_bringup bringup_launch.py
    "use_sim_time:=${USE_SIM_TIME}"
    "map:=${NAV2_MAP_PATH}"
    slam:=false
  )
else
  echo "[run_ab] ERROR: unknown mode '${MODE}' (use: mapping | localization)" >&2
  exit 1
fi

LOG_DIR="${ROOT}/logs/run_ab_$(date +%Y%m%d_%H%M%S)"
mkdir -p "${LOG_DIR}"

echo "[run_ab] mode=${MODE}"
echo "[run_ab] logs at: ${LOG_DIR}"
echo "[run_ab] starting Terminal A..."
"${CMD_A[@]}" >"${LOG_DIR}/A_sim.log" 2>&1 &
PID_A=$!

sleep "${SLEEP_NAV2}"

echo "[run_ab] starting Terminal B..."
"${CMD_B[@]}" >"${LOG_DIR}/B_nav2.log" 2>&1 &
PID_B=$!

cleanup() {
  echo "[run_ab] stopping..."
  kill "${PID_B}" 2>/dev/null || true
  kill "${PID_A}" 2>/dev/null || true
}
trap cleanup EXIT INT TERM

echo "[run_ab] running. Press Ctrl+C to stop both."
wait "${PID_A}" "${PID_B}"