#!/usr/bin/env bash
# =============================================================================
# scripts/stop_real.sh —— 干净地停掉真机栈（Rizon4 + arm_control + arm_application）
#
# 为什么需要它（不是"能 pkill 就别写脚本"）
# ----------------------------------------
# 以前真机侧没有任何停止脚本，只有 run_real.sh 里两行 pkill。而真机栈里有一族
# 进程**独立于那两条模式存活**：2026-09-28 实测抓到上一轮栈主体早已退出、只剩
#     real_trajectory_adapter / named_pose_server / arm_status_node /
#     joint_state_publisher
# 四条。它们会**冒名顶替**新一轮的栈：照样接 /trajectory、照样发 /arm_status、
# 照样发 /joint_states —— 新一轮的请求被 DDS 负载均衡到残留的那个，
# 现象是"指令发了臂没动"或"数据乱跳"，而日志上什么都看不出来。
#
# 与之对称的仿真侧早就有 stop_sim_gz.sh（R4.10：停仿真别用 pkill -f）。
# 这个脚本把同样的纪律补给真机侧。扫描规则在 scripts/lib/real_procs.sh，
# 与 run_real.sh 共用一份，不会各写一份漂移。
#
# ⚠️ 为什么这里**不**用 sim 那套"进程组"停止法：
#    run_sim_gz.sh 用 setsid 把仿真起在自己的会话里，PGID 记下来整组 kill。
#    真机栈是**前台交互**跑的（要能 Ctrl-C、要看日志），没有独立进程组；
#    它的 PGID 就是调用它的那个 shell —— 记下来再 kill 会把你自己的终端杀掉。
#    所以真机侧靠"模式扫描 + ROS_DOMAIN_ID 判别"，并且杀完复查。
#
# 用法：
#     bash scripts/stop_real.sh            # 停真机栈（TERM，等 5s，仍在则 KILL）
#     bash scripts/stop_real.sh --list     # 只列出会被停掉的进程，不动手
#
# 不会动的东西：ROS_DOMAIN_ID=77 的进程（那是仿真，归 stop_sim_gz.sh 管）。
# =============================================================================
set -uo pipefail

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

# shellcheck source=lib/real_procs.sh
source "$(dirname "${BASH_SOURCE[0]}")/lib/real_procs.sh"

mapfile -t targets < <(real_proc_targets)

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

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

if [ "$LIST_ONLY" = "1" ]; then
    echo "（--list：未动手。要连仿真进程一起看：REAL_PROC_INCLUDE_SIM=1 bash scripts/stop_real.sh --list）"
    exit 0
fi

# 先 TERM：ros2_control_node 要机会去把 RDK 的会话放掉、把臂置于安全状态。
for pid in "${targets[@]}"; do
    kill -TERM "$pid" 2>/dev/null
done

# 为什么等：RDK / flexiv_hardware 的析构不是瞬时的（要停实时线程、放 socket）。
# 立刻 KILL 有可能把 RDK 会话留在"半开"状态，下一次连接要等它超时。
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

# ---------------------------------------------------------------------------
# 复查：杀完再扫一遍。
# 这一步是为了让"表漏了某个进程"变成**看得见的告警**，而不是
# "脚本说已停止、实际有残留"。上面那四条孤儿当初就是这么漏掉的。
# ---------------------------------------------------------------------------
sleep 0.5
mapfile -t leftover < <(real_proc_targets)

if [ "${#leftover[@]}" != "0" ]; then
    echo "⚠ 仍有真机相关进程存活（说明它既不在 lib/real_procs.sh 的点名表里，"
    echo "   也不在 install/ 下 —— 请把它加进点名表，或让它从 install/ 启动）："
    for pid in "${leftover[@]}"; do
        printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-130)"
    done
    echo "真机栈已停止，但有残留 —— 残留会与下一次启动抢话题/TF。"
    exit 3
fi

echo "真机栈已停止。"