#!/usr/bin/env bash
set -euo pipefail

MODULE_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
FOLDER_NAME="$(basename "$MODULE_PATH")"
CONTAINER_NAME="${FOLDER_NAME#[}"; CONTAINER_NAME="${CONTAINER_NAME/]/-}"
CONTAINER_NAME="${CONTAINER_NAME%_ws}"
MODULE_ROOT="$(dirname "$MODULE_PATH")"
IMAGE="${NAV2_DOCKER_IMAGE:-future-nav2-camera-test:humble-jetson_orin}"
DEVICE=/dev/video0
for arg in "$@"; do
  case "$arg" in video_device:=*) DEVICE=${arg#video_device:=} ;; esac
done
[[ "$DEVICE" =~ ^/dev/video[0-9]+$ ]] && [ -c "$DEVICE" ] || { echo "Camera device not found: $DEVICE" >&2; exit 1; }
test -f "$MODULE_PATH/install/setup.bash" || { echo 'Module is not built' >&2; exit 1; }

docker rm -f "$CONTAINER_NAME" >/dev/null 2>&1 || true
device_args=(--device "$DEVICE:$DEVICE")
# Jetson VI capture opens additional channel devices from the container.
for capture_device in /dev/capture-vi-channel*; do
  if [ -c "$capture_device" ]; then
    device_args+=(--device "$capture_device:$capture_device")
  fi
done
exec docker run --rm -d "${device_args[@]}" --network host --ipc=host \
  --name "$CONTAINER_NAME" \
  -e "ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-77}" \
  -e "ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0}" \
  -v "$MODULE_ROOT:/modules" \
  "$IMAGE" bash -lc "
    source /opt/ros/humble/setup.bash
    source '/modules/$FOLDER_NAME/install/setup.bash'
    exec ros2 launch camera_test_pkg camera.launch.py \"\$@\"
  " bash "$@"