#!/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

# \"哪些进程算仿真栈\"这张表**不在本文件里**，在 scripts/lib/sim_procs.sh：
# 它同时被 run_sim_gz.sh（启动前的预检）使用，两处共用一份实现，不会漂移。
#
# 历史教训（都真实发生过，所以规则才写这么细）：
#   - 漏过 robot_state_publisher：脚本说"已停止"，它还在跑；下一次启动时
#     继续发上一份 /robot_description 与 TF，表现成"改了描述却不生效"。
#   - 漏过 static_transform_publisher：更阴 —— 它发的是 latched 的 /tf_static，
#     残留进程会一直把**旧相机外参**按在新仿真的 TF 树上。
#   - 漏过 sim_gripper_adapter：残留的适配器与新适配器**同时**订阅 /joint_states、
#     同时发布 /gripper/status，服务调用还会被随机分给其中一个。
#     验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（实际 127 mm），
#     看起来就像夹爪代码算错了 —— 这是最难查的一类残留。
#
# 所以停止分两步：
#   ① 主通道：run_sim_gz.sh 用 setsid 起仿真，PGID 记在 log_sim/sim.pgid，
#      `kill -TERM -PGID` 一次带走整组（**不依赖任何模式串**）；
#   ② 兜底：模式扫描（点名表 + install_sim/ 兜底），收拾历史孤儿与
#      ign 自己 fork 出去的进程；杀完再扫一遍，有残留就报出来并非零退出。
# shellcheck source=lib/sim_procs.sh
source "$(dirname "${BASH_SOURCE[0]}")/lib/sim_procs.sh"

PGID="$(sim_pgid_read)"
if [ -n "$PGID" ] && sim_pgid_alive "$PGID"; then
    if ! sim_pgid_leader_is_ours "$PGID"; then
        echo "⚠ log_sim/sim.pgid 里的进程组 $PGID 看起来不是仿真栈（PID 可能被复用），忽略它。"
        PGID=""
    fi
else
    PGID=""
fi

if [ "$LIST_ONLY" = "1" ]; then
    if [ -n "$PGID" ]; then
        echo "仿真的进程组：$PGID（kill -TERM -$PGID 可整组停掉）"
        ps -o pid=,args= -g "$PGID" 2>/dev/null | cut -c1-150
    fi
fi

# 模式扫描的候选（--list 时先打印，再决定动不动手）。
mapfile -t targets < <(sim_proc_targets)

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

if [ -n "$PGID" ] && [ "$LIST_ONLY" != "1" ]; then
    echo "停止仿真进程组 $PGID（整组，含不在 install_sim/ 下的节点）…"
    sim_pgid_stop "$PGID"
    # 组信号发完后重新扫一遍：可能还有孤儿。
    mapfile -t targets < <(sim_proc_targets)
fi

if [ "${#targets[@]}" = "0" ]; then
    echo "仿真已停止。"
    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 与 sim_gripper_adapter
# 都这么漏过，而后者导致的假象是"夹爪算错了"。
# ---------------------------------------------------------------------------
sleep 0.5
mapfile -t leftover < <(sim_proc_targets)

if [ "${#leftover[@]}" != "0" ]; then
    echo "⚠ 仍有仿真相关进程存活（说明它既不在 lib/sim_procs.sh 的点名表里，"
    echo "   也不在 install_sim/ 下 —— 请把它加进点名表，或让它从 install_sim/ 启动）："
    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 "仿真已停止。"