#!/usr/bin/env bash
# 工控机上的 DH 夹爪（Modbus RTU）。不是 Flexiv Studio / RDK 那一套。
#
# 左臂 89  PGEA 平行夹爪  /dev/ttyUSB0
# 右臂 91  RGI 旋转夹爪   /dev/ttyUSB1
#
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli close
#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli pos 500
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
#   bash ~/ros2/scripts/run_gripper.sh --arm 89          # ROS 节点 PGEA
#   bash ~/ros2/scripts/run_gripper.sh --arm 91          # ROS 节点 RGI
#   bash ~/ros2/scripts/run_dual_gripper.sh              # 两个 ROS 节点都起

set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
CLI=()
DUAL=0

# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/parse_arm_args.sh"
parse_arm_args "$@"
set -- "${PARSE_ARM_REST[@]}"

while [[ $# -gt 0 ]]; do
  case "$1" in
    --cli) shift; CLI=("$@"); break ;;
    --dual) DUAL=1; shift ;;
    -h|--help) sed -n '2,24p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done

# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"

BACKEND="${SITE_GRIP_BACKEND:-usb}"
MODEL="${SITE_GRIP_MODEL:-PGEA-100-20-W}"
ARM="${SITE_ROBOT_ARM:-}"
IS_ROTARY=0
if [[ "${MODEL}" == *RGI* || "${ARM}" == "right" ]]; then
  IS_ROTARY=1
fi

if [[ "${BACKEND}" == "rdk" || "${BACKEND}" == "flexiv" ]]; then
  echo "ERROR: gripper.backend=${BACKEND} is Flexiv Studio. Use usb (IPC Modbus)." >&2
  exit 1
fi

# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"

if [[ ${#CLI[@]} -gt 0 ]]; then
  if [[ "${IS_ROTARY}" -eq 1 ]]; then
    CMD="${CLI[0]}"
    REST=("${CLI[@]:1}")
    ARGS=(--arm right --test "${CMD}")
    case "${CMD}" in
      probe|status) ARGS=(--arm right --test probe) ;;
      pos|move)
        ARGS=(--arm right --test pos)
        if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--pos "${REST[0]}"); fi
        ;;
      rotate|rotate_to)
        ARGS=(--arm right --test rotate)
        if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
        ;;
      revolutions)
        ARGS=(--arm right --test rotate)
        if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
        ;;
    esac
    echo "=== DH RGI  SN=${SITE_GRIP_SN:-?}  ${SITE_GRIP_PORT}  arm=${SITE_ROBOT_SHORT_ID:-?} ==="
    exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
  fi
  PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
  BAUD="${SITE_GRIP_BAUD:-115200}"
  SLAVE="${SITE_GRIP_SLAVE:-1}"
  echo "=== DH ${MODEL}  SN=${SITE_GRIP_SN:-?}  ${PORT} @${BAUD} slave=${SLAVE} ==="
  exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \
    --port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \
    --force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \
    --rts "${SITE_GRIP_RS485_RTS:-}" \
    "${CLI[@]}"
fi

if [[ "${DUAL}" -eq 1 ]]; then
  exec bash "${ROOT}/scripts/run_dual_gripper.sh"
fi

if [[ "${IS_ROTARY}" -eq 1 ]]; then
  pkill -f 'arm_application/dual_gripper_node.py' 2>/dev/null || true
  pkill -f 'lib/arm_application/dual_gripper' 2>/dev/null || true
  sleep 0.3
  echo "=== DH RGI ROS node  ${SITE_GRIP_PORT:-/dev/ttyUSB1}  (arm_msgs dual_gripper right) ==="
  exec ros2 launch arm_application dual_gripper.launch.py sides:=right allow_offline:=true
fi

PORT="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
BAUD="${SITE_GRIP_BAUD:-115200}"
SLAVE="${SITE_GRIP_SLAVE:-1}"
GSN="${SITE_GRIP_SN:-}"
PY="${ROOT}/src/arm_application/arm_application/dh_pgea.py"
NODE_PY="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"

if [[ ! -e "${PORT}" ]]; then
  echo "ERROR: ${PORT} missing. Plug USB–RS485 into the IPC." >&2
  exit 1
fi

pkill -f 'arm_application/dh_pgea_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/dh_pgea' 2>/dev/null || true
sleep 0.4

echo "=== DH ${MODEL}  SN=${GSN:-?}  ${PORT} @${BAUD}  ROS node ==="
exec python3 "${NODE_PY}" --ros-args \
  -p port:="${PORT}" \
  -p baud:="${BAUD}" \
  -p slave_id:="${SLAVE}" \
  -p stroke_mm:="${SITE_GRIP_STROKE_MM:-20.0}" \
  -p force_pct:="${SITE_GRIP_FORCE:-50}" \
  -p speed_pct:="${SITE_GRIP_SPEED:-50}" \
  -p auto_init:="${SITE_GRIP_AUTO_INIT:-true}" \
  -p model:="${MODEL}" \
  -p "serial_number:='${GSN}'" \
  -p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'"