#!/usr/bin/env bash
# Start the real-vehicle Autoware stack and one preset-route tool mode.
#
# 用法:
#   ./run_preset_route_real_vehicle.sh [模式] [站点文件] [是否启动RViz] [地图目录] [车型]
#
# 位置参数都可以省略，省略时使用下面的默认值或对应环境变量:
#   模式:      ui-only
#              可选 record | replay | mission | none | ui-only
#   站点文件:  $PRESET_ROUTE_STATIONS_FILE
#              默认 src/UI/preset_route_cruise/paths/recorded_stations.yaml
#   RViz:      $PRESET_ROUTE_RVIZ
#              非ui-only模式默认 true；ui-only打开UI时默认 false，可选 true | false
#   地图目录:  $PRESET_ROUTE_MAP_PATH
#              默认 /home/nvidia/FaRui-test/map/School06
#   车型:      $PRESET_ROUTE_VEHICLE_MODEL
#              默认 ecar_vehicle
#
# 常用示例:
#   只打开UI，不启动主板Autoware:
#     ./run_preset_route_real_vehicle.sh
#
#   只启动主板Autoware，不启动预设路线工具:
#     ./run_preset_route_real_vehicle.sh none
#
#   启动主板Autoware并关闭RViz:
#     ./run_preset_route_real_vehicle.sh mission "" false
#
#   使用指定地图:
#     ./run_preset_route_real_vehicle.sh mission "" true /home/nvidia/FaRui-test/map/School06
#
#   启动主板Autoware + 站点任务管理器:
#     ./run_preset_route_real_vehicle.sh mission
#
# 常用环境变量:
#   PRESET_ROUTE_MAP_PATH=/path/to/map
#   PRESET_ROUTE_RVIZ=false
#   PRESET_ROUTE_NO_UI=true
#   PRESET_ROUTE_SKIP_MAIN_STACK=true
#   PRESET_ROUTE_START_ON_LAUNCH=true
#   PRESET_ROUTE_LOOP_MISSION=true
#   PRESET_ROUTE_START_STATION_INDEX=0
#   PRESET_ROUTE_AUTOWARE_LAUNCH_FILE=/path/to/ecar_master.launch.xml
#
# 双Orin说明:
#   本脚本用于主板，默认启动 src/Launcher/farui_launch/launch/ecar_master.launch.xml。
#   主板默认不启动 system、vehicle、control；这些由从板脚本负责。

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 "${PRESET_ROUTE_WORKSPACE_ROOT:-}" ]]; then
    echo "${PRESET_ROUTE_WORKSPACE_ROOT}"
    return
  fi

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

  echo "${PWD}"
}

kill_process_group() {
  local pid="$1"
  local signal="${2:-TERM}"

  if [[ -z "${pid}" ]] || ! kill -0 "${pid}" 2>/dev/null; then
    return 0
  fi

  local pgid
  pgid="$(ps -o pgid= -p "${pid}" 2>/dev/null | tr -d ' ')"

  if [[ -n "${pgid}" ]]; then
    kill "-${signal}" -- "-${pgid}" 2>/dev/null || true
  else
    kill "-${signal}" "${pid}" 2>/dev/null || true
  fi
}

launch_preset_route_ui() {
  local source_ui="${WORKSPACE_ROOT}/src/UI/preset_route_cruise/scripts/preset_route_operator.py"
  if [[ -f "${source_ui}" ]]; then
    log_info "Launching preset route UI from source."
    python3 "${source_ui}"
    return
  fi

  local installed_ui="${WORKSPACE_ROOT}/install/preset_route_cruise/lib/preset_route_cruise/preset_route_operator.py"
  if [[ -f "${installed_ui}" ]]; then
    log_warn "preset_route_operator.py is not registered as a ros2 executable; launching installed script."
    python3 "${installed_ui}"
    return
  fi

  if ros2 pkg executables preset_route_cruise 2>/dev/null | awk '{print $2}' | grep -qx "preset_route_operator.py"; then
    ros2 run preset_route_cruise preset_route_operator.py
    return
  fi

  log_error "Preset route UI executable not found. Expected ros2 executable, ${source_ui}, or ${installed_ui}"
  return 1
}

find_default_map_path() {
  echo "/home/nvidia/FaRui-test/map/School06"
}

