#!/usr/bin/env bash
set -eo pipefail
cd "$(dirname "$0")/.."
source /opt/ros/jazzy/setup.bash
source install/setup.bash
export ROS_DOMAIN_ID=74
export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
case "${1:-}" in
  glove)
    exec ros2 run linkerhand_retarget handretarget --ros-args \
      -p calibration:=false -p ports:='["/dev/ttyUSB0"]' -p baudrate:=2000000 \
      -r /cb_left_hand_control_cmd:=/ffg_trial/left_cmd \
      -r /cb_left_hand_matrix_touch:=/ffg_trial/unused_left_touch \
      -r /cb_right_hand_matrix_touch:=/ffg_trial/unused_right_touch \
      -r /hand_teleop_param:=/ffg_trial/unused_params
    ;;
  hand|half|full|follow)
    scale=0.0
    slew=10.0
    speed=30
    confirmation=ARM
    case "$1" in
      hand) echo '相对当前位置 +/-10 SDK单位小幅测试。' ;;
      half) scale=0.5; confirmation=ARM50; echo '50%位置映射：目标=启动时机械手位置+0.5*(手套映射位置-启动时机械手位置)。' ;;
      full) scale=1.0; confirmation=ARM100; echo '100%标定位置映射。必须先完成50%空载检查；不是满速/满力。' ;;
      follow) scale=1.0; slew=60.0; speed=60; confirmation=FOLLOW; echo '完整位置跟随档：仅在慢速全范围已验证正常后使用。' ;;
    esac
    echo "空载测试，速度${speed}、目标缓变${slew}单位/秒。收到手套数据后可能立即开始运动。不是急停保护。"
    read -r -p "确认周围无人、无夹持物且可立即断电，输入 $confirmation：" answer
    [[ "$answer" == "$confirmation" ]] || exit 1
    exec ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
      -p hand_type:=left -p hand_joint:=G20 -p can:=can0 \
      -p is_touch:=false -p modbus:=None -p safe_trial:=true \
      -p trial_motion_scale:="$scale" \
      -p trial_slew_rate:="$slew" -p trial_motor_speed:="$speed" \
      -p state_poll_rate:=10.0 -p velocity_poll_rate:=2.0 \
      -r /cb_left_hand_control_cmd:=/ffg_trial/left_cmd
    ;;
  *) echo "用法：bash tools/ffg_trial.sh glove|hand|half|full|follow"; exit 2 ;;
esac