#!/usr/bin/env bash
# Start only the master Orin Autoware launch for debugging, without preset-route UI.
#
# 用法:
#   ./run_ecar_master_debug.sh [地图目录] [是否启动RViz] [车型]
#
# 位置参数都可以省略:
#   地图目录:  $ECAR_MASTER_MAP_PATH
#              默认 /home/nvidia/FaRui-test/map/School06
#   RViz:      $ECAR_MASTER_RVIZ
#              默认 true，可选 true | false
#   车型:      $ECAR_MASTER_VEHICLE_MODEL
#              默认 ecar_vehicle
#
# 常用示例:
#   启动主板 Autoware，并打开 RViz:
#     ./run_ecar_master_debug.sh
#
#   启动主板 Autoware，但关闭 RViz:
#     ./run_ecar_master_debug.sh /home/nvidia/FaRui-test/map/School06 false
#
#   用环境变量指定地图:
#     ECAR_MASTER_MAP_PATH=/home/nvidia/FaRui-test/map/School06 ./run_ecar_master_debug.sh
#
# 双 Orin 说明:
#   本脚本只启动 src/Launcher/farui_launch/launch/ecar_master.launch.xml。
#   不启动 UI、预设路线工具、vehicle、control、system。
#   从板仍需单独启动 ecar_slave.launch.xml，负责底盘驱动、控制和 system 反馈。

set -euo pipefail

GREEN='\033[0;32m'
YELLOW='\033[1;33m'
RED='\033[0;31m'
NC='\033[0m'

log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }
log_error() { echo -e "${RED}[ERROR]${NC} $*" >&2; }

script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

find_workspace_root() {
  if [[ -n "${ECAR_MASTER_WORKSPACE_ROOT:-}" ]]; then
    echo "${ECAR_MASTER_WORKSPACE_ROOT}"
    return
  fi

  local candidates=(
    "${script_dir}"
    "${PWD}"
  )
  for candidate in "${candidates[@]}"; do
    if [[ -f "${candidate}/install/setup.bash" ]]; then
      (cd "${candidate}" && pwd)
      return
    fi
  done

  echo "${script_dir}"
}

WORKSPACE_ROOT="$(find_workspace_root)"
#DEFAULT_MAP_PATH="/home/nvidia/FaRui-test/map/School06" parker01
DEFAULT_MAP_PATH="/home/nvidia/FaRui-test/map/School06" 
DEFAULT_VEHICLE_MODEL="ecar_vehicle"
DEFAULT_LAUNCH_FILE="${WORKSPACE_ROOT}/src/Launcher/farui_launch/launch/ecar_master.launch.xml"
DEFAULT_OPERATION_MODE_FALLBACK_SCRIPT="${WORKSPACE_ROOT}/debug/operation_mode_state_fallback.py"

MAP_PATH="${1:-${ECAR_MASTER_MAP_PATH:-${DEFAULT_MAP_PATH}}}"
RVIZ="${2:-${ECAR_MASTER_RVIZ:-true}}"
VEHICLE_MODEL="${3:-${ECAR_MASTER_VEHICLE_MODEL:-${DEFAULT_VEHICLE_MODEL}}}"
LAUNCH_FILE="${ECAR_MASTER_LAUNCH_FILE:-${DEFAULT_LAUNCH_FILE}}"
OPERATION_MODE_FALLBACK="${ECAR_MASTER_OPERATION_MODE_FALLBACK:-false}"
OPERATION_MODE_FALLBACK_SCRIPT="${ECAR_MASTER_OPERATION_MODE_FALLBACK_SCRIPT:-${DEFAULT_OPERATION_MODE_FALLBACK_SCRIPT}}"

case "${RVIZ}" in
  true|false)
    ;;
  *)
    log_error "RViz must be true or false, got: ${RVIZ}"
    exit 1
    ;;
esac

case "${OPERATION_MODE_FALLBACK}" in
  true|false)
    ;;
  *)
    log_error "ECAR_MASTER_OPERATION_MODE_FALLBACK must be true or false, got: ${OPERATION_MODE_FALLBACK}"
    exit 1
    ;;
esac

if [[ ! -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
  log_error "Workspace setup file not found: ${WORKSPACE_ROOT}/install/setup.bash"
  exit 1
fi

if [[ ! -f "${LAUNCH_FILE}" ]]; then
  log_error "Master launch file not found: ${LAUNCH_FILE}"
  exit 1
fi

if [[ ! -d "${MAP_PATH}" ]]; then
  log_error "Map path not found: ${MAP_PATH}"
  exit 1
fi

if [[ "${OPERATION_MODE_FALLBACK}" == "true" && ! -f "${OPERATION_MODE_FALLBACK_SCRIPT}" ]]; then
  log_error "Operation mode fallback script not found: ${OPERATION_MODE_FALLBACK_SCRIPT}"
  exit 1
fi

log_info "Workspace: ${WORKSPACE_ROOT}"
log_info "Master launch: ${LAUNCH_FILE}"
log_info "Map path: ${MAP_PATH}"
log_info "Vehicle model: ${VEHICLE_MODEL}"
log_info "RViz: ${RVIZ}"
log_info "Operation mode fallback: ${OPERATION_MODE_FALLBACK}"
log_info "Starting master Autoware without UI."

cd "${WORKSPACE_ROOT}"
set +u
source install/setup.bash
set -u

log_ros_pkg_prefix() {
  local package="$1"
  local prefix
  if prefix="$(ros2 pkg prefix "${package}" 2>/dev/null)"; then
    log_info "ROS package ${package}: ${prefix}"
  else
    log_warn "ROS package ${package} not found in the sourced environment."
  fi
}

log_ros_pkg_prefix "farui_launch"
log_ros_pkg_prefix "autoware_behavior_path_planner"
log_ros_pkg_prefix "glog_component"

OPERATION_MODE_FALLBACK_PID=""
cleanup() {
  local status=$?
  trap - EXIT INT TERM
  if [[ -n "${OPERATION_MODE_FALLBACK_PID}" ]]; then
    kill "${OPERATION_MODE_FALLBACK_PID}" 2>/dev/null || true
    wait "${OPERATION_MODE_FALLBACK_PID}" 2>/dev/null || true
  fi
  exit "${status}"
}
trap cleanup EXIT INT TERM

if [[ "${OPERATION_MODE_FALLBACK}" == "true" ]]; then
  log_info "Starting planning operation mode fallback."
  python3 "${OPERATION_MODE_FALLBACK_SCRIPT}" &
  OPERATION_MODE_FALLBACK_PID=$!
fi

ros2 launch "${LAUNCH_FILE}" \
  map_path:="${MAP_PATH}" \
  vehicle_model:="${VEHICLE_MODEL}" \
  launch_planning:=true \
  launch_system:=false \
  launch_vehicle:=false \
  launch_control:=false \
  launch_api:=true \
  use_sim_time:=false \
  rviz:="${RVIZ}" \
  rviz_respawn:=false