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

patterns=(
  'ros2 launch go2_core go2_start.launch.py'
  '/opt/ros/humble/bin/ros2 launch go2_core go2_start.launch.py'
  'go2_start.launch.py'
  'async_slam_toolbox_node'
  'slam_toolbox'
  'robot_state_publisher'
  'robot_localization/ekf_node'
  'go2_driver/lib/go2_driver/driver'
  'go2_driver/lib/go2_driver/lowstate_to_imu'
  'go2_twist_bridge/lib/go2_twist_bridge/twist_bridge'
  'go2_perception/lib/go2_perception/cloud_accumulation'
  'go2_perception/lib/go2_perception/pointcloud_to_laserscan_node'
  'scan_throttle_node'
  'scan_throttle.py'
  '/opt/ros/humble/lib/rviz2/rviz2'
)

python3 - "${patterns[@]}" <<'PY'
import os
import signal
import sys
import time
patterns = sys.argv[1:]
me = os.getpid()
ppid = os.getppid()
rows = []
for name in os.listdir('/proc'):
    if not name.isdigit():
        continue
    pid = int(name)
    if pid in (me, ppid):
        continue
    try:
        raw = open(f'/proc/{pid}/cmdline', 'rb').read()
        cmd = raw.replace(b'\x00', b' ').decode('utf-8', 'replace')
    except Exception:
        continue
    if cmd and any(pattern in cmd for pattern in patterns):
        if 'go2_stop_mapping.sh' in cmd:
            continue
        rows.append((pid, cmd))
if not rows:
    print('[go2_stop_mapping] no mapping processes found')
    raise SystemExit(0)
for pid, cmd in sorted(rows):
    print(f'[go2_stop_mapping] TERM pid={pid} {cmd[:220]}')
    try:
        os.kill(pid, signal.SIGTERM)
    except ProcessLookupError:
        pass
time.sleep(1.0)
for pid, cmd in sorted(rows):
    try:
        os.kill(pid, 0)
    except ProcessLookupError:
        continue
    print(f'[go2_stop_mapping] KILL pid={pid}')
    try:
        os.kill(pid, signal.SIGKILL)
    except ProcessLookupError:
        pass
PY