#!/usr/bin/env bash
# 终端方式运行 D405：走官方 pyrealsense2 SDK —— 发布彩色 + 深度 + 双目 IR 话题。
# 带 --preview 时，Qt 窗口跑在**独立的**查看器进程（`realsense_view`）里，
# 这样绘制界面永远不会拖慢相机数据流。关掉窗口 ≠ 停止相机。
# 不要同时启动 `view_realsense_depth.sh`（USB 是独占的）。
#
# 用法：
#   bash ~/ros2/scripts/run_camera_d405.sh
#   bash ~/ros2/scripts/run_camera_d405.sh --preview
#   bash ~/ros2/scripts/run_camera_d405.sh --arm 91 --preview

set -eo pipefail
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
PREVIEW=-1   # -1 = 沿用 site；0/1 = 强制
HIGH_FPS=0

# 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=1; shift ;;
    --no-preview) PREVIEW=0; shift ;;
    --30fps) HIGH_FPS=1; shift ;;
    -h|--help) sed -n '2,14p' "$0"; exit 0 ;;
    *) echo "Unknown: $1" >&2; exit 1 ;;
  esac
done

# shellcheck disable=SC1091
source "${ROOT}/scripts/env.sh"
# 用源码里的采集逻辑，不走 install 里那份旧的。install 的入口仍由 ros2 run 找到。
export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"

CAM_NAME="${SITE_CAM_NAME:-d405}"
# 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view，
# 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉），
# 现场看起来就像"只剩眼外画面、绿框也不见了"。
pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true
sleep 2

USB_INFO="$(python3 - <<PY 2>/dev/null || true
import pyrealsense2 as rs
want_sn = "${SITE_CAM_SERIAL:-}"
ds = list(rs.context().devices)
if not ds:
    print("")
    raise SystemExit
pick = None
for d in ds:
    sn = d.get_info(rs.camera_info.serial_number)
    if want_sn and sn == want_sn:
        pick = d
        break
if pick is None:
    for d in ds:
        name = (d.get_info(rs.camera_info.name) or "").lower()
        if "405" in name:
            pick = d
            break
pick = pick or ds[0]
usb = pick.get_info(rs.camera_info.usb_type_descriptor)
sn = pick.get_info(rs.camera_info.serial_number)
name = pick.get_info(rs.camera_info.name)
print(f"{usb}|{sn}|{name}")
PY
)"
USB_TYPE="${USB_INFO%%|*}"
echo "=== RealSense USB=${USB_TYPE:-unknown}  ${USB_INFO} ==="
echo "=== site: ${SITE_CONFIG:-?}  weights=${SITE_YOLO_WEIGHTS:-?} ==="

if [[ "${USB_TYPE}" == 2* ]]; then
  CAM_W=640; CAM_H=480; CAM_FPS=15
  echo "=== camera 640x480@15 (USB2: depth+IR still ON) ==="
  echo "=== WARN: D405 is on USB2. Plug it into a USB3 (blue) port for 848x480@30 ==="
elif [[ "${SITE_CAM_USE_SITE_RES:-1}" == "1" ]]; then
  # site.yaml camera.width/height/fps 优先；--30fps 只抬帧率，不再写死 848x480
  CAM_W="${SITE_CAM_WIDTH:-848}"
  CAM_H="${SITE_CAM_HEIGHT:-480}"
  CAM_FPS="${SITE_CAM_FPS:-30}"
  if [[ "${HIGH_FPS}" -eq 1 && "${CAM_FPS}" -lt 30 ]]; then
    CAM_FPS=30
  fi
  echo "=== camera ${CAM_W}x${CAM_H}@${CAM_FPS} serial=${SITE_CAM_SERIAL:-} arm=${SITE_ROBOT_SN:-} ==="
elif [[ "${HIGH_FPS}" -eq 1 ]]; then
  CAM_W=848; CAM_H=480; CAM_FPS=30
  echo "=== camera 848x480@30 (--30fps, site use_site_resolution=false) ==="
else
  CAM_W=1280; CAM_H=720; CAM_FPS=15
  echo "=== camera 1280x720@15 ==="
fi

CAM_ARGS=(
  --ros-args
  -p camera_name:="${CAM_NAME}"
  -p width:="${CAM_W}"
  -p height:="${CAM_H}"
  -p fps:="${CAM_FPS}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
  CAM_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
fi
CAM_ARGS+=(
  -p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
  -p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-4000}"
  -p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
  -p laser_power:="${SITE_CAM_LASER_POWER:-0}"
)
if [[ -n "${SITE_CAM_EXPOSURE_US:-}" && "${SITE_CAM_EXPOSURE_US}" != "0" && "${SITE_CAM_EXPOSURE_US}" != "0.0" ]]; then
  CAM_ARGS+=(-p exposure_us:="${SITE_CAM_EXPOSURE_US}")
fi
if [[ -n "${SITE_CAM_GAIN:-}" && "${SITE_CAM_GAIN}" != "0" && "${SITE_CAM_GAIN}" != "0.0" ]]; then
  CAM_ARGS+=(-p gain:="${SITE_CAM_GAIN}")
fi

if [[ "${SITE_CAM_ENABLE_DEPTH:-1}" == "1" ]]; then
  CAM_ARGS+=(-p enable_depth:=true)
  echo "=== depth ON ==="
else
  CAM_ARGS+=(-p enable_depth:=false)
  echo "=== depth OFF (site.yaml) ==="
fi

DO_PREVIEW="${SITE_CAM_PREVIEW:-1}"
if [[ "${PREVIEW}" -ge 0 ]]; then
  DO_PREVIEW="${PREVIEW}"
fi
# 程序界面只看彩色和深度。红外是旧的四窗格查看器才要的。
# 彩色要等红外齐帧时，场景一动，整组帧对不齐，彩色就被丢掉。
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_ENABLE_INFRA:-1}" == "1" ]]; then
  CAM_ARGS+=(-p enable_infra:=true)
else
  CAM_ARGS+=(-p enable_infra:=false)
fi
if [[ "${DO_PREVIEW}" == "1" && "${SITE_CAM_PREVIEW_STEREO:-1}" == "1" ]]; then
  CAM_ARGS+=(-p preview_stereo:=true)
else
  CAM_ARGS+=(-p preview_stereo:=false)
fi

if [[ "${DO_PREVIEW}" == "1" ]]; then
  echo "=== SDK preview ON (separate viewer process, hold ${SITE_CAM_OVERLAY_HOLD:-0.1}s) ==="
  echo "=== close window = hide preview only; camera keeps publishing ==="
fi

echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ==="
echo "===   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ==="

if [[ "${DO_PREVIEW}" == "1" ]]; then
  # 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
  # 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
  # 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。
  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:="${CAM_NAME}" \
    -p overlay_hold_sec:="${SITE_CAM_OVERLAY_HOLD:-0.1}" \
    -p hud_label:="${SITE_ROBOT_LABEL:-} ${SITE_ROBOT_SN:-} ${SITE_CAM_SERIAL:-}"
else
  exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi