#!/usr/bin/env bash
# Hole detection on the current-arm wrist D405. Detect only — no insert motion.
#
#   bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
#   bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
#
# backend from site.yaml hole_detect.backend:
#   two_stage  YOLO workpiece bbox + HoughCircles (camera_test hole_detect.py)
#   yolo       YOLO boxes treated as holes
#   hough      full-image HoughCircles

set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=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
    --preview) PREVIEW=true; shift ;;
    -h|--help) sed -n '2,16p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done

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

pkill -f 'arm_application/hole_detect_node.py' 2>/dev/null || true
pkill -f 'lib/arm_application/hole_detect' 2>/dev/null || true
sleep 0.2

BACKEND="${SITE_HOLE_BACKEND:-two_stage}"
echo "=== hole_detect  arm=${SITE_ROBOT_ARM:-?} cam=${SITE_CAM_NAME} backend=${BACKEND} preview=${PREVIEW} ==="
exec ros2 launch arm_application hole_detect.launch.py \
  backend:="${BACKEND}" \
  preview:="${PREVIEW}"