#!/usr/bin/env bash
# =============================================================================
# scripts/stop_sim_gz.sh —— 干净地停掉 Gazebo 仿真栈
#
# 为什么要专门写一个脚本，而不是让你敲 pkill：
#
#   ⚠️ `pkill -f <模式>` 在"用脚本启动仿真"的场景下会**把自己的启动壳一起杀掉**。
#      因为那条命令自己的命令行里就含那个模式字符串，pkill -f 匹配整条命令行，
#      于是它先匹配到自己所在的 shell。表现是：命令返回得飞快、什么输出都没有、
#      仿真也没停成 —— 而这看起来像脚本写错了，极难往"进程匹配"上想。
#
#   本脚本的做法：扫描候选 PID → 排除自己/祖先/扫描自身的临时进程 → kill，
#   最后**再扫一遍复查**（杀掉后仍有残留就报出来）。
#   所以它可以在任何调用链里安全使用（包括被其它脚本 `source`/调用）。
#
# 用法：
#     bash scripts/stop_sim_gz.sh            # 停仿真（TERM，等 5s，仍在则 KILL）
#     bash scripts/stop_sim_gz.sh --list     # 只列出会被停掉的进程，不动手
# =============================================================================
set -uo pipefail

LIST_ONLY=0
[ "${1:-}" = "--list" ] && LIST_ONLY=1

# 这些模式覆盖仿真栈的全部进程：
#   gz sim / ign gazebo   服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
#   gazebo_sim.launch.py   ros2 launch 的壳
#   gazebo_sim_launcher    入口可执行文件（展开描述后转交 launch）
#   parameter_bridge       相机/触觉/clock 的桥接
#   sim_camera_adapter / sim_tactile_adapter  两个适配器
#   robot_state_publisher  发 TF 与 /robot_description
#   controller_manager.*spawner / gz_ros2_control  控制器
#
# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
#    下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
#    参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
#    所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
#    加进新进程时，务必确认复查也能扫到它。
#
# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'

# ---------------------------------------------------------------------------
# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
#
# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
#
# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
#    用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
#    这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
#    多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
#
# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
scan_pids() {
    # ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
    export PAT="$PATTERNS"
    ps -eo pid=,ppid=,args= | awk '
        BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
        $0 ~ pat {
            if (NF < 3) next                 # 没有 args 字段（僵尸/内核线程）：跳过
            pid = $1; ppid = $2
            args = substr($0, index($0, $3))
            if (ppid == me) next             # 本脚本的直接子进程（进程替换 fork 出的子 shell）
            if (args ~ /stop_sim_gz\.sh/) next   # 本脚本自己 / 调用它的壳的文本
            if (args ~ /^\s*awk/) next       # 扫描用的 awk 本身
            print pid "\t" args
        }'
}
export ME=$$

# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
exclude=()
p=$$
while [ "$p" -gt 1 ] 2>/dev/null; do
    exclude+=("$p")
    p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
    [ -z "$p" ] && break
done

# 从扫描结果里剔掉自己/祖先，只输出 PID。
# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
filter_targets() {
    local pid
    while read -r pid _rest; do
        [ -z "$pid" ] && continue
        local skip=0
        for e in "${exclude[@]}"; do
            [ "$pid" = "$e" ] && skip=1 && break
        done
        [ "$skip" = "1" ] && continue
        echo "$pid"
    done
}

mapfile -t SCAN < <(scan_pids)
mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)

if [ "${#targets[@]}" = "0" ]; then
    echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
    exit 0
fi

echo "将停止以下进程："
for pid in "${targets[@]}"; do
    printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
done

if [ "$LIST_ONLY" = "1" ]; then
    echo "（--list：未动手）"
    exit 0
fi

for pid in "${targets[@]}"; do
    kill -TERM "$pid" 2>/dev/null
done

# 给 HAL 一点时间退出。Gazebo 的渲染线程与 DART 的求解线程不会立刻收尾，
# 立刻 KILL 有可能留下共享内存段，下一次启动表现成"渲染初始化失败"。
for _ in $(seq 1 10); do
    alive=0
    for pid in "${targets[@]}"; do
        kill -0 "$pid" 2>/dev/null && alive=1
    done
    [ "$alive" = "0" ] && break
    sleep 0.5
done

for pid in "${targets[@]}"; do
    if kill -0 "$pid" 2>/dev/null; then
        echo "  PID $pid 没响应 TERM，用 KILL"
        kill -KILL "$pid" 2>/dev/null
    fi
done

# ---------------------------------------------------------------------------
# 复查：杀完再扫一遍。
# 这一步是为了让"模式漏了某个进程"变成**看得见的告警**，而不是
# "脚本说已停止、实际有残留"。robot_state_publisher 就这么漏过一次。
# ---------------------------------------------------------------------------
sleep 0.5
mapfile -t SCAN2 < <(scan_pids)
mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)

if [ "${#leftover[@]}" != "0" ]; then
    echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
    for pid in "${leftover[@]}"; do
        printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
    done
    echo "仿真已停止，但有残留 —— 残留会与下一次启动抢话题/TF。"
    exit 3
fi

echo "仿真已停止。"