#!/usr/bin/env bash
# Shared environment bootstrap for the open-source go2-nav2-amcl workspace.
# Source this from scripts that need ROS 2 + Unitree DDS + this workspace.

_go2_os_fail() {
  printf '[go2_env] ERROR: %s\n' "$*" >&2
  return 1 2>/dev/null || exit 1
}

_go2_os_script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
export GO2_REPO_ROOT="$(cd "${_go2_os_script_dir}/.." && pwd)"

if [[ -n "${1:-}" ]]; then
  GO2_OS_ROLE="$1"
fi
_go2_os_restore_nounset=0
case "$-" in
  *u*)
    _go2_os_restore_nounset=1
    set +u
    ;;
esac

_go2_runtime_env="${GO2_OS_RUNTIME_ENV:-${GO2_REPO_ROOT}/scripts/go2_runtime.env}"
if [[ -f "${_go2_runtime_env}" ]]; then
  # shellcheck disable=SC1090
  source "${_go2_runtime_env}"
fi

_go2_os_filter_old_go2_path_var() {
  local var_name="$1"
  local raw_value="${!var_name:-}"
  local filtered=""
  local part
  [[ -n "${raw_value}" ]] || return 0
  IFS=':' read -r -a _go2_os_path_parts <<< "${raw_value}"
  for part in "${_go2_os_path_parts[@]}"; do
    [[ -n "${part}" ]] || continue
    if [[ "${part}" == *"/GO2/"* || "${part}" == *"/GO2" ]]; then
      continue
    fi
    if [[ -z "${filtered}" ]]; then
      filtered="${part}"
    else
      filtered="${filtered}:${part}"
    fi
  done
  printf -v "${var_name}" '%s' "${filtered}"
  export "${var_name}"
}

_go2_os_cyclonedds_uri() {
  local iface="$1"
  local peers="${2:-}"
  local discovery_xml=""
  local peer_xml=""
  local peer

  peers="${peers//,/ }"
  for peer in ${peers}; do
    [[ -n "${peer}" ]] || continue
    peer_xml="${peer_xml}<Peer Address=\"${peer}\"/>"
  done

  if [[ -n "${peer_xml}" ]]; then
    discovery_xml="<Discovery><Peers>${peer_xml}</Peers></Discovery>"
  fi

  printf '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="%s" priority="default" multicast="default" /></Interfaces></General>%s</Domain></CycloneDDS>' "${iface}" "${discovery_xml}"
}

_go2_os_iface_exists() {
  local iface="$1"
  [[ -n "${iface}" && -d "/sys/class/net/${iface}" ]]
}

_go2_os_iface_operstate() {
  local iface="$1"
  cat "/sys/class/net/${iface}/operstate" 2>/dev/null || true
}

_go2_os_iface_carrier() {
  local iface="$1"
  cat "/sys/class/net/${iface}/carrier" 2>/dev/null || true
}

_go2_os_iface_ipv4() {
  local iface="$1"
  ip -4 -o addr show dev "${iface}" 2>/dev/null | awk '{print $4}' | cut -d/ -f1 | head -n 1
}

_go2_os_iface_ready() {
  local iface="$1"
  local require_ipv4="${2:-1}"
  local state=""
  local carrier=""
  local ipv4=""
  _go2_os_iface_exists "${iface}" || return 1
  state="$(_go2_os_iface_operstate "${iface}")"
  [[ "${state}" == "up" ]] || return 1
  carrier="$(_go2_os_iface_carrier "${iface}")"
  if [[ -n "${carrier}" && "${carrier}" != "1" ]]; then
    return 1
  fi
  if [[ "${require_ipv4}" == "1" ]]; then
    ipv4="$(_go2_os_iface_ipv4 "${iface}")"
    [[ -n "${ipv4}" ]] || return 1
  fi
  return 0
}

_go2_os_iface_status_text() {
  local iface="$1"
  if ! _go2_os_iface_exists "${iface}"; then
    printf 'missing'
    return
  fi
  local state carrier ipv4
  state="$(_go2_os_iface_operstate "${iface}")"
  carrier="$(_go2_os_iface_carrier "${iface}")"
  ipv4="$(_go2_os_iface_ipv4 "${iface}")"
  printf 'state=%s carrier=%s ipv4=%s' "${state:-unknown}" "${carrier:-n/a}" "${ipv4:-none}"
}

_go2_os_role_transport_policy() {
  if [[ "${GO2_OS_FORCE_WIRED:-0}" == "1" ]]; then
    printf 'wired_required'
    return
  fi
  case "${GO2_OS_ROLE}" in
    mapping|map|slam|save_map|teleop|build)
      printf 'wired_required'
      ;;
    dashboard|nav|navigation|patrol|inspection)
      printf 'wired_required'
      ;;
    *)
      printf 'wired_required'
      ;;
  esac
}

_go2_os_select_iface() {
  local policy="$1"
  local selected_iface=""
  local selected_reason=""
  local fallback_active=0

  case "${policy}" in
    wired_required)
      if _go2_os_iface_ready "${GO2_WIRED_IFACE}" 1; then
        selected_iface="${GO2_WIRED_IFACE}"
        selected_reason="wired_required:${GO2_WIRED_IFACE}"
      else
        GO2_NETWORK_ERROR="wired_required:${GO2_WIRED_IFACE}:$(_go2_os_iface_status_text "${GO2_WIRED_IFACE}")"
        export GO2_NETWORK_ERROR
        return 1
      fi
      ;;
    wireless_preferred)
      if _go2_os_iface_ready "${GO2_WIRELESS_IFACE}" 1; then
        selected_iface="${GO2_WIRELESS_IFACE}"
        selected_reason="wireless_selected:${GO2_WIRELESS_IFACE}"
      elif _go2_os_iface_ready "${GO2_WIRED_IFACE}" 1; then
        selected_iface="${GO2_WIRED_IFACE}"
        selected_reason="wireless_unavailable_using_wired_fallback:${GO2_WIRELESS_IFACE}:${GO2_WIRED_IFACE}"
        fallback_active=1
      else
        GO2_NETWORK_ERROR="robot_ros_network_unavailable:${GO2_WIRELESS_IFACE}:${GO2_WIRED_IFACE}"
        export GO2_NETWORK_ERROR
        return 1
      fi
      ;;
    *)
      GO2_NETWORK_ERROR="unknown_network_policy:${policy}"
      export GO2_NETWORK_ERROR
      return 1
      ;;
  esac

  GO2_OS_SELECTED_IFACE="${selected_iface}"
  GO2_OS_SELECTED_IFACE_REASON="${selected_reason}"
  GO2_OS_FALLBACK_ACTIVE="${fallback_active}"
  export GO2_OS_SELECTED_IFACE GO2_OS_SELECTED_IFACE_REASON GO2_OS_FALLBACK_ACTIVE
  return 0
}

for _go2_os_path_var in AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PYTHONPATH LD_LIBRARY_PATH PATH; do
  _go2_os_filter_old_go2_path_var "${_go2_os_path_var}"
done

: "${GO2_OS_ROLE:=generic}"
: "${GO2_WIRED_IFACE:=ens160}"
: "${GO2_WIRELESS_IFACE:=ens192}"
: "${GO2_OS_MAPPING_IFACE:=${GO2_WIRED_IFACE}}"
: "${GO2_OS_NAV_IFACE:=${GO2_WIRED_IFACE}}"
: "${GO2_OS_DASHBOARD_IFACE:=${GO2_WIRED_IFACE}}"
: "${GO2_OS_MAPPING_PEERS:=}"
: "${GO2_OS_NAV_PEERS:=}"
: "${GO2_OS_DASHBOARD_PEERS:=${GO2_OS_NAV_PEERS}}"
: "${GO2_UNITREE_SETUP:=${HOME}/unitree_ros2/cyclonedds_ws/install/setup.bash}"
: "${GO2_OS_MAP_DIR:=${HOME}/go2_maps}"
: "${GO2_OS_MAP_NAME:=competition_map}"

