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

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

cd "${OMNISOCKETGO_ROOT}"

export OMNISOCKET_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
export OMNISOCKET_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
export OMNI_VIDEO_SERVER_ADDR="${OMNI_VIDEO_SERVER_ADDR}"
export OMNI_VIDEO_RELAY_VIA="${OMNI_VIDEO_RELAY_VIA}"
export OMNI_CONTROL_SERVER_ADDR="${OMNI_CONTROL_SERVER_ADDR}"
export OMNI_CONTROL_RELAY_VIA="${OMNI_CONTROL_RELAY_VIA}"

logger_pid=""
bridge_pid=""

cleanup() {
  if [[ -n "${logger_pid}" ]]; then
    kill "${logger_pid}" 2>/dev/null || true
    wait "${logger_pid}" 2>/dev/null || true
  fi
  if [[ -n "${bridge_pid}" ]]; then
    kill "${bridge_pid}" 2>/dev/null || true
    wait "${bridge_pid}" 2>/dev/null || true
  fi
}

start_5g_link_logger_if_needed() {
  if [[ "${OMNI_5G_LINK_LOG_ENABLED:-1}" != "1" ]]; then
    echo "[start-b-side-omnid] 5G link logger disabled" >&2
    return 0
  fi
  if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then
    return 0
  fi
  bash "${SCRIPT_DIR}/start-5g-link-logger.sh" &
  logger_pid=$!
  echo "[start-b-side-omnid] 5G link logger -> ${BLITZ_5G_LINK_LOG_PATH:-unset}" >&2
}

start_ros_camera_bridge_if_needed() {
  if [[ "${OMNI_ROS2_BRIDGE_AUTO_START:-1}" != "1" ]]; then
    echo "[start-b-side-omnid] ROS2 camera bridge auto-start disabled" >&2
    return 0
  fi
  if pgrep -af 'omnisocket_ros_camera_bridge' >/dev/null 2>&1; then
    echo "[start-b-side-omnid] ROS2 camera bridge already running" >&2
    return 0
  fi
  if [[ ! -f "${OMNI_ROS2_BRIDGE_SETUP}" ]]; then
    echo "[start-b-side-omnid] missing ROS2 bridge setup: ${OMNI_ROS2_BRIDGE_SETUP}" >&2
    echo "[start-b-side-omnid] build it with: cd ${OMNI_ROS2_BRIDGE_ROOT} && colcon build" >&2
    return 1
  fi
  mkdir -p "$(dirname "${OMNI_ROS2_BRIDGE_LOG_PATH}")"
  (
    if [[ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
      # shellcheck disable=SC1091
      set +u
      source "/opt/ros/${ROS_DISTRO}/setup.bash"
      set -u
    fi
    # shellcheck disable=SC1091
    set +u
    source "${OMNI_ROS2_BRIDGE_SETUP}"
    set -u
    exec ros2 run omnisocket_camera_bridge omnisocket_ros_camera_bridge \
      --ros-args \
      -p "head_topic:=${OMNI_ROS2_HEAD_RGB_TOPIC}" \
      -p "waist_topic:=${OMNI_ROS2_WAIST_RGB_TOPIC}" \
      -p "head_shm:=${OMNI_ROS2_HEAD_SHM}" \
      -p "waist_shm:=${OMNI_ROS2_WAIST_SHM}" \
      -p "max_frame_bytes:=${OMNI_ROS2_MAX_FRAME_BYTES}"
  ) >>"${OMNI_ROS2_BRIDGE_LOG_PATH}" 2>&1 &
  bridge_pid=$!
  sleep 2
  if ! kill -0 "${bridge_pid}" 2>/dev/null; then
    echo "[start-b-side-omnid] ROS2 camera bridge exited; inspect ${OMNI_ROS2_BRIDGE_LOG_PATH}" >&2
    return 1
  fi
  echo "[start-b-side-omnid] ROS2 camera bridge pid=${bridge_pid}" >&2
}

if [[ ! -x "./bin/b_side_omnid" ]]; then
  if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then
    echo "Missing ./bin/b_side_omnid in boot mode; build it before enabling the autostart service." >&2
    exit 1
  fi
  make b_side_omnid
fi

launch_b_side_omnid() {
  trap cleanup EXIT INT TERM
  start_5g_link_logger_if_needed
  if [[ "${OMNI_CAMERA_SOURCE}" == "ros2" ]]; then
    bash "${SCRIPT_DIR}/ensure-ros-camera-services.sh"
    start_ros_camera_bridge_if_needed
  else
    OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" \
      OMNI_CAMERA_OCCUPANCY_POLICY=check \
      bash "${SCRIPT_DIR}/prepare-camera-device.sh"
    OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" \
      OMNI_CAMERA_OCCUPANCY_POLICY=check \
      bash "${SCRIPT_DIR}/prepare-camera-device.sh"
    OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh"
    OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh"
  fi
  ./bin/b_side_omnid
}

if [[ "${OMNI_CAMERA_SOURCE}" == "ros2" ]]; then
  # ROS2/DDS must run as the robot user; the C daemon no longer needs root
  # because it reads shared memory instead of opening /dev/video*.
  export B_SIDE_OMNID_USE_SUDO=0
fi

if [[ "${B_SIDE_OMNID_USE_SUDO}" == "1" && "${EUID}" -ne 0 ]]; then
  exec sudo -E bash -lc 'cd "$1" && export B_SIDE_OMNID_USE_SUDO=0 && exec bash "$2"' _ "${OMNISOCKETGO_ROOT}" "${SCRIPT_DIR}/start-b-side-omnid.sh"
fi

launch_b_side_omnid