#!/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
  # 这些选项不启动任何东西，只准备描述，直接转发。
  exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
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

exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"