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

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
# shellcheck disable=SC1091
source "${SCRIPT_DIR}/load-env.sh"

if [[ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
  # shellcheck disable=SC1091
  set +u
  source "/opt/ros/${ROS_DISTRO}/setup.bash"
  set -u
fi
if [[ -f "${HOME}/xos/setup.bash" ]]; then
  # shellcheck disable=SC1091
  set +u
  source "${HOME}/xos/setup.bash"
  set -u
fi

service_active() {
  systemctl is-active --quiet "$1"
}

start_service_if_allowed() {
  local service="$1"
  local auto_start="${2:-0}"

  if service_active "${service}"; then
    echo "[ros-camera-services] ${service} is active" >&2
    return 0
  fi
  if [[ "${auto_start}" != "1" ]]; then
    echo "[ros-camera-services] ${service} is inactive; automatic start is disabled" >&2
    return 1
  fi
  echo "[ros-camera-services] starting ${service}" >&2
  sudo systemctl start "${service}"
  service_active "${service}"
}

topic_exists() {
  local topic="$1"
  ros2 topic list 2>/dev/null | grep -Fxq "${topic}"
}

wait_for_topics() {
  local timeout_sec="${1:-20}"
  local deadline=$((SECONDS + timeout_sec))

  while (( SECONDS < deadline )); do
    if topic_exists "${OMNI_ROS2_HEAD_RGB_TOPIC}" \
      && topic_exists "${OMNI_ROS2_WAIST_RGB_TOPIC}"; then
      echo "[ros-camera-services] RGB topics are available" >&2
      return 0
    fi
    sleep 1
  done
  return 1
}

camera_devices_are_free() {
  local device
  local busy=0

  if ! command -v fuser >/dev/null 2>&1; then
    echo "[ros-camera-services] fuser is required to verify camera ownership" >&2
    return 1
  fi
  for device in "${OMNI_CAMERA_HEAD_DEVICE}" "${OMNI_CAMERA_WAIST_DEVICE}"; do
    if [[ ! -e "${device}" ]]; then
      echo "[ros-camera-services] camera device is missing: ${device}" >&2
      busy=1
      continue
    fi
    if fuser "${device}" >/dev/null 2>&1; then
      echo "[ros-camera-services] camera device is busy: ${device}" >&2
      fuser -v "${device}" 2>&1 || true
      busy=1
    fi
  done
  return "${busy}"
}

if ! command -v systemctl >/dev/null 2>&1; then
  echo "[ros-camera-services] systemctl is required on the robot" >&2
  exit 1
fi
if ! command -v ros2 >/dev/null 2>&1; then
  echo "[ros-camera-services] ros2 is not in PATH; source the ROS2 and robot setup files first" >&2
  exit 1
fi

if ! service_active "${OMNI_PROC_MANAGER_SERVICE}"; then
  echo "[ros-camera-services] ${OMNI_PROC_MANAGER_SERVICE} is not active" >&2
  if [[ "${OMNI_PROC_MANAGER_AUTO_START:-0}" == "1" ]]; then
    start_service_if_allowed "${OMNI_PROC_MANAGER_SERVICE}" "${OMNI_PROC_MANAGER_AUTO_START}"
  else
    echo "[ros-camera-services] proc_manager auto-start disabled; using the explicit camera-service fallback if allowed" >&2
  fi
fi

# proc_manager owns the ROS camera components when it is active.  Never start
# separate camera services on top of it: that would create a second device
# owner and reproduce EBUSY.  The individual services are only a fallback when
# proc_manager is inactive and explicitly allowed by the environment.
if wait_for_topics 20; then
  exit 0
fi

if service_active "${OMNI_PROC_MANAGER_SERVICE}"; then
  echo "[ros-camera-services] proc_manager is active but the expected RGB topics are missing" >&2
  if ! camera_devices_are_free; then
    echo "[ros-camera-services] refusing to start ${OMNI_CAMERA_HEAD_SERVICE}/${OMNI_CAMERA_WAIST_SERVICE}: a camera device is already owned" >&2
    exit 1
  fi
  echo "[ros-camera-services] proc_manager does not own the head/waist devices; starting the configured camera services" >&2
fi

if [[ "${OMNI_CAMERA_START_SERVICES:-1}" != "1" ]]; then
  echo "[ros-camera-services] individual camera service fallback is disabled" >&2
  exit 1
fi

if ! camera_devices_are_free; then
  echo "[ros-camera-services] refusing to start camera services while a device is busy" >&2
  exit 1
fi

start_service_if_allowed "${OMNI_CAMERA_HEAD_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}"
start_service_if_allowed "${OMNI_CAMERA_WAIST_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}"
if ! wait_for_topics 20; then
  echo "[ros-camera-services] camera services started but RGB topics did not appear" >&2
  exit 1
fi