GO2_TRANSPORT_POLICY="$(_go2_os_role_transport_policy)"
export GO2_TRANSPORT_POLICY
unset GO2_NETWORK_ERROR
unset GO2_OS_SELECTED_IFACE
unset GO2_OS_SELECTED_IFACE_REASON
unset GO2_OS_FALLBACK_ACTIVE

if [[ -z "${GO2_ROS_IFACE:-}" ]]; then
  _go2_os_select_iface "${GO2_TRANSPORT_POLICY}" || _go2_os_fail "${GO2_NETWORK_ERROR}"
  GO2_ROS_IFACE="${GO2_OS_SELECTED_IFACE}"
fi

if [[ -z "${GO2_DDS_PEERS:-}" ]]; then
  case "${GO2_OS_ROLE}" in
    mapping|map|slam|save_map|teleop|build)
      GO2_DDS_PEERS="${GO2_OS_MAPPING_PEERS}"
      ;;
    dashboard)
      GO2_DDS_PEERS="${GO2_OS_DASHBOARD_PEERS}"
      ;;
    nav|navigation|patrol|inspection)
      GO2_DDS_PEERS="${GO2_OS_NAV_PEERS}"
      ;;
    *)
      GO2_DDS_PEERS="${GO2_OS_MAPPING_PEERS}"
      ;;
  esac
fi

[[ -f /opt/ros/humble/setup.bash ]] || _go2_os_fail "missing /opt/ros/humble/setup.bash"
[[ -f "${GO2_UNITREE_SETUP}" ]] || _go2_os_fail "missing Unitree setup: ${GO2_UNITREE_SETUP}"
[[ -d "/sys/class/net/${GO2_ROS_IFACE}" ]] || _go2_os_fail "network interface not found: ${GO2_ROS_IFACE}"

_go2_os_iface_state="$(_go2_os_iface_operstate "${GO2_ROS_IFACE}")"
if [[ "${_go2_os_iface_state}" != "up" && "${GO2_ALLOW_IFACE_DOWN:-0}" != "1" ]]; then
  _go2_os_fail "network interface ${GO2_ROS_IFACE} is ${_go2_os_iface_state:-unknown}; connect the Go2 network first"
fi

# shellcheck disable=SC1091
source /opt/ros/humble/setup.bash
# shellcheck disable=SC1090
source "${GO2_UNITREE_SETUP}"

export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI="$(_go2_os_cyclonedds_uri "${GO2_ROS_IFACE}" "${GO2_DDS_PEERS}")"
export GO2_OS_ROLE
export GO2_WIRED_IFACE
export GO2_WIRELESS_IFACE
export GO2_OS_MAPPING_IFACE
export GO2_OS_NAV_IFACE
export GO2_OS_DASHBOARD_IFACE
export GO2_OS_MAPPING_PEERS
export GO2_OS_NAV_PEERS
export GO2_OS_DASHBOARD_PEERS
export GO2_DDS_PEERS
export GO2_ROS_IFACE
export GO2_OS_MAP_DIR
export GO2_OS_MAP_NAME

if [[ "${GO2_OS_SKIP_WORKSPACE_SETUP:-0}" != "1" && -f "${GO2_REPO_ROOT}/install/setup.bash" ]]; then
  # shellcheck disable=SC1091
  source "${GO2_REPO_ROOT}/install/setup.bash"
fi

if [[ "${_go2_os_restore_nounset}" == "1" ]]; then
  set -u
fi

unset _go2_os_script_dir _go2_runtime_env _go2_os_iface_state _go2_os_restore_nounset
unset _go2_os_path_var _go2_os_path_parts
unset -f _go2_os_filter_old_go2_path_var
unset -f _go2_os_cyclonedds_uri