#!/usr/bin/env bash
# 针对站点机器人对 Flexiv RDK Python 做冒烟测试（不需要 ROS / 不访问 GitHub）。
set -euo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh" 2>/dev/null || true

SN="${1:-${ROBOT_SN:-${SITE_ROBOT_SN:-}}}"
ROBOT_IP="${ROBOT_IP:-${SITE_ROBOT_IP:-192.168.100.1}}"
IFACE="${SITE_NET_IFACE:-}"
PY="${RDK_PYTHON:-/home/cc/rdk_py310/bin/python}"

echo "Using: ${PY}"
echo "Robot SN: ${SN}  IP: ${ROBOT_IP}  iface: ${IFACE:-auto}"
echo "Ping ${ROBOT_IP} ..."
ping -c 1 -W 1 "${ROBOT_IP}" >/dev/null && echo "  network OK" || echo "  network FAIL"

"${PY}" - <<PY
import flexivrdk
sn = "${SN}"
iface = "${IFACE}"
print("Connecting...", sn, "iface", iface or "auto")
robot = None
if iface:
    for args in ((sn, [iface]), (sn, [iface], True), (sn, [iface], True, False)):
        try:
            robot = flexivrdk.Robot(*args)
            break
        except TypeError:
            continue
if robot is None:
    robot = flexivrdk.Robot(sn)
print("Connected. Clearing fault if any...")
try:
    robot.ClearFault()
except Exception as e:
    print("ClearFault:", e)
mode = robot.mode()
print("mode:", mode)
states = robot.states()
print("q:", list(states.q))
print("OK — RDK Python link works. Enable RDK on teach pendant if mode/fault unexpected.")
PY