#!/usr/bin/env bash
# 实时视觉伺服（所有可调参数全部从 ~/ros2/config/site.yaml 读取）。
#
#   bash ~/ros2/scripts/run_visual_servo_rt.sh
#   bash ~/ros2/scripts/run_visual_servo_rt.sh --control
#   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
#
# --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# 跟踪相机由 site.yaml 的 perception.track_camera 决定：
#   wrist    腕部 D405 主跟踪；画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0
#            —— 需要同时开 run_camera_d405.sh 与 run_camera_d435.sh
#   overhead 整段只用顶置 D435（实验）—— 另开 run_camera_d435.sh
#
# 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights

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

# 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
    --control) CONTROL=true; shift ;;
    -h|--help) sed -n '2,12p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done

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

python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true

if [[ -n "${SITE_FASTDDS_PROFILE:-}" && -f "${SITE_FASTDDS_PROFILE}" ]]; then
  export FASTRTPS_DEFAULT_PROFILES_FILE="${SITE_FASTDDS_PROFILE}"
  export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}"
  echo "=== FastDDS: ${FASTRTPS_DEFAULT_PROFILES_FILE} ==="
fi

if [[ "${CONTROL}" == true ]]; then
  echo "=== waiting for rizon_arm_controller ACTIVE (T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}) ==="
  # 注意：action-list 可能显示出崩溃过的 ros2_control 留下的僵尸条目，
  # 而且这个环境里的 `ros2 control` CLI 是坏的（xmlrpc Fault）。直接发
  # 服务请求只有控制器管理器真的活着时才会应答。
  ready=false
  for _i in $(seq 1 40); do
    if timeout 5 ros2 service call /controller_manager/list_controllers \
        controller_manager_msgs/srv/ListControllers 2>/dev/null \
        | grep -q "name='rizon_arm_controller', state='active'"; then
      ready=true
      break
    fi
    sleep 0.5
  done
  if [[ "${ready}" != true ]]; then
    echo "ERROR: 机械臂控制器未激活（可能 T1 已崩溃或机器人不在网）。" >&2
    echo "  先确认 ping 通机器人，再重启 T1 并等 operational：" >&2
    echo "  bash ~/ros2/scripts/run_real.sh --arm ${SITE_ROBOT_ARM:-91}" >&2
    exit 1
  fi
  echo "=== stop old perception/servo ==="
  pkill -f 'visual_servo.launch' 2>/dev/null || true
  pkill -f 'lib/arm_application/servo_node' 2>/dev/null || true
  pkill -f 'lib/arm_application/perception_node' 2>/dev/null || true
  sleep 1
  echo "=== start perception + servo CONTROL ON (lock_z from robots[].servo.ee_z_m) ==="
else
  echo "=== launch perception + servo (observe only, no arm motion) ==="
fi
EE="${SITE_EE_FRAME:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}"
TRACK="${SITE_TRACK_CAMERA:-wrist}"
HANDEYE="${SITE_TRACK_HANDEYE_FILE:-${SITE_HANDEYE_FILE:-}}"
CAM_FRAME="${SITE_TRACK_CAMERA_FRAME:-${SITE_CAMERA_FRAME:-}}"
CAM_NAME="${SITE_TRACK_CAM_NAME:-${SITE_CAM_NAME:-}}"
if [[ -z "${HANDEYE}" ]]; then
  echo "ERROR: 跟踪用手眼文件为空（SITE_TRACK_HANDEYE_FILE / SITE_HANDEYE_FILE）" >&2
  exit 2
fi
if [[ ! -f "${HANDEYE}" ]]; then
  echo "ERROR: 跟踪用手眼文件不存在：${HANDEYE}" >&2
  if [[ "${TRACK}" == "overhead" ]]; then
    echo "  先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate --mode eye_to_hand" >&2
  else
    echo "  先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate" >&2
  fi
  exit 2
fi
echo "    arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-}  track=${TRACK}  camera=${CAM_NAME}"
echo "    handeye=${HANDEYE}  frame=${CAM_FRAME}  control=${CONTROL}"
if [[ "${TRACK}" == "overhead" ]]; then
  echo "=== 整段顶视：D435 追踪，D405 不参与一阶段 ==="
  echo "    相机请开：bash ~/ros2/scripts/run_camera_d435.sh --preview"
else
  echo "=== 本脚本不弹预览窗。工件框画在 D405 那个查看器上 ==="
  echo "    bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview"
  echo "    bash ~/ros2/scripts/run_camera_d435.sh --preview   # 顶视可开，绿框仍在 D405 窗"
fi
LAUNCH_ARGS=(
  enable_control:="${CONTROL}"
  handeye_file:="${HANDEYE}"
  ee_frame:="${EE}"
  camera_frame:="${CAM_FRAME}"
)
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"