#!/usr/bin/env bash
# Overhead D435 between the two arms (eye-to-hand). Does not open either D405.
#
#   bash ~/ros2/scripts/run_camera_d435.sh --preview
#
set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=1
while [[ $# -gt 0 ]]; do
  case "$1" in
    --preview) PREVIEW=1; shift ;;
    --no-preview) PREVIEW=0; shift ;;
    -h|--help) sed -n '2,8p' "$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"
SN="${SITE_D435_SERIAL:-}"
if [[ -z "${SN}" ]]; then
  echo "ERROR: cameras.d435.serial empty in site.yaml" >&2
  exit 2
fi
echo "=== D435 overhead serial=${SN}  ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
pkill -f 'camera_name:=d435' 2>/dev/null || true
CAM_ARGS=(
  --ros-args
  -p camera_name:=d435
  -p camera_serial:="${SN}"
  -p width:="${SITE_D435_WIDTH:-1280}"
  -p height:="${SITE_D435_HEIGHT:-720}"
  -p fps:="${SITE_D435_FPS:-30}"
  -p enable_depth:=true
  -p enable_infra:=false
  -p preview_stereo:=false
  -p auto_exposure:=true
  -p ae_limit_us:=8000
  -p emitter_enabled:=true
)
if [[ "${PREVIEW}" == "1" ]]; then
  pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
  ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
  CAM_PID=$!
  trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
  ros2 run arm_application realsense_view --ros-args -p camera_name:=d435
else
  exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi