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

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"

source_with_nounset_off() {
  set +u
  # shellcheck disable=SC1090
  source "$1"
  set -u
}

# shellcheck disable=SC1091
source "${SCRIPT_DIR}/load-env.sh"
source_with_nounset_off "/opt/ros/${ROS_DISTRO}/setup.bash"

cd "${ROS_CONTROL_PY_DIR}"
source_with_nounset_off "install/setup.bash"

launch_args=(
  "transport:=${ROBOT_RECEIVER_TRANSPORT}"
  "peer_id:=${ROBOT_RECEIVER_PEER_ID}"
  "local_socket_path:=${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}"
  "output_topic:=${ROBOT_RECEIVER_OUTPUT_TOPIC}"
  "frame_id:=${ROBOT_RECEIVER_FRAME_ID}"
  "watchdog_timeout:=${ROBOT_RECEIVER_WATCHDOG_TIMEOUT}"
  "publish_rate_hz:=${ROBOT_RECEIVER_PUBLISH_RATE_HZ}"
)

if [[ -n "${ROBOT_RECEIVER_SERVER_ADDR}" ]]; then
  launch_args+=("server_addr:=${ROBOT_RECEIVER_SERVER_ADDR}")
fi

if [[ -n "${ROBOT_RECEIVER_RELAY_VIA}" ]]; then
  launch_args+=("relay_via:=${ROBOT_RECEIVER_RELAY_VIA}")
fi

if [[ -n "${ROBOT_RECEIVER_EXPECTED_SENDER}" ]]; then
  launch_args+=("expected_sender:=${ROBOT_RECEIVER_EXPECTED_SENDER}")
fi

exec ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py "${launch_args[@]}"