WORKSPACE_ROOT="$(find_workspace_root)"
DEFAULT_STATIONS_FILE="${WORKSPACE_ROOT}/src/UI/preset_route_cruise/paths/recorded_stations.yaml"
DEFAULT_MAP_PATH="$(find_default_map_path)"
DEFAULT_VEHICLE_MODEL="ecar_vehicle"
DEFAULT_AUTOWARE_LAUNCH_FILE="${WORKSPACE_ROOT}/src/Launcher/farui_launch/launch/ecar_master.launch.xml"

ROUTE_MODE="${1:-ui-only}"
STATIONS_FILE="${2:-${PRESET_ROUTE_STATIONS_FILE:-${DEFAULT_STATIONS_FILE}}}"
RVIZ="${3:-${PRESET_ROUTE_RVIZ:-true}}"
MAP_PATH="${4:-${PRESET_ROUTE_MAP_PATH:-${DEFAULT_MAP_PATH}}}"
VEHICLE_MODEL="${5:-${PRESET_ROUTE_VEHICLE_MODEL:-${DEFAULT_VEHICLE_MODEL}}}"

UI_START_RECORDING_ON_LAUNCH="${PRESET_ROUTE_START_RECORDING_ON_LAUNCH:-false}"
UI_AUTO_SEND_ON_START="${PRESET_ROUTE_AUTO_SEND_ON_START:-false}"
UI_MISSION_START_ON_LAUNCH="${PRESET_ROUTE_START_ON_LAUNCH:-false}"
UI_ALLOW_GOAL_MODIFICATION="${PRESET_ROUTE_ALLOW_GOAL_MODIFICATION:-false}"
UI_MISSION_LOOP="${PRESET_ROUTE_LOOP_MISSION:-false}"
UI_MISSION_START_STATION_INDEX="${PRESET_ROUTE_START_STATION_INDEX:-0}"
UI_SKIP_MAIN_STACK="${PRESET_ROUTE_SKIP_MAIN_STACK:-false}"
UI_RECORD_CLEAR_ON_START="${PRESET_ROUTE_RECORD_CLEAR_ON_START:-false}"
UI_RECORD_APPEND_EXISTING="${PRESET_ROUTE_RECORD_APPEND_EXISTING:-true}"
AUTOWARE_LAUNCH_FILE="${PRESET_ROUTE_AUTOWARE_LAUNCH_FILE:-${DEFAULT_AUTOWARE_LAUNCH_FILE}}"

case "${ROUTE_MODE}" in
  record|replay|mission|none|ui-only)
    ;;
  *)
    log_error "Unsupported route mode: ${ROUTE_MODE}. Expected record, replay, mission, none, or ui-only."
    exit 1
    ;;
esac

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

if [[ ("${ROUTE_MODE}" == "replay" || "${ROUTE_MODE}" == "mission") && ! -f "${STATIONS_FILE}" ]]; then
  log_error "Stations file not found: ${STATIONS_FILE}"
  exit 1
fi

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

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

if [[ "${UI_SKIP_MAIN_STACK}" != "true" && "${ROUTE_MODE}" != "ui-only" && ! -f "${AUTOWARE_LAUNCH_FILE}" ]]; then
  log_error "Autoware launch file not found: ${AUTOWARE_LAUNCH_FILE}"
  exit 1
fi

log_info "Workspace: ${WORKSPACE_ROOT}"
log_info "Mode: ${ROUTE_MODE}, stations: ${STATIONS_FILE}"
log_info "Autoware launch: ${AUTOWARE_LAUNCH_FILE}"
log_info "Map path: ${MAP_PATH}"

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

AUTOWARE_STACK_PID=""
EXTRA_TOOL_PID=""
UI_PID=""

if [[ "${UI_SKIP_MAIN_STACK}" != "true" && "${ROUTE_MODE}" != "ui-only" ]]; then
  log_info "Launching master Orin Autoware stack."
  setsid ros2 launch "${AUTOWARE_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 &
  AUTOWARE_STACK_PID=$!
else
  log_info "Main stack launch skipped by PRESET_ROUTE_SKIP_MAIN_STACK=true."
fi

