#!/usr/bin/env bash
# 同时开两个 MuJoCo：当前标定 G20 + L20 V10.1 原版 URDF
# 共用同一条 0~255 控制话题（默认 /g20/cb_left_hand_control_cmd）
#
# 用法:
#   ./run_dual_mujoco_compare.sh
#   ./run_dual_mujoco_compare.sh --cmd-topic /cb_left_hand_control_cmd
#   ./run_dual_mujoco_compare.sh --speed 1.0
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
cd "$ROOT"

PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2"
G20_XML="$PKG_SRC/urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml"
L20_XML="$PKG_SRC/urdf/L20_V10.1/linker_hand_l20v101_left/linker_hand_l20v101_left.xml"

if [[ ! -f "$G20_XML" ]]; then
  echo "[error] missing G20 MJCF: $G20_XML" >&2
  exit 1
fi
if [[ ! -f "$L20_XML" ]]; then
  echo "[error] missing L20 MJCF: $L20_XML" >&2
  exit 1
fi

EXTRA=("$@")

cleanup() {
  [[ -n "${PID_G20:-}" ]] && kill "$PID_G20" 2>/dev/null || true
  [[ -n "${PID_L20:-}" ]] && kill "$PID_L20" 2>/dev/null || true
}
trap cleanup EXIT INT TERM

echo "[dual] G20 calibrated  xml=$G20_XML"
"$ROOT/run_g20_mujoco_plan.sh" \
  --xml "$G20_XML" \
  --node-name g20_calibrated \
  --window-title "G20 calibrated (current)" \
  --sim-state-topic /sim/mujoco/g20/left/joint_state \
  --mapping g20_calibrated \
  "${EXTRA[@]}" &
PID_G20=$!

# 错开一点，避免两个 GLFW 同时抢显示
sleep 1

echo "[dual] L20 V10.1 stock xml=$L20_XML (URDF-native mapping, no G20 corrections)"
# L20 mesh 弯曲时自碰多，默认 collision_project 会把运动全部打回；对比时关闭。
"$ROOT/run_g20_mujoco_plan.sh" \
  --xml "$L20_XML" \
  --node-name l20v101_stock \
  --window-title "L20 V10.1 stock (URDF only)" \
  --sim-state-topic /sim/mujoco/l20v101/left/joint_state \
  --mapping l20_urdf \
  --no-collision-project \
  "${EXTRA[@]}" &
PID_L20=$!

echo "[dual] both started. same cmd topic (default /g20/cb_left_hand_control_cmd)."
echo "[dual] L20: collision_project=off (mesh self-overlap would freeze motion)."
echo "[dual] Ctrl+C stops both."
wait "$PID_G20" "$PID_L20"