udpscope/src/model.cpp

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

#include "model.hpp"

#include <udps_client.h>

#include <algorithm>
#include <cmath>
#include <cstdio>
#include <stdexcept>
#include <string>
#include <unordered_map>

namespace udpscope {
namespace {

u64 pow2_floor(u64 v) {
  if (v == 0) return 0;
  u64 p = 1;
  while (p <= v / 2) p <<= 1;
  return p;
}

}  // namespace

u64 Partition::total_samples() const noexcept {
  u64 total = time_capacity;
  for (u64 c : value_capacity) total += c;
  return total;
}

Partition plan_partition(const std::vector<double>& channel_dt, double buffer_bytes,
                         double target_span_s) {
  Partition p;
  const std::size_t n = channel_dt.size();
  p.stride.assign(n, 1);
  p.value_capacity.assign(n, 0);
  if (n == 0) return p;

  // The fastest channel drives the shared time base: every other channel is
  // decimated by the integer ratio of its own interval to that one.
  double base_dt = 0.0;
  for (double dt : channel_dt) {
    if (dt > 0.0) base_dt = base_dt > 0.0 ? std::min(base_dt, dt) : dt;
  }
  // No declared rate: assume 1 kHz rather than one sample per second, so a
  // stream that only says "here are samples" still gets a sane time scale and a
  // sane first frame in `Acquisition::build_stamps()`.
  if (!(base_dt > 0.0)) base_dt = 1.0e-3;
  p.base_dt = base_dt;

  // No *per-channel* decimation. A stride of `round(dt_i / dt_fastest)` keeps one sample
  // in every stride, and when that stride equals the signal's own period -- a 2 kHz sine
  // on a 1 MHz clock, stride 500 -- every kept sample sits at the same phase and the
  // channel plots as a flat line. The shared clock is decimated below instead, which
  // affects every channel equally and goes through the ingest filter.
  for (std::size_t i = 0; i < n; ++i) p.stride[i] = 1;

  const u64 budget_bytes = static_cast<u64>(std::max(1.0, buffer_bytes));
  const u64 budget = budget_bytes / sizeof(f32);
  double weight = 1.0;  // the time slice itself
  for (u64 s : p.stride) weight += 1.0 / static_cast<double>(s);

  const u64 slots = budget / static_cast<u64>(std::llround(weight));
  u64 time_capacity = pow2_floor(std::max<u64>(slots, 128));
  if (time_capacity == 0) {
    throw std::runtime_error("buffer too small for one time-base sample per channel");
  }

  // The target span sets a floor, not a ceiling: the rest of the budget buys a
  // longer ring, which is strictly more history to scroll back through. Growth
  // stops before the total would exceed the budget. A channel's slots are rounded
  // *up* on the power of two `RingBuffer` will round them to anyway, so every
  // channel covers at least the time ring's span.
  // Floor, not ceiling: a channel's window must not be *longer* than the time ring
  // it reads its stamps from. Rounding up (with a stride that is not a power of two)
  // gave a channel more samples than the shared timeline covers, and the oldest of
  // them referenced time slots the ring had already overwritten -- they were then
  // stamped with the epoch, a plausible-looking coordinate that collapsed the trace
  // against the axis. Flooring can leave a channel holding as little as ~62% of the
  // time ring's span, which is a shorter history, not a wrong one.
  const auto capacity_for = [&](u64 ticks, u64 stride) {
    return std::max<u64>(pow2_floor(ticks / stride), 1);
  };
  const auto bytes_for = [&](u64 ticks) {
    u64 total = ticks;
    for (u64 s : p.stride) total += capacity_for(ticks, s);
    return total * sizeof(f32);
  };
  while (time_capacity * 2 <= budget && bytes_for(time_capacity * 2) <= budget_bytes) {
    time_capacity *= 2;
  }
  p.time_capacity = time_capacity;
  for (std::size_t i = 0; i < n; ++i) {
    p.value_capacity[i] = capacity_for(p.time_capacity, p.stride[i]);
  }

  // Only now, with the ring as long as the budget can buy, decimate the shared clock
  // enough for the requested window to fit in it: deciding this earlier would over-
  // decimate by whatever factor the growth loop was about to add. A power of two keeps
  // the ring capacities (themselves powers of two) in step and keeps `tick % period`
  // cheap. The cap stops a nonsense request -- a window far past `--max-window` -- from
  // spinning here; at 2^20 the stored rate is six decades below the wire rate and the
  // picture is averages of averages anyway.
  u64 m = 1;
  if (target_span_s > 0.0) {
    while (m < (u64(1) << 20) &&
           static_cast<double>(time_capacity) * static_cast<double>(m) * base_dt <
               target_span_s) {
      m *= 2;
    }
  }
  p.time_stride = m;
  p.span_s = static_cast<double>(p.time_capacity) * static_cast<double>(p.time_stride) * base_dt;
  return p;
}

// --- BufferStore ------------------------------------------------------------

BufferStore::BufferStore(const Partition& part, f64 span_s)
    : part_(part),
      time_(std::make_shared<plot::data::RingBuffer<f32>>(part.time_capacity)) {
  (void)span_s;
  const std::size_t n = part.value_capacity.size();
  values_.reserve(n);
  channels_.reserve(n);
  for (std::size_t i = 0; i < n; ++i) {
    values_.push_back(std::make_shared<plot::data::RingBuffer<f32>>(part.value_capacity[i]));
    // `ChannelBuffer::dx()` multiplies this by the channel's stride, so the
    // value here is the *slot* interval: stride * slot_dt is the real sample
    // interval at the decimation the partition chose.
    const double dt = part.slot_dt();
    channels_.push_back(std::make_shared<ChannelBuffer>(values_[i], time_, part.stride[i], 0.0,
                                                        dt));
  }
}

void BufferStore::acquire_all() {
  for (auto& c : channels_) c->acquire();
}

// --- ScopeModel -------------------------------------------------------------

ScopeModel::ScopeModel(const Config& cfg) : cfg_(cfg) {
  trigger_.holdoff_s = cfg.trigger_holdoff_s;
}

void ScopeModel::set_signals(std::vector<SignalInfo> signals) {
  std::lock_guard<std::mutex> lock(mutex_);
  // Keep the enable state across CONFIG updates for signals that kept their name.
  std::unordered_map<std::string, bool> was_enabled;
  for (std::size_t i = 0; i < signals_.size(); ++i) {
    was_enabled.emplace(signals_[i].name, i < enabled_.size() ? enabled_[i] : false);
  }
  signals_ = std::move(signals);
  enabled_.assign(signals_.size(), false);
  for (std::size_t i = 0; i < signals_.size(); ++i) {
    auto it = was_enabled.find(signals_[i].name);
    // Off until the user enables it (spec: the signal panel's switch is what
    // starts acquisition of a signal). A later CONFIG keeps whatever the user
    // chose for a signal that kept its name.
    enabled_[i] = it != was_enabled.end() ? it->second : false;
  }
  rebuild_channels();
  rebuild_locked();
  generation_.fetch_add(1, std::memory_order_release);
}

void ScopeModel::rebuild_channels() {
  channels_.clear();
  for (std::size_t s = 0; s < signals_.size(); ++s) {
    const SignalInfo& sig = signals_[s];
    if (s >= enabled_.size() || !enabled_[s]) continue;
    if (is_time_signal(signals_, s)) continue;
    ChannelInfo ch;
    ch.signal = s;
    ch.enabled = true;
    // A UDPStreamer signal is one channel: its columns are consecutive samples of
    // that channel within a DATA packet (1x1000 is 1000 samples of the same
    // signal, not 1000 signals). The worker client hands a multi-element signal
    // exactly `elements` values per packet, which is the block of samples.
    ch.name = sig.name;
    ch.unit = sig.unit;
    ch.elements = sig.elements();
    ch.dt = sig.sampling_rate > 0.0 ? 1.0 / sig.sampling_rate : 0.0;
    // A calibration the user already set for this name (before it was enabled, or
    // before a CONFIG renumbered the channels) still applies.
    if (auto it = calibration_.find(sig.name); it != calibration_.end()) {
      ch.gain = it->second.first;
      ch.offset = it->second.second;
    }
    channels_.push_back(std::move(ch));
  }
}

void ScopeModel::rebuild_locked() {
  std::vector<double> dts;
  dts.reserve(channels_.size());
  for (const ChannelInfo& c : channels_) dts.push_back(c.dt);

  if (dts.empty()) {
    part_ = Partition{};
    store_.reset();
    return;
  }

  part_ = plan_partition(dts, cfg_.buffer_mb * 1024.0 * 1024.0,
                         target_span_.load(std::memory_order_relaxed));
  store_ = std::make_unique<BufferStore>(part_, part_.span_s);

  if (trigger_.channel >= static_cast<int>(channels_.size())) trigger_.channel = -1;
}

bool ScopeModel::set_target_span(double seconds) {
  if (!(seconds > 0.0)) return false;
  std::lock_guard<std::mutex> lock(mutex_);
  const double previous = target_span_.exchange(seconds, std::memory_order_relaxed);
  if (channels_.empty()) return false;
  // Re-plan only if the factor moves. The planner is cheap, so ask it rather than
  // reimplementing its arithmetic here -- two expressions for the same decision is how
  // the buffer and the view end up disagreeing about the stored rate.
  std::vector<double> dts;
  dts.reserve(channels_.size());
  for (const ChannelInfo& c : channels_) dts.push_back(c.dt);
  const Partition want = plan_partition(dts, cfg_.buffer_mb * 1024.0 * 1024.0, seconds);
  if (want.time_stride == part_.time_stride) return false;
  std::fprintf(stderr,
               "[udpscope] window %.4f s -> %.4f s: decimation %llu -> %llu, stored rate "
               "%.4g -> %.4g S/s (anti-alias %.4g Hz); buffer rebuilt\n",
               previous, seconds, static_cast<unsigned long long>(part_.time_stride),
               static_cast<unsigned long long>(want.time_stride), part_.stored_rate(),
               want.stored_rate(), want.antialias_hz());
  std::fflush(stderr);
  rebuild_locked();
  return true;
}

bool ScopeModel::is_time_signal(const std::vector<SignalInfo>& signals,
                                std::size_t signal) {
  if (signal >= signals.size()) return false;
  const SignalInfo& s = signals[signal];
  // Referenced by name: another signal declares this one as its time base.
  for (const SignalInfo& other : signals) {
    if (other.time_signal_idx == signal) return true;
  }
  // A full-array signal that declares **no sample rate** is the stream's timestamp
  // signal: it has stamps but no cadence of its own, because it is not data.
  // Data channels can also be declared full-array (per-element stamps), and they
  // keep a declared rate -- treating them as time signals hid every real channel.
  if (s.time_mode == UDPS_TIME_FULL_ARRAY && s.elements() > 1 && s.sampling_rate == 0.0) {
    return true;
  }
  return false;
}

bool ScopeModel::signal_enabled(std::size_t signal) const {
  return signal < enabled_.size() && enabled_[signal];
}

void ScopeModel::set_signal_enabled(std::size_t signal, bool on) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (signal >= enabled_.size() || enabled_[signal] == on) return;
  enabled_[signal] = on;
  // The trigger names a channel by its display name, which is stable across
  // renumbering: when its own signal is disabled the channel vanishes (the trigger
  // then waits, and comes back when the signal is re-enabled).
  const int old_trigger = trigger_.channel;
  const std::string trigger_name =
      (old_trigger >= 0 && old_trigger < static_cast<int>(channels_.size()))
          ? channels_[static_cast<std::size_t>(old_trigger)].name
          : std::string();
  rebuild_channels();
  trigger_.channel = -1;
  if (!trigger_name.empty()) {
    for (std::size_t i = 0; i < channels_.size(); ++i) {
      if (channels_[i].name == trigger_name) trigger_.channel = static_cast<int>(i);
    }
  }
  rebuild_locked();
  generation_.fetch_add(1, std::memory_order_release);
}

