#!/usr/bin/env bash
# MuJoCo G20 左手 — 桌面 URDF 验证
# URDF: /home/lxqs/桌面/L20/LEFT/linkerhand_g20_left.urdf（已同步到包内）
# 默认 mapping=g20_urdf（四指侧摆无外部取负，跟 URDF 轴正向）
#
# 用法:
#   ./hands/G20/scripts/run_g20_left_desktop_urdf.sh
#   ./hands/G20/scripts/run_g20_left_desktop_urdf.sh --pd-drive
# 另开终端扫侧摆方向:
#   ./hands/G20/scripts/run_g20_left_mcp_roll_check.sh
# 监视关节弧度:
#   ./hands/G20/scripts/run_g20_joint_gui.sh
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)"
cd "$ROOT"

set +u
# shellcheck disable=SC1091
source /opt/ros/jazzy/setup.bash
if [[ -f "$ROOT/install/setup.bash" ]]; then
  # shellcheck disable=SC1091
  source "$ROOT/install/setup.bash"
fi
set -u

VENV_SITE="$ROOT/venv/lib/python3.12/site-packages"
if [[ -d "$VENV_SITE" ]]; then
  export PYTHONPATH="$VENV_SITE${PYTHONPATH:+:$PYTHONPATH}"
fi

PKG_SRC="$ROOT/src/linkerhand-sim/linker_hand_mujoco_ros2"
export PYTHONPATH="$PKG_SRC${PYTHONPATH:+:$PYTHONPATH}"

XML="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linker_hand_g20_left_desktop.xml"
URDF="$PKG_SRC/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/linkerhand_g20_left_desktop.urdf"
if [[ ! -f "$XML" ]]; then
  echo "[error] missing MJCF: $XML" >&2
  exit 1
fi

echo "[g20-left-desktop] urdf=$URDF"
echo "[g20-left-desktop] xml=$XML"
echo "[g20-left-desktop] cmd-topic=/cb_left_hand_control_cmd"
echo "[g20-left-desktop] sim-state=/sim/mujoco/g20/left/joint_state"
echo "[g20-left-desktop] mapping=g20_urdf (侧摆 cmd0→负限位, cmd255→正限位, 无外部取负)"
echo "[g20-left-desktop] URDF 四指 mcp_roll 轴: +X (未翻转；右手已改为 -X)"
echo "[g20-left-desktop] 侧摆验证: 另开终端运行 ./hands/G20/scripts/run_g20_left_mcp_roll_check.sh"

exec python3 -m linker_hand_mujoco_ros2.g20_plan_bridge \
  --xml "$XML" \
  --node-name g20_mujoco_plan_bridge_left_desktop \
  --window-title "G20 left (desktop URDF)" \
  --cmd-topic /cb_left_hand_control_cmd \
  --sim-state-topic /sim/mujoco/g20/left/joint_state \
  --nominal-topic /retarget/g20/left/joint_target_nominal \
  --mapping g20_urdf \
  "$@"