#!/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 --force 40 --speed 20 --cli rotate_by -15
#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
#   --force / --speed 写在 --cli 前面，覆盖 site.yaml 里的力度和速度。
#   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
GRIP_FORCE=""
GRIP_SPEED=""

# 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
    --force) GRIP_FORCE="${2:?}"; shift 2 ;;
    --speed) GRIP_SPEED="${2:?}"; shift 2 ;;
    --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"

# 平行爪节点起来之后串口是它的。再开一次会报占用，开合就失效。
# 这时把命令交给节点已经在听的 /gripper/ctrl。
pgea_node_up() {
  local line
  while IFS= read -r line; do
    [[ "${line}" == *pgrep* ]] && continue
    [[ -n "${line}" ]] && return 0
  done < <(pgrep -af 'dh_pgea_node.py|lib/arm_application/dh_pgea' 2>/dev/null || true)
  return 1
}

# 控制前把平行爪驱动拉起来。不 pkill 已经在跑的节点，也不写 0xA5。
# auto_init 用 site 里这一臂的值（现场是 false，回零会把夹着的件丢掉）。
# setsid：命令脚本退出后驱动还在，不会跟着会话一起被挂断。
ensure_pgea_node() {
  if pgea_node_up; then
    return 0
  fi
  local port="${SITE_GRIP_PORT:-/dev/ttyUSB0}"
  if [[ ! -e "${port}" ]]; then
    echo "ERROR: ${port} 不在。USB–RS485 没插上，夹爪驱动起不来。" >&2
    return 1
  fi
  local node_py="${ROOT}/src/arm_application/arm_application/dh_pgea_node.py"
  local short="${SITE_ROBOT_SHORT_ID:-89}"
  local log_dir="/tmp/orisys_console"
  local log="${log_dir}/gripper_node_${short}.log"
  local auto="${SITE_GRIP_AUTO_INIT:-true}"
  mkdir -p "${log_dir}"
  echo "=== 平行爪驱动没在跑，先拉起来（auto_init=${auto}，不另写回零） ${port} ==="
  setsid python3 "${node_py}" --ros-args \
    -p port:="${port}" \
    -p baud:="${SITE_GRIP_BAUD:-115200}" \
    -p slave_id:="${SITE_GRIP_SLAVE:-1}" \
    -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:="${auto}" \
    -p model:="${MODEL}" \
    -p "serial_number:='${SITE_GRIP_SN:-}'" \
    -p "rs485_rts:='${SITE_GRIP_RS485_RTS:-}'" \
    >>"${log}" 2>&1 < /dev/null &
  local pid=$!
  local i
  for i in $(seq 1 24); do
    if ! kill -0 "${pid}" 2>/dev/null; then
      echo "ERROR: 夹爪驱动起来就退出了。" >&2
      tail -n 30 "${log}" >&2 || true
      return 1
    fi
    if timeout 2 ros2 topic echo /gripper/states --once >/dev/null 2>&1; then
      echo "=== 夹爪驱动已在发 /gripper/states（pid ${pid}）==="
      return 0
    fi
  done
  echo "ERROR: 夹爪驱动没有发出 /gripper/states。" >&2
  tail -n 30 "${log}" >&2 || true
  kill "${pid}" 2>/dev/null || true
  return 1
}

if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
  ensure_pgea_node || exit 1
fi

if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]] && pgea_node_up; then
  export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
  export GRIP_CMD="${CLI[0]}"
  export GRIP_VAL="${CLI[1]:-}"
  export GRIP_FORCE="${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}"
  export GRIP_SPEED="${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}"
  echo "=== 平行爪节点已占 ${SITE_GRIP_PORT:-/dev/ttyUSB0}，命令走 /gripper/ctrl ==="
  CTRL="$(python3 - <<'PY'
import os
from arm_application.dh_pgea import ctrl_values
vals = ctrl_values(
    os.environ.get("GRIP_CMD", ""),
    os.environ.get("GRIP_VAL", ""),
    int(os.environ.get("GRIP_FORCE") or "50"),
    int(os.environ.get("GRIP_SPEED") or "50"),
)
if vals is None:
    raise SystemExit(2)
print(", ".join(str(v) for v in vals))
PY
)" && rc=0 || rc=$?
  if [[ "${rc}" -eq 2 ]]; then
    exec timeout 4 ros2 topic echo /gripper/states --once
  fi
  if [[ "${rc}" -ne 0 ]]; then
    echo "ERROR: 无法把夹爪命令转成 /gripper/ctrl" >&2
    exit 1
  fi
  exec timeout 8 ros2 topic pub --once /gripper/ctrl std_msgs/msg/Float32MultiArray "{data: [${CTRL}]}"
fi

if [[ ${#CLI[@]} -gt 0 && "${IS_ROTARY}" -eq 0 ]]; then
  echo "ERROR: 平行爪命令没有送到 /gripper/ctrl。" >&2
  exit 1
fi

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
        ;;
      rotate_by)
        ARGS=(--arm right --test rotate_by)
        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
    if [[ -n "${GRIP_FORCE}" ]]; then ARGS+=(--force "${GRIP_FORCE}"); fi
    if [[ -n "${GRIP_SPEED}" ]]; then ARGS+=(--speed "${GRIP_SPEED}"); fi
    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 "${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" --speed "${GRIP_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:-}'"