void ScopeModel::set_calibration(std::size_t signal, double gain, double offset) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (signal < signals_.size()) calibration_[signals_[signal].name] = {gain, offset};
  bool any = false;
  for (ChannelInfo& c : channels_) {
    if (c.signal != signal) continue;
    c.gain = gain;
    c.offset = offset;
    any = true;
  }
  // Calibration is applied at ingest, so the buffers are rebuilt to avoid mixing
  // calibrated and uncalibrated samples (and stale LOD aggregates).
  if (any) {
    rebuild_locked();
    generation_.fetch_add(1, std::memory_order_release);
  }
}

double ScopeModel::channel_dt(std::size_t channel) const {
  return channel < channels_.size() ? channels_[channel].dt : 0.0;
}

BufferStore& ScopeModel::live() { return *store_; }

void ScopeModel::live_window_locked(double lo_s, double hi_s, u64& first, u64& last) {
  BufferStore& src = live();
  plot::data::RingBuffer<f32>& tsrc = src.time();

  // The slice is one absolute index range, found in the time ring and then shared by
  // every value ring. Sharing it is what makes a cut's stamps and each of its channels
  // the same length and index-aligned *by construction*, instead of by two independent
  // truncations that could disagree.
  const u64 tbase = tsrc.produced() - tsrc.size();
  const u64 tend = tsrc.produced();
  // The bounds are rounded to the ring's own `f32` before they are compared against
  // it. The window arrives in f64 and the stamps are stored as f32, so an f64
  // comparison decides a sample that sits exactly on a bound by which way its stamp
  // happened to round -- the same nominal window then keeps or drops its first sample
  // depending on how large the stream time is (`f32(38.001) < 38.001` but
  // `f32(18.001) > 18.001`). One sample is invisible at 10 kS/s and a third of the
  // capture at 1 S/s. Comparing in the stored type makes a bound that lands on a
  // sample include it, always.
  const f32 t_lo = static_cast<f32>(lo_s);
  const f32 t_hi = static_cast<f32>(hi_s);

  // Stream time is non-decreasing, so the bounds are a binary search. Searching the
  // stamps is the whole point: the window is in seconds and this ring is the only
  // thing that knows how many samples a second of this stream holds.
  u64 a = tbase, b = tend;
  while (a < b) {
    const u64 m = a + (b - a) / 2;
    if (tsrc[m - tbase] < t_lo) {
      a = m + 1;
    } else {
      b = m;
    }
  }
  first = a;
  b = tend;
  while (a < b) {
    const u64 m = a + (b - a) / 2;
    if (tsrc[m - tbase] <= t_hi) {
      a = m + 1;
    } else {
      b = m;
    }
  }
  last = a;

  for (std::size_t c = 0; c < channels_.size(); ++c) {
    plot::data::RingBuffer<f32>& from = src.values(c);
    first = std::max(first, from.produced() - from.size());
    last = std::min(last, from.produced());
  }
  if (last < first) last = first;
}

