#!/usr/bin/env bash
# MuJoCo O6 左手仿真桥
# URDF/MJCF 来源: /home/lxqs/Linkerbot_URDF/06/LEFT（9/3 标定版）
# 默认订阅 /cb_left_hand_control_cmd（6 维 SDK 0~255）
# 发布 /sim/mujoco/o6/left/joint_state
#
# 用法:
#   ./hands/O6/scripts/run_o6_left_mujoco_plan.sh
#   ./hands/O6/scripts/run_o6_left_mujoco_plan.sh --pd-drive
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/O6/linker_hand_o6_left/linker_hand_o6_left.xml"
URDF="$PKG_SRC/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linkerhand_o6_left.urdf"
if [[ ! -f "$XML" ]]; then
  echo "[error] missing O6 left MJCF: $XML" >&2
  exit 1
fi

echo "[o6-left] urdf=$URDF"
echo "[o6-left] xml=$XML"
echo "[o6-left] cmd-topic=/cb_left_hand_control_cmd"
echo "[o6-left] sim-state=/sim/mujoco/o6/left/joint_state"
echo "[o6-left] SDK 6ch: pitch, yaw, index, middle, ring, pinky"

exec python3 -m linker_hand_mujoco_ros2.o6_left_plan_bridge \
  --xml "$XML" \
  --node-name o6_mujoco_plan_bridge_left \
  --window-title "O6 left" \
  --cmd-topic /cb_left_hand_control_cmd \
  --sim-state-topic /sim/mujoco/o6/left/joint_state \
  --nominal-topic /retarget/o6/left/joint_target_nominal \
  --mapping g20_urdf \
  "$@"