#!/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=""

cleanup() {
  if [[ -n "${logger_pid}" ]]; then
    kill "${logger_pid}" 2>/dev/null || true
    wait "${logger_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
}

release_known_camera_service() {
  local service="$1"

  if [[ "${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" != "release-known" || -z "${service}" ]]; then
    return 0
  fi
  if systemctl is-active --quiet "${service}"; then
    echo "[start-b-side-omnid] stopping known camera service ${service} before discovery" >&2
    systemctl stop "${service}"
  fi
}

resolve_camera_devices() {
  if [[ "${OMNI_CAMERA_AUTO_DISCOVER:-0}" != "1" ]]; then
    return 0
  fi

  OMNI_CAMERA_HEAD_DEVICE="$(
    bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_HEAD_SERIAL}" head
  )"
  OMNI_CAMERA_WAIST_DEVICE="$(
    bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_WAIST_SERIAL}" waist
  )"
  if [[ "${OMNI_CAMERA_HEAD_DEVICE}" == "${OMNI_CAMERA_WAIST_DEVICE}" ]]; then
    echo "[start-b-side-omnid] head and waist resolved to the same device: ${OMNI_CAMERA_HEAD_DEVICE}" >&2
    return 1
  fi
  export OMNI_CAMERA_HEAD_DEVICE OMNI_CAMERA_WAIST_DEVICE
  echo "[start-b-side-omnid] resolved head=${OMNI_CAMERA_HEAD_DEVICE} waist=${OMNI_CAMERA_WAIST_DEVICE}" >&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
  release_known_camera_service "${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}"
  release_known_camera_service "${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}"
  resolve_camera_devices
  OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" \
    OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" \
    bash "${SCRIPT_DIR}/prepare-camera-device.sh"
  OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" \
    OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" \
    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"
  ./bin/b_side_omnid
}

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