Initial commit

src/model.cpp

@@ -0,0 +1,378 @@
+// SPDX-License-Identifier: MIT
+// SPDX-FileCopyrightText: 2026 Martino Ferrari
+
+#include "model.hpp"
+
+#include <udps_client.h>
+
+#include <algorithm>
+#include <cmath>
+#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) {
+  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, and none on the shared clock either. A stride of
+  // `round(dt_i / dt_fastest)` throws away all but one sample per 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: the channel plots as a
+  // flat line. Decimating the shared clock to make a 600 s window fit did the same
+  // to *every* channel. The LOD min/max pyramid is what preserves the extremes at
+  // draw time, and it needs the real samples; the buffer simply holds as many as
+  // it holds, and the window dropdown is capped to that span.
+  for (std::size_t i = 0; i < n; ++i) p.stride[i] = 1;
+  p.time_stride = 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]);
+  }
+  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;
+    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);
+  store_ = std::make_unique<BufferStore>(part_, part_.span_s);
+
+  if (trigger_.channel >= static_cast<int>(channels_.size())) trigger_.channel = -1;
+}
+
+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_);
+  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::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});
+  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;
+    for (f32 v : vals) {
+      lo = std::min(lo, v);
+      hi = std::max(hi, 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);
+}
+
+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