void ScopeModel::reset_live_history() {
  std::lock_guard<std::mutex> lock(mutex_);
  reset_live_history_locked();
}

void ScopeModel::reset_live_history_locked() {
  BufferStore& src = live();
  src.time().clear();
  for (std::size_t c = 0; c < channels_.size(); ++c) {
    src.values(c).clear();
    src.channel(c).invalidate();
  }
}

void ScopeModel::publish_pause_view(double lo_s, double hi_s) {
  auto snap = std::make_shared<EventSnapshot>();
  std::lock_guard<std::mutex> lock(mutex_);
  BufferStore& src = live();
  u64 first = 0;
  u64 last = 0;
  live_window_locked(lo_s, hi_s, first, last);
  const u64 n = last - first;
  snap->stamps.resize(static_cast<std::size_t>(n));
  if (n > 0) {
    // The stamps go in *as the ring holds them*: the stream's own seconds, which is the
    // coordinate the axis was already reading. Nothing is rebased, so a pause does not move the
    // dates under the reader the way a trigger event's edge-relative axis deliberately does.
    src.time().copy_from(first, n, snap->stamps.data());
  }
  snap->values.resize(channels_.size());
  snap->y.resize(channels_.size(), plot::Range{0.0, 0.0});
  for (std::size_t c = 0; c < channels_.size(); ++c) {
    const u64 stride = std::max<u64>(1, part_.stride[c]);
    auto& vals = snap->values[c];
    const u64 take = std::min(n / stride, src.values(c).size());
    vals.resize(static_cast<std::size_t>(take));
    if (take > 0) src.values(c).copy_from(first + n - take, take, vals.data());
    f32 lo = vals.empty() ? 0.0f : vals.front();
    f32 hi = lo;
    for (f32 v : vals) {
      lo = std::min(lo, v);
      hi = std::max(hi, v);
    }
    snap->y[c] = plot::Range{lo, hi};
  }
  pause_view_.store(std::move(snap), std::memory_order_release);
}