if [[ "${ROUTE_MODE}" != "ui-only" ]]; then
  if [[ "${ROUTE_MODE}" == "record" ]]; then
    log_info "Launching station recorder."
    setsid ros2 launch preset_route_cruise record_stations_from_rviz.launch.xml \
      output_file:="${STATIONS_FILE}" \
      clear_file_on_start:="${UI_RECORD_CLEAR_ON_START}" \
      append_existing:="${UI_RECORD_APPEND_EXISTING}" \
      write_on_each_point:=true \
      start_recording_on_launch:="${UI_START_RECORDING_ON_LAUNCH}" \
      use_sim_time:=false &
    EXTRA_TOOL_PID=$!
  fi

  if [[ "${ROUTE_MODE}" == "replay" ]]; then
    log_info "Launching preset route sender."
    setsid ros2 launch preset_route_cruise preset_route_cruise.launch.xml \
      stations_file:="${STATIONS_FILE}" \
      clear_route_before_send:=true \
      allow_goal_modification:="${UI_ALLOW_GOAL_MODIFICATION}" \
      auto_shutdown_after_send:=false \
      auto_send_on_start:="${UI_AUTO_SEND_ON_START}" \
      use_sim_time:=false &
    EXTRA_TOOL_PID=$!
  fi

  if [[ "${ROUTE_MODE}" == "mission" ]]; then
    log_info "Launching station mission manager."
    setsid ros2 launch preset_route_cruise station_mission_manager.launch.xml \
      stations_file:="${STATIONS_FILE}" \
      loop_mission:="${UI_MISSION_LOOP}" \
      auto_start_on_each_leg:=false \
      manual_start_first_leg_only:=true \
      clear_route_before_send:=true \
      allow_goal_modification:="${UI_ALLOW_GOAL_MODIFICATION}" \
      start_on_launch:="${UI_MISSION_START_ON_LAUNCH}" \
      start_station_index:="${UI_MISSION_START_STATION_INDEX}" \
      auto_shutdown_on_finish:=false \
      use_sim_time:=false &
    EXTRA_TOOL_PID=$!
  fi

  if [[ "${ROUTE_MODE}" == "none" ]]; then
    log_info "Extra preset-route tool launch skipped."
  fi
else
  log_info "UI-only mode: main stack and tools will be started from UI."
fi

# 启动 UI 界面
if [[ "${PRESET_ROUTE_NO_UI:-false}" != "true" ]]; then
  export PRESET_ROUTE_MAP_PATH="${MAP_PATH}"
  export PRESET_ROUTE_STATIONS_FILE="${STATIONS_FILE}"
  export PRESET_ROUTE_VEHICLE_MODEL="${VEHICLE_MODEL}"
  if [[ "${ROUTE_MODE}" == "ui-only" ]]; then
    log_info "Launching preset route UI (UI-only mode)."
    PRESET_ROUTE_RVIZ="${PRESET_ROUTE_RVIZ:-false}" launch_preset_route_ui &
    UI_PID=$!
  else
    UI_LAUNCH_DELAY="${PRESET_ROUTE_UI_LAUNCH_DELAY:-3}"
    log_info "UI will launch in ${UI_LAUNCH_DELAY} seconds..."
    (
      sleep "${UI_LAUNCH_DELAY}"
      log_info "Launching preset route UI."
      launch_preset_route_ui
    ) &
    UI_PID=$!
  fi
fi

cleanup() {
  trap - EXIT INT TERM

  local pids=()
  if [[ -n "${UI_PID}" ]]; then
    pids+=("${UI_PID}")
  fi
  if [[ -n "${EXTRA_TOOL_PID}" ]]; then
    pids+=("${EXTRA_TOOL_PID}")
  fi
  if [[ -n "${AUTOWARE_STACK_PID}" ]]; then
    pids+=("${AUTOWARE_STACK_PID}")
  fi

  if [[ ${#pids[@]} -eq 0 ]]; then
    return
  fi

  log_info "Shutting down launched processes."

  for pid in "${pids[@]}"; do
    kill_process_group "${pid}" TERM
  done

  sleep 2

  for pid in "${pids[@]}"; do
    kill_process_group "${pid}" KILL
  done

  wait "${pids[@]}" 2>/dev/null || true
}

trap cleanup EXIT INT TERM

WAIT_PIDS=()
if [[ -n "${AUTOWARE_STACK_PID}" ]]; then
  WAIT_PIDS+=("${AUTOWARE_STACK_PID}")
fi
if [[ -n "${EXTRA_TOOL_PID}" ]]; then
  WAIT_PIDS+=("${EXTRA_TOOL_PID}")
fi
if [[ -n "${UI_PID}" ]]; then
  WAIT_PIDS+=("${UI_PID}")
fi

if [[ ${#WAIT_PIDS[@]} -gt 0 ]]; then
  wait "${WAIT_PIDS[@]}"
fi