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

PATTERN='turtlebot3_office.launch.py|websocket.gzlaunch|robot_state_publisher|/ros_gz_bridge/bridge_node|rosapi_node|__node:=rosapi|__node:=rosapi_params|/ros_gz_sim/create'

kill_by_pattern() {
  local signal="$1"
  local pids
  pids="$(pgrep -f "$PATTERN" || true)"

  if [[ -z "$pids" ]]; then
    echo "[cleanup] no matching processes for SIG${signal}"
    return 0
  fi

  echo "[cleanup] sending SIG${signal} to pids: $(echo "$pids" | tr '\n' ' ')"
  # shellcheck disable=SC2086
  kill -"${signal}" $pids 2>/dev/null || true
}

kill_port_9002() {
  local pids
  pids="$(ss -ltnp 2>/dev/null | grep ':9002 ' | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u || true)"

  if [[ -z "$pids" ]]; then
    echo "[cleanup] port 9002 is free"
    return 0
  fi

  echo "[cleanup] killing port 9002 listeners: $pids"
  # shellcheck disable=SC2086
  kill -9 $pids 2>/dev/null || true
}

echo "[cleanup] start"
kill_by_pattern TERM
sleep 2
kill_by_pattern KILL
kill_port_9002
if command -v ros2 >/dev/null 2>&1; then
  echo "[cleanup] restarting ros2 daemon to clear cached graph..."
  ros2 daemon stop >/dev/null 2>&1 || true
  ros2 daemon start >/dev/null 2>&1 || true
  echo "[cleanup] remaining ROS2 nodes (--no-daemon):"
  ros2 node list --no-daemon || true
  echo "[cleanup] remaining ROS2 topics (--no-daemon):"
  ros2 topic list --no-daemon || true
fi
echo "[cleanup] done"