#!/usr/bin/env bash
# =============================================================================
# 启动 Gazebo 仿真。
#
# 用法:
#     bash scripts/run_sim_gz.sh                  # 无头（无显示器环境用这个）
#     bash scripts/run_sim_gz.sh --gui            # 带 Gazebo GUI
#     bash scripts/run_sim_gz.sh --check          # 只自检描述，不启动任何东西
#     bash scripts/run_sim_gz.sh --print-urdf     # 打印归一化后的 URDF
#     bash scripts/run_sim_gz.sh --dry-run        # 只打印将执行的命令
#     bash scripts/run_sim_gz.sh --verbose        # 打开 Gazebo 详细日志
#     bash scripts/run_sim_gz.sh --world /abs/other.world
#     bash scripts/run_sim_gz.sh --no-gripper --no-sensors   # 最省资源的运动学验证
#
# 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）：
#     run_sim.sh     ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理
#     run_sim_gz.sh  ← 走 Gazebo：真物理、真接触
#   两者互不影响，也都不需要真机。要验证力控/接触类逻辑只能用后者。
#
# ⚠️ 这个脚本此前**不存在**，却被 install_sim_env.sh（两处）与 build_sim.sh 的
#    提示语引用 —— 照做会直接报"文件不存在"。现在补上。
# =============================================================================
set -euo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
cd "${ROOT}"

# 显式映射到 gazebo_sim_launcher 的选项，不做原样透传。
# 透传会把拼错的选项静默吞掉（ros2 launch 对未知的 key:=value 不报错），
# 而"我明明设了 --gui 却没生效"这类问题极难查。
LAUNCH_ARGS=()
PASS_ONLY=0   # 1 = 只准备描述，不启动（--check / --print-urdf / --dry-run）

while [[ $# -gt 0 ]]; do
  case "$1" in
    --gui)         LAUNCH_ARGS+=(--headless false); shift ;;
    --headless)    LAUNCH_ARGS+=(--headless true);  shift ;;
    --verbose|-v)  LAUNCH_ARGS+=(--verbose);        shift ;;
    --no-gripper)  LAUNCH_ARGS+=(--no-gripper);     shift ;;
    --no-sensors)  LAUNCH_ARGS+=(--no-sensors);     shift ;;
    --check|--print-urdf|--dry-run)
                   LAUNCH_ARGS+=("$1"); PASS_ONLY=1; shift ;;
    --world)
                   [[ -n "${2:-}" ]] || { echo "--world 需要跟一个路径" >&2; exit 2; }
                   LAUNCH_ARGS+=(--world "$2"); shift 2 ;;
    -h|--help)     sed -n '2,18p' "$0"; exit 0 ;;
    *)             echo "未知参数: $1（用 --help 看用法）" >&2; exit 2 ;;
  esac
done

if [[ ! -f "${ROOT}/install_sim/setup.bash" ]]; then
  echo "install_sim/ 还不存在。先跑:" >&2
  echo "    bash scripts/build_sim.sh" >&2
  exit 1
fi

# sim_env.sh 会先退到 base 再激活目标环境，避免同一 shell 里来回切造成的
# PATH/PYTHONPATH 残留（详见 scripts/lib/conda_switch.sh 顶部的实测说明）。
# shellcheck disable=SC1091
source "${ROOT}/scripts/sim_env.sh"

if ! command -v ign >/dev/null 2>&1; then
  echo "找不到 ign（Gazebo Fortress 的 CLI）。先跑:" >&2
  echo "    bash scripts/install_sim_env.sh" >&2
  exit 1
fi

if [[ "${PASS_ONLY}" -eq 1 ]]; then
  # 这些选项不启动任何东西，只准备描述，直接转发。
  # （所以放在"残留进程"预检**之前**：手上有仿真在跑时，照样应该能 --print-urdf。）
  exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
fi

# ---------------------------------------------------------------------------
# 预检：不许在"上一轮仿真还有残留"的情况下启动
# ---------------------------------------------------------------------------
# ⚠️ 这不是洁癖。实测过的最坏情况：残留了一个 sim_gripper_adapter，新起一个之后
#    两个适配器**同时**订阅 /joint_states、同时发布 /gripper/status，
#    服务调用还会被随机分给其中一个 —— 验收脚本读到的开口恒为 0 mm、
#    指尖间距 0.03 mm（真实值 127 mm），看起来完全像夹爪换算写错了。
#    残留的 static_transform_publisher 更阴：它发 latched 的 /tf_static，
#    会一直把**旧外参**按在新仿真的 TF 树上。
#    所以这里宁可拒绝启动，也不给你一个"看起来能跑、数据是错的"仿真。
# shellcheck source=lib/sim_procs.sh
source "${ROOT}/scripts/lib/sim_procs.sh"

mapfile -t running < <(sim_proc_targets)
if [[ "${#running[@]}" != "0" ]]; then
  {
    echo "已有仿真相关进程在跑，拒绝启动第二个："
    for pid in "${running[@]}"; do
      printf '  %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-120)"
    done
    echo
    echo "先停掉它们：  bash scripts/stop_sim_gz.sh"
    echo "只想看看残留是谁、不动手：bash scripts/stop_sim_gz.sh --list"
  } >&2
  exit 3
fi

echo "=== 启动 Gazebo 仿真 ==="
echo "  环境      : ${CONDA_DEFAULT_ENV:-?}   ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-?}"
echo "  退出方式  : Ctrl-C"
echo
echo "  首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。"
echo "  验收:  ros2 control list_controllers   应看到两个 active"
echo

# ---------------------------------------------------------------------------
# 起在自己的**会话/进程组**里，并把 PGID 记下来
# ---------------------------------------------------------------------------
# 这样 stop_sim_gz.sh 可以用 `kill -TERM -PGID` 一次性带走整栈（含
# robot_state_publisher / static_transform_publisher 这些"不在 install_sim 下"
# 的节点），不必依赖模式匹配 —— 模式表漏一个就留一个孤儿的教训见脚本注释。
#
# 代价：进程组不在前台，终端里的 Ctrl-C **不会**传到仿真。所以这里自己接
# SIGINT/SIGTERM，转发成组信号，行为与以前"Ctrl-C 停仿真"一致。
PGID_FILE="$(sim_pgid_file)"
mkdir -p "$(dirname "${PGID_FILE}")"

setsid bash -c 'echo $$ > "$1"; shift; exec "$@"' _ "${PGID_FILE}" \
  ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}" &
SIM_PID=$!

_forward_signal() {
  local pgid
  pgid="$(sim_pgid_read)"
  if [[ -n "${pgid}" ]]; then
    echo
    echo "收到中断信号，正在停止整栈（进程组 ${pgid}）…" >&2
    sim_pgid_stop "${pgid}"
  fi
}
trap _forward_signal INT TERM

wait "${SIM_PID}"
status=$?
rm -f "${PGID_FILE}"
exit "${status}"