#!/usr/bin/env bash
# Real-time visual servo (reads ALL tunables from ~/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 starts tracking from the current pose (no lift, no return-to-home).
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# Switch YOLO weights: edit paths.yolo_weights in site.yaml

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:-?}) ==="
  # NOTE: action-list can show zombie entries from a crashed ros2_control,
  # and `ros2 control` CLI is broken in this env (xmlrpc Fault). A raw
  # service call only answers when the controller manager is actually alive.
  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:-Rizon4-063989_flange}"
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
echo "    arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-}  handeye=${HANDEYE}"
echo "    weights=${SITE_YOLO_WEIGHTS}  camera=${SITE_CAM_NAME:-}  control=${CONTROL}"
LAUNCH_ARGS=(
  enable_control:="${CONTROL}"
  handeye_file:="${HANDEYE}"
  ee_frame:="${EE}"
  camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}"
)
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"