#!/usr/bin/env bash
# Clean leftover slave-board Autoware/control/vehicle processes.
#
# Usage:
#   ./cleanup_slave_autoware.sh
#
# Notes:
#   This script is intended for the slave Orin. It stops the slave launch,
#   control containers, chassis/CAN bridge, MRM, diagnostics, and derived ROS
#   processes. Do not run it on a board where you intentionally keep another
#   Autoware stack running in the same ROS environment.

set -euo pipefail

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

log_info() { echo -e "${GREEN}[INFO]${NC} $*"; }
log_warn() { echo -e "${YELLOW}[WARN]${NC} $*"; }

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

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

  local candidates=(
    "${PWD}"
    "${script_dir}"
  )
  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)"

PATTERNS=(
  "run_slave_real_vehicle.sh"
  "ros2 launch.*ecar_slave"
  "ros2 launch.*farui_launch.*/launch/ecar_slave"
  "launch_ros_[0-9]+"
  "launch_ros"
  "component_container"
  "component_container_mt"
  "robot_state_publisher"
  "ecar_can"
  "ecar_can_receiver"
  "ecar_can_sender"
  "socket_can"
  "socket_can_receiver"
  "socket_can_sender"
  "vehicle_velocity_converter"
  "mrm_emergency_stop_operator"
  "mrm_handler"
  "hazard_status_converter"
  "diagnostic_graph_aggregator"
  "autoware_trajectory_follower"
  "autoware_lane_departure_checker"
  "autoware_shift_decider"
  "autoware_vehicle_cmd_gate"
  "autoware_operation_mode_transition_manager"
  "autoware_autonomous_emergency_braking"
  "autoware_control_evaluator"
  "glog_component"
  "heartbeat_monitor.sh"
)

MATCH_RE='run_slave_real_vehicle|ecar_slave|component_container|robot_state_publisher|ecar_can|socket_can|vehicle_velocity_converter|mrm_|hazard_status|diagnostic_graph|trajectory_follower|lane_departure|shift_decider|vehicle_cmd_gate|operation_mode_transition|autonomous_emergency_braking|control_evaluator|glog_component|launch_ros|heartbeat_monitor'

terminate_patterns() {
  local signal="$1"
  local pattern

  for pattern in "${PATTERNS[@]}"; do
    pkill "-${signal}" -f "${pattern}" 2>/dev/null || true
  done
}

log_info "Workspace: ${WORKSPACE_ROOT}"

if [[ -f "${WORKSPACE_ROOT}/install/setup.bash" ]]; then
  cd "${WORKSPACE_ROOT}"
  set +u
  source install/setup.bash
  set -u
else
  log_warn "Workspace setup file not found; continuing without sourcing: ${WORKSPACE_ROOT}/install/setup.bash"
fi

log_info "Sending TERM to leftover slave Autoware processes."
terminate_patterns TERM
sleep 3

if pgrep -f "${MATCH_RE}" >/dev/null 2>&1; then
  log_warn "Some slave processes are still alive; sending KILL."
  terminate_patterns KILL
  sleep 1
fi

log_info "Restarting ROS daemon to refresh node discovery."
ros2 daemon stop >/dev/null 2>&1 || true
ros2 daemon start >/dev/null 2>&1 || true

log_info "Remaining matching processes:"
ps -ef | grep -E "${MATCH_RE}" | grep -v grep || true

log_info "Remaining ROS nodes:"
ros2 node list 2>/dev/null || true