udpscope/src/source/udps_source.cpp

// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: 2026 Martino Ferrari

#include "source/udps_source.hpp"

#include <stdexcept>

namespace udpscope {
namespace {

static_assert(kTimePacket == UDPS_TIME_PACKET && kTimeFullArray == UDPS_TIME_FULL_ARRAY &&
                  kTimeFirstSample == UDPS_TIME_FIRST_SAMPLE &&
                  kTimeLastSample == UDPS_TIME_LAST_SAMPLE,
              "model.hpp's kTime* must equal the wire protocol's UDPS_TIME_*");

SignalInfo to_info(const udps_signal_t& s) {
  SignalInfo info;
  info.name = s.name;
  info.unit = s.unit;
  info.type_code = s.type_code;
  info.quant_type = s.quant_type;
  // Store the effective dimensions `udps_signal_num_elements` uses, so `SignalInfo::elements()`
  // (rows * cols) gives the same count for any CONFIG, hostile or odd ones included: a zero
  // dimension means 1 on its own (rows 0, cols 5 is 5 elements), and the product is clamped to
  // UDPS_MAX_ELEMENTS, which also keeps rows * cols from overflowing uint32.
  std::uint64_t rows = s.num_rows == 0 ? 1u : s.num_rows;
  std::uint64_t cols = s.num_cols == 0 ? 1u : s.num_cols;
  if (rows > UDPS_MAX_ELEMENTS) rows = UDPS_MAX_ELEMENTS;
  if (cols > UDPS_MAX_ELEMENTS) cols = UDPS_MAX_ELEMENTS;
  if (rows * cols > UDPS_MAX_ELEMENTS) cols = UDPS_MAX_ELEMENTS / rows;
  info.rows = static_cast<std::uint32_t>(rows);
  info.cols = static_cast<std::uint32_t>(cols);
  info.range_min = s.range_min;
  info.range_max = s.range_max;
  info.time_mode = s.time_mode;
  info.sampling_rate = s.sampling_rate;
  info.time_signal_idx = s.time_signal_idx;
  return info;
}

}  // namespace

UdpsSource::UdpsSource(const SourceConfig& cfg)
    : server_(cfg.get("server", "127.0.0.1")),
      port_(static_cast<std::uint16_t>(cfg.get_int("port", 8000))),
      multicast_(cfg.get("multicast")),
      interface_(cfg.get("interface")),
      recv_buffer_(static_cast<int>(cfg.get_int("recv_buffer", 8 * 1024 * 1024))) {}

UdpsSource::~UdpsSource() { close(); }

void UdpsSource::open(SourceSink& sink) {
  close();
  sink_ = &sink;
  udps_client_config_t c;
  udps_client_config_init(&c);
  c.server_addr = server_.c_str();
  c.server_port = port_;
  c.multicast_group = multicast_.empty() ? nullptr : multicast_.c_str();
  c.interface_addr = interface_.empty() ? nullptr : interface_.c_str();
  c.recv_buffer_bytes = static_cast<std::uint32_t>(recv_buffer_);
  c.silence_timeout_s = 2.0;
  c.reconnect_delay_s = 0.5;
  c.keepalive_interval_s = 1.0;

  client_ = udps_client_create(&c);
  if (client_ == nullptr) {
    throw std::runtime_error("could not create the UDPS client for " + server_ + ":" +
                             std::to_string(port_));
  }
  udps_client_set_callbacks(client_, &UdpsSource::config_cb, &UdpsSource::data_cb,
                            &UdpsSource::event_cb, this);
  udps_client_set_datagram_cb(client_, &UdpsSource::datagram_cb, this);
}

void UdpsSource::poll(int timeout_ms) {
  drain_commands();
  if (client_ != nullptr) udps_client_poll(client_, timeout_ms);
}

void UdpsSource::close() {
  if (client_ != nullptr) {
    udps_client_destroy(client_);
    client_ = nullptr;
  }
}

SourceStats UdpsSource::stats() const {
  SourceStats out;
  if (client_ == nullptr) return out;
  udps_stats_t s;
  udps_client_stats(client_, &s);
  out.packets = s.packets_received;
  out.frames = s.frames_delivered;
  out.gaps = s.counter_gaps;
  out.late = s.late_delivered;
  out.resequenced = s.late_resequenced;
  out.stale = s.stale_packets;
  out.overflows = s.reorder_overflows;
  out.reconnects = s.reconnects;
  out.connected = udps_client_is_connected(client_) != 0;
  return out;
}

bool UdpsSource::connected() const {
  return client_ != nullptr && udps_client_is_connected(client_) != 0;
}

void UdpsSource::config_cb(const udps_signal_t* signals, std::uint32_t count,
                           std::uint8_t publish_mode, void* user) {
  auto* self = static_cast<UdpsSource*>(user);
  self->signals_.clear();
  self->signals_.reserve(count);
  for (std::uint32_t i = 0; i < count; ++i) self->signals_.push_back(to_info(signals[i]));
  self->sink_->on_config(self->signals_, publish_mode);
}

void UdpsSource::data_cb(const udps_frame_t* f, void* user) {
  auto* self = static_cast<UdpsSource*>(user);
  // The frame's signals are the CONFIG's; the cache is rebuilt only if a frame ever disagrees.
  if (self->signals_.size() != f->num_signals) {
    self->signals_.clear();
    for (std::uint32_t i = 0; i < f->num_signals; ++i) {
      self->signals_.push_back(to_info(f->signals[i]));
    }
  }
  self->values_.resize(f->num_signals);
  for (std::uint32_t i = 0; i < f->num_signals; ++i) {
    self->values_[i].values = f->values[i].values;
    self->values_[i].count = f->values[i].count;
  }
  Frame fr;
  fr.counter = f->counter;
  fr.lost = f->lost;
  fr.late = f->late != 0;
  fr.recv_time = f->recv_time;
  fr.publish_mode = f->publish_mode;
  fr.num_samples = f->num_samples;
  fr.num_signals = f->num_signals;
  fr.signals = self->signals_.data();
  fr.values = self->values_.data();
  self->sink_->on_data(fr);
}

void UdpsSource::event_cb(udps_event_t event, const char* detail, void* user) {
  auto* self = static_cast<UdpsSource*>(user);
  SourceEvent e = SourceEvent::Error;
  if (event == UDPS_EVENT_CONNECTED) e = SourceEvent::Connected;
  else if (event == UDPS_EVENT_DISCONNECTED) e = SourceEvent::Disconnected;
  self->sink_->on_event(e, detail);
}

void UdpsSource::datagram_cb(const std::uint8_t* bytes, std::size_t len, double recv_time,
                             void* user) {
  auto* self = static_cast<UdpsSource*>(user);
  if (self->tap_) self->tap_(bytes, len, recv_time);
}

}  // namespace udpscope