#!/usr/bin/env bash
# 真机开工前的**一次性体检**（一条命令说清"能不能开工、不能的话人要去做什么"）。
#
#   bash ~/ros2/scripts/run_real_preflight.sh --arm 89
#   bash ~/ros2/scripts/run_real_preflight.sh --arm 89 --json      # 给脚本消费
#   bash ~/ros2/scripts/run_real_preflight.sh --arm 89 --watch-usb 10
#
# 查什么（顺序就是现场该处理的顺序：根因在前，症状在后）：
#   1. 现场配置：这一轮在读哪份 site.yaml、认哪台臂/哪台相机/哪个串口
#   2. 遗留进程：上一轮没退干净的栈会让这一轮的现象指向别处
#   3. 网络：机器人是否在网（含"两台臂 ARP 撞同一个 MAC"这个坑）
#   4. 机械臂 RDK：能不能使能（Manual 模式是**硬阻断**，只能人工在示教器上切）
#   5. USB：RealSense 协商速率 + Hub 是否在内核里卡死
#   6. 触觉 V4L2：设备在不在，是"没插"还是"卡死"
#   7. 几何：手眼矩阵是否自洽 + 悬停高度会不会把工具捅进工件
#
# 退出码：0 = 全绿（GO）；1 = 有需要处理的项（NO-GO，看输出里"要人工做的事"）。
#
# 为什么要有这个而不是让人挨个跑：这些故障**同时出现时现象会互相串**
#（Hub 卡死会让相机消失，相机消失又会让 S00 自检报"缺相机"，
# 而真正拦住机械臂的其实是示教器模式）。分开查会让人先怀疑软件。
#
# 注意：这里**不加** `set -u` —— conda/RoboStack 的钩子会引用未绑定的变量。
set -eo pipefail

ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
WATCH_USB=0
JSON=false

# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定选哪台臂
# 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
    --watch-usb) WATCH_USB="${2:?--watch-usb 需要秒数}"; shift 2 ;;
    --json) JSON=true; shift ;;
    -h|--help) sed -n '2,20p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done

# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh" >/dev/null 2>&1 || true
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"

# site.yaml 不存在时不要直接失败：网络与 RDK 自检在配置前就该能用，
# 而"配置没写"本身也是体检要报出来的一项。
if [[ "${SITE_LOAD_STATUS}" == "2" ]]; then
  echo "ERROR: site.yaml 有问题，先按上面的提示修好再体检。" >&2
  exit 2
fi

SN="${SITE_ROBOT_SN:-${ROBOT_SN:-}}"
if [[ -z "${SN}" ]]; then
  echo "ERROR: 没选到机械臂。给命令加 --arm <臂号>，或在 site.yaml 里填 robot.sn。" >&2
  exit 2
fi

echo "=== 真机开工体检  arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SN}) ==="
echo

ARGS=(--sn "${SN}" --site "${SITE_CONFIG:-${ROOT}/config/site.yaml}")
[[ -n "${SITE_NET_IFACE:-}" ]] && ARGS+=(--iface "${SITE_NET_IFACE}")
[[ "${JSON}" == true ]] && ARGS+=(--json)

RC=0
if [[ "${WATCH_USB}" != "0" ]]; then
  # 先单独盯一会儿 USB：Hub 半死时会"设备在 / 不在"反复跳，
  # 单次采样只会看到一个瞬间，看不出它在抖。
  echo "--- USB 链路盯 ${WATCH_USB}s（看掉线/重连）---"
  python3 "${ROOT}/scripts/lib/usb_health.py" --watch "${WATCH_USB}" || RC=$?
  echo
fi

python3 "${ROOT}/scripts/lib/real_preflight.py" "${ARGS[@]}" || RC=$?
exit "${RC}"