void ScopeModel::publish_event(double pre_s, double post_s, double edge) {
  auto snap = std::make_shared<EventSnapshot>();
  BufferStore& src = live();
  plot::data::RingBuffer<f32>& tsrc = src.time();

  u64 first = 0, last = 0;
  live_window_locked(edge - std::max(0.0, pre_s), edge + std::max(0.0, post_s), first, last);
  const u64 n = last - first;

  snap->stamps.resize(static_cast<std::size_t>(n));
  if (n > 0) {
    tsrc.copy_from(first, n, snap->stamps.data());
    // Edge-relative from here on: the frozen event reads [-pre, +post] with the
    // edge at exactly 0, with no axis-level shift any other code path could
    // disagree with.
    for (auto& t : snap->stamps) t -= static_cast<f32>(edge);
  }

  snap->values.resize(channels_.size());
  snap->y.resize(channels_.size(), plot::Range{0.0, 0.0});
  snap->pyramids.resize(channels_.size());
  for (std::size_t c = 0; c < channels_.size(); ++c) {
    auto& vals = snap->values[c];
    vals.resize(static_cast<std::size_t>(n));
    if (n > 0) src.values(c).copy_from(first, n, vals.data());
    f32 lo = vals.empty() ? 0.0f : vals.front();
    f32 hi = lo;
    // The LOD index is folded into the bounds pass that was already here, so the
    // snapshot costs one O(n) walk per channel rather than two. Without a pyramid
    // the renderer's `level_for()` descends to level 0 and the whole capture is
    // drawn raw on every frame the event is held: see `EventSnapshot::pyramids`.
    auto& pyr = snap->pyramids[c];
    pyr.reserve(n);
    for (f32 v : vals) {
      lo = std::min(lo, v);
      hi = std::max(hi, v);
      pyr.append(v);
    }
    snap->y[c] = plot::Range{lo, hi};
  }

  // The span is read off the stamps that were actually captured, never off the
  // request: the ring may not reach back a full `pre_s`, and the producer stops the
  // capture at the first sample past `post_s` rather than exactly on it.
  snap->pre_s = snap->stamps.empty() ? 0.0 : -static_cast<double>(snap->stamps.front());
  snap->post_s = snap->stamps.empty() ? 0.0 : static_cast<double>(snap->stamps.back());
  event_.store(std::move(snap), std::memory_order_release);
  // Bumped after the snapshot is in place, so a consumer that sees the new sequence
  // number sees the snapshot it counts.
  event_seq_.fetch_add(1, std::memory_order_release);
}

void BufferStore::set_epoch(f64 epoch) noexcept {
  for (auto& c : channels_) c->set_epoch(epoch);
}

void ScopeModel::set_display_epoch(f64 e) noexcept {
  display_epoch_.store(e, std::memory_order_relaxed);
  if (store_) store_->set_epoch(e);
}

void ScopeModel::acquire_all() {
  if (store_) store_->acquire_all();
}

void ScopeModel::set_trigger_enabled(bool on) {
  std::lock_guard<std::mutex> lock(mutex_);
  trigger_.enabled = on;
  trigger_enabled_.store(on, std::memory_order_relaxed);
}

void ScopeModel::set_epoch_if_unset(f64 e) noexcept {
  if (epoch_ == 0.0) epoch_ = e;
}

}  // namespace udpscope