vita49_output_sink.cpp

Go to the documentation of this file.

// SPDX-License-Identifier: GPL-2.0-only
//
// Copyright (c) 2026-present FERS Contributors (see AUTHORS.md).
//
// See the GNU GPLv2 LICENSE file in the FERS project root for more information.
 
#include "serial/vita49/vita49_output_sink.h"
 
#include <algorithm>
#include <chrono>
#include <optional>
#include <stdexcept>
 
#include "core/parameters.h"
#include "serial/vita49/udp_sender.h"
 
namespace serial::vita49
{
    namespace
    {
        constexpr auto kStatsEmitInterval = std::chrono::milliseconds(250);
        constexpr auto kPacketTraceEmitInterval = std::chrono::milliseconds(250);
        constexpr std::size_t kPacketTraceBatchSize = 64;
 
        [[nodiscard]] std::uint64_t defaultEpochNanoseconds()
        {
            const auto now = std::chrono::system_clock::now().time_since_epoch();
            return static_cast<std::uint64_t>(std::chrono::duration_cast<std::chrono::nanoseconds>(now).count());
        }
 
        [[nodiscard]] core::Vita49Timestamp toCoreTimestamp(const Timestamp& timestamp) noexcept
        {
            return core::Vita49Timestamp{.integer_seconds = timestamp.integer_seconds,
                                         .fractional_picoseconds = timestamp.fractional_picoseconds};
        }
 
        [[nodiscard]] std::optional<core::Vita49Timestamp>
        tryCoreTimestampFromEpoch(const std::uint64_t epoch_unix_nanoseconds,
                                  const RealType sample_time_seconds) noexcept
        {
            try
            {
                return toCoreTimestamp(timestampFromEpoch(epoch_unix_nanoseconds, sample_time_seconds));
            }
            catch (...)
            {
                return std::nullopt;
            }
        }
    }
 
    Vita49OutputSink::Vita49OutputSink(std::unique_ptr<DatagramSender> sender,
                                       core::ReceiverOutputTelemetryCallback telemetry_callback) :
        _telemetry_callback(std::move(telemetry_callback)), _provided_sender(std::move(sender))
    {
    }
 
    Vita49OutputSink::~Vita49OutputSink()
    {
        if (!_finalized)
        {
            try
            {
                (void)finalize();
            }
            catch (...)
            {
            }
        }
    }
 
    void Vita49OutputSink::initializeRun(const core::OutputConfig& config, std::string simulation_name)
    {
        std::scoped_lock const lock(_mutex);
        if (config.mode != core::OutputMode::Vita49Udp)
        {
            throw std::invalid_argument("Vita49OutputSink requires VITA output mode");
        }
        if (config.vita49.host.empty() || config.vita49.port == 0)
        {
            throw std::invalid_argument("VITA output requires destination host and port");
        }
        if (config.vita49.queue_depth == 0)
        {
            throw std::invalid_argument("VITA output queue depth must be positive");
        }
 
        _config = config;
        _simulation_name = std::move(simulation_name);
        _packetizer.reset();
        _sender = std::make_unique<PacedSender>(
            _provided_sender ? std::move(_provided_sender) : std::make_unique<UdpSender>(), config.vita49.queue_depth);
        _sender->open(config.vita49.host, config.vita49.port);
        _pending_contexts.clear();
        _pacing_started = false;
        _last_stats_emit = std::chrono::steady_clock::time_point::min();
        _last_packet_trace_emit = std::chrono::steady_clock::now();
        _pending_packet_traces.clear();
        _trace_sequence = 0;
        _initialized = true;
        _finalized = false;
        emitTelemetry({}, true);
    }
 
    std::uint32_t Vita49OutputSink::registerStream(const core::ReceiverStreamDescriptor& stream)
    {
        std::scoped_lock const lock(_mutex);
        const auto stream_id = _registry.registerStream(stream);
        if (!_streams.contains(stream_id))
        {
            StreamState state;
            state.descriptor = stream;
            state.stats.receiver_id = stream.receiver_id;
            state.stats.receiver_name = stream.receiver_name;
            state.stats.stream_id = stream_id;
            state.stats.mode = stream.mode.empty() ? "unknown" : stream.mode;
            state.stats.sample_rate = stream.sample_rate;
            state.stats.reference_frequency = stream.reference_frequency;
            _streams.emplace(stream_id, std::move(state));
            emitTelemetry({}, true);
        }
        return stream_id;
    }
 
    void Vita49OutputSink::openStream(const std::uint32_t stream_id, const RealType first_sample_time)
    {
        std::scoped_lock const lock(_mutex);
        emitContext(stream_id, first_sample_time, true, false);
        stateFor(stream_id).opened = true;
        emitTelemetry({}, true);
    }
 
    void Vita49OutputSink::submitBlock(const core::ReceiverSampleBlock& block) { submitBlocks({&block, 1}); }
 
    void Vita49OutputSink::submitBlocks(const std::span<const core::ReceiverSampleBlock> blocks)
    {
        std::scoped_lock const lock(_mutex);
        if (!_initialized || !_sender)
        {
            throw std::logic_error("VITA output sink has not been initialized");
        }
        emitTelemetry(consumeSenderDropsLocked(), false);
        ensurePacketizer();
 
        struct PacketAccounting
        {
            std::uint32_t stream_id = 0;
            std::uint64_t sample_count = 0;
            RealType first_sample_time = 0.0;
            RealType end_sample_time = 0.0;
            core::Vita49Timestamp timestamp;
            std::optional<core::Vita49Timestamp> end_timestamp;
            bool over_range = false;
        };
 
        std::vector<SerializedPacket> packets;
        std::vector<PacketAccounting> accounting;
        std::vector<std::uint32_t> context_stream_ids;
        for (const auto& block : blocks)
        {
            const auto stream_id = registerStream(block.stream);
            auto& state = stateFor(stream_id);
            if (!state.opened)
            {
                emitContext(stream_id, block.first_sample_time, true, false);
                state.opened = true;
            }
        }
        appendPendingContexts(packets, context_stream_ids);
 
        for (const auto& block : blocks)
        {
            const auto stream_id = registerStream(block.stream);
            auto& state = stateFor(stream_id);
            auto result = _packetizer->packetize(block, stream_id, state.packet_counts, state.sample_loss_pending);
            state.sample_loss_pending = false;
            const RealType sample_rate = block.sample_rate > 0.0 ? block.sample_rate : state.stats.sample_rate;
            for (const auto& packet : result.packets)
            {
                const auto end_sample_time = sample_rate > 0.0
                    ? packet.first_sample_time + static_cast<RealType>(packet.sample_count) / sample_rate
                    : packet.first_sample_time;
                accounting.push_back(PacketAccounting{
                    .stream_id = stream_id,
                    .sample_count = packet.sample_count,
                    .first_sample_time = packet.first_sample_time,
                    .end_sample_time = end_sample_time,
                    .timestamp = toCoreTimestamp(packet.timestamp),
                    .end_timestamp = tryCoreTimestampFromEpoch(_packetizer->epochUnixNanoseconds(), end_sample_time),
                    .over_range = packet.over_range});
            }
            state.stats.over_range_count += result.over_range_count;
            packets.insert(packets.end(), std::make_move_iterator(result.packets.begin()),
                           std::make_move_iterator(result.packets.end()));
        }
 
        std::stable_sort(packets.begin(), packets.end(), [](const SerializedPacket& lhs, const SerializedPacket& rhs)
                         { return lhs.first_sample_time < rhs.first_sample_time; });
        startPacing();
        if (!enqueuePackets(std::move(packets)))
        {
            return;
        }
 
        for (const auto stream_id : context_stream_ids)
        {
            ++stateFor(stream_id).stats.context_packets;
        }
        for (const auto& packet : accounting)
        {
            auto& state = stateFor(packet.stream_id);
            state.stats.samples_emitted += packet.sample_count;
            ++state.stats.packets_emitted;
            if (!state.stats.first_sample_time.has_value())
            {
                state.stats.first_sample_time = packet.first_sample_time;
                state.stats.first_timestamp = packet.timestamp;
            }
            state.stats.end_sample_time = packet.end_sample_time;
            state.stats.end_timestamp = packet.end_timestamp;
            if (packet.over_range)
            {
                state.over_range_pending = true;
            }
        }
    }
 
    void Vita49OutputSink::emitContextHeartbeat(const RealType simulation_time)
    {
        std::scoped_lock const lock(_mutex);
        emitTelemetry(consumeSenderDropsLocked(), false);
        for (auto& [stream_id, state] : _streams)
        {
            if (!state.closed && simulation_time - state.last_context_time >= 1.0)
            {
                emitContext(stream_id, simulation_time, false, false);
            }
        }
    }
 
    void Vita49OutputSink::closeStream(const std::uint32_t stream_id)
    {
        std::scoped_lock const lock(_mutex);
        emitTelemetry(consumeSenderDropsLocked(), false);
        auto& state = stateFor(stream_id);
        if (!state.closed)
        {
            emitContext(stream_id, state.last_context_time, false, true);
            state.closed = true;
            emitTelemetry({}, true);
        }
    }
 
    core::OutputStats Vita49OutputSink::finalize()
    {
        std::scoped_lock const lock(_mutex);
        if (_finalized)
        {
            auto stats = snapshotStatsLocked();
            emitTelemetry({}, true);
            return stats;
        }
 
        if (_sender)
        {
            _sender->flush();
            emitTelemetry(consumeSenderDropsLocked(), false);
        }
 
        for (auto& [stream_id, state] : _streams)
        {
            if (!state.closed)
            {
                emitContext(stream_id, state.last_context_time, false, true);
                state.closed = true;
            }
        }
 
        if (!_pacing_started && !_pending_contexts.empty())
        {
            ensurePacketizer();
            std::vector<SerializedPacket> packets;
            std::vector<std::uint32_t> context_stream_ids;
            appendPendingContexts(packets, context_stream_ids);
            std::stable_sort(packets.begin(), packets.end(),
                             [](const SerializedPacket& lhs, const SerializedPacket& rhs)
                             { return lhs.first_sample_time < rhs.first_sample_time; });
            startPacing();
            if (enqueuePackets(std::move(packets)))
            {
                for (const auto stream_id : context_stream_ids)
                {
                    ++stateFor(stream_id).stats.context_packets;
                }
            }
        }
 
        if (_sender)
        {
            _sender->stop();
            emitTelemetry(consumeSenderDropsLocked(), false);
        }
 
        core::OutputStats stats{.mode = core::OutputMode::Vita49Udp,
                                .epoch_unix_nanoseconds = _packetizer
                                    ? std::optional<std::uint64_t>(_packetizer->epochUnixNanoseconds())
                                    : _config.vita49.epoch_unix_nanoseconds,
                                .streams = {}};
        for (auto& [stream_id, state] : _streams)
        {
            if (_sender)
            {
                state.stats.late_data_packet_count = _sender->lateDataPacketCount(stream_id);
                state.stats.late_context_packet_count = _sender->lateContextPacketCount(stream_id);
            }
            stats.streams.push_back(state.stats);
        }
        _finalized = true;
        emitTelemetry({}, true);
        return stats;
    }
 
    core::OutputStats Vita49OutputSink::snapshotStats() const
    {
        std::scoped_lock const lock(_mutex);
        return snapshotStatsLocked();
    }
 
    Vita49OutputSink::StreamState& Vita49OutputSink::stateFor(const std::uint32_t stream_id)
    {
        const auto found = _streams.find(stream_id);
        if (found == _streams.end())
        {
            throw std::out_of_range("Unknown VITA stream ID");
        }
        return found->second;
    }
 
    const Vita49OutputSink::StreamState& Vita49OutputSink::stateFor(const std::uint32_t stream_id) const
    {
        const auto found = _streams.find(stream_id);
        if (found == _streams.end())
        {
            throw std::out_of_range("Unknown VITA stream ID");
        }
        return found->second;
    }
 
    void Vita49OutputSink::ensurePacketizer()
    {
        if (_packetizer)
        {
            return;
        }
        const auto epoch_ns = _config.vita49.epoch_unix_nanoseconds.value_or(defaultEpochNanoseconds());
        _packetizer =
            std::make_unique<Vita49Packetizer>(epoch_ns, _config.vita49.adc_fullscale, _config.vita49.max_udp_payload);
    }
 
    void Vita49OutputSink::startPacing()
    {
        if (_pacing_started)
        {
            return;
        }
        if (!_sender)
        {
            throw std::logic_error("VITA paced sender is unavailable");
        }
        _sender->start(params::startTime());
        _pacing_started = true;
    }
 
    void Vita49OutputSink::appendPendingContexts(std::vector<SerializedPacket>& packets,
                                                 std::vector<std::uint32_t>& context_stream_ids)
    {
        packets.reserve(packets.size() + _pending_contexts.size());
        context_stream_ids.reserve(context_stream_ids.size() + _pending_contexts.size());
        for (const auto& pending : _pending_contexts)
        {
            packets.push_back(buildContextPacket(pending.stream_id, pending.simulation_time, pending.stream_open,
                                                 pending.stream_close));
            context_stream_ids.push_back(pending.stream_id);
        }
        _pending_contexts.clear();
    }
 
    core::OutputStats Vita49OutputSink::snapshotStatsLocked() const
    {
        core::OutputStats stats{.mode = core::OutputMode::Vita49Udp,
                                .epoch_unix_nanoseconds = _packetizer
                                    ? std::optional<std::uint64_t>(_packetizer->epochUnixNanoseconds())
                                    : _config.vita49.epoch_unix_nanoseconds,
                                .streams = {}};
        for (const auto& [stream_id, state] : _streams)
        {
            auto stream_stats = state.stats;
            if (_sender)
            {
                stream_stats.late_data_packet_count = _sender->lateDataPacketCount(stream_id);
                stream_stats.late_context_packet_count = _sender->lateContextPacketCount(stream_id);
            }
            stats.streams.push_back(std::move(stream_stats));
        }
        return stats;
    }
 
    std::vector<core::ReceiverOutputPacketTrace> Vita49OutputSink::consumeSenderDropsLocked()
    {
        std::vector<core::ReceiverOutputPacketTrace> traces;
        if (!_sender)
        {
            return traces;
        }
 
        for (const auto& dropped : _sender->consumeDroppedDatagrams())
        {
            applyDropped(dropped);
            if (_config.vita49.packet_trace_enabled)
            {
                traces.push_back(makeDropTrace(dropped));
            }
        }
        return traces;
    }
 
    bool Vita49OutputSink::enqueuePacket(SerializedPacket&& packet)
    {
        if (!_sender)
        {
            throw std::logic_error("VITA paced sender is unavailable");
        }
 
        std::vector<core::ReceiverOutputPacketTrace> traces;
        std::optional<core::ReceiverOutputPacketTrace> sent_trace;
        if (_config.vita49.packet_trace_enabled)
        {
            sent_trace = makeTrace(packet, packet.context_packet ? "context" : "data");
        }
        const auto result = _sender->enqueue(std::move(packet));
        if (_config.vita49.packet_trace_enabled && result.dropped)
        {
            traces.push_back(makeDropTrace(*result.dropped));
        }
        if (result.dropped)
        {
            applyDropped(*result.dropped);
        }
        if (result.enqueued && sent_trace.has_value())
        {
            traces.push_back(std::move(*sent_trace));
        }
        emitTelemetry(std::move(traces), false);
        return result.enqueued;
    }
 
    bool Vita49OutputSink::enqueuePackets(std::vector<SerializedPacket> packets)
    {
        if (!_sender)
        {
            throw std::logic_error("VITA paced sender is unavailable");
        }
 
        std::vector<core::ReceiverOutputPacketTrace> traces;
        if (_config.vita49.packet_trace_enabled)
        {
            traces.reserve(packets.size());
            for (const auto& packet : packets)
            {
                traces.push_back(makeTrace(packet, packet.context_packet ? "context" : "data"));
            }
        }
        const bool enqueued = _sender->enqueueBatch(std::move(packets));
        if (!enqueued)
        {
            traces.clear();
        }
        emitTelemetry(std::move(traces), false);
        return enqueued;
    }
 
    void Vita49OutputSink::emitTelemetry(std::vector<core::ReceiverOutputPacketTrace> packets, const bool force_stats)
    {
        if (!_telemetry_callback)
        {
            return;
        }
 
        std::optional<core::OutputStats> stats;
        const auto now = std::chrono::steady_clock::now();
        if (force_stats || _last_stats_emit == std::chrono::steady_clock::time_point::min() ||
            now - _last_stats_emit >= kStatsEmitInterval)
        {
            stats = snapshotStatsLocked();
            _last_stats_emit = now;
        }
 
        for (auto& packet : packets)
        {
            packet.sequence = ++_trace_sequence;
            _pending_packet_traces.push_back(std::move(packet));
        }
 
        std::vector<core::ReceiverOutputPacketTrace> packet_batch;
        const bool trace_interval_elapsed = _last_packet_trace_emit == std::chrono::steady_clock::time_point::min() ||
            now - _last_packet_trace_emit >= kPacketTraceEmitInterval;
        if (!_pending_packet_traces.empty() &&
            (force_stats || _pending_packet_traces.size() >= kPacketTraceBatchSize || trace_interval_elapsed))
        {
            packet_batch = std::move(_pending_packet_traces);
            _pending_packet_traces.clear();
            _last_packet_trace_emit = now;
        }
 
        if (!stats.has_value() && packet_batch.empty())
        {
            return;
        }
        _telemetry_callback(stats, packet_batch);
    }
 
    core::ReceiverOutputPacketTrace Vita49OutputSink::makeTrace(const SerializedPacket& packet, std::string event) const
    {
        return core::ReceiverOutputPacketTrace{.sequence = 0,
                                               .event = std::move(event),
                                               .stream_id = packet.stream_id,
                                               .byte_count = packet.bytes.size(),
                                               .sample_count = packet.sample_count,
                                               .first_sample_time = packet.first_sample_time,
                                               .timestamp = toCoreTimestamp(packet.timestamp),
                                               .data_packet = packet.data_packet,
                                               .context_packet = packet.context_packet,
                                               .dropped = false,
                                               .over_range = packet.over_range,
                                               .sample_loss = packet.sample_loss};
    }
 
    core::ReceiverOutputPacketTrace Vita49OutputSink::makeDropTrace(const DroppedDatagram& dropped) const
    {
        return core::ReceiverOutputPacketTrace{.sequence = 0,
                                               .event = "drop",
                                               .stream_id = dropped.stream_id,
                                               .byte_count = 0,
                                               .sample_count = dropped.sample_count,
                                               .first_sample_time = 0.0,
                                               .timestamp = std::nullopt,
                                               .data_packet = dropped.data_packet,
                                               .context_packet = dropped.context_packet,
                                               .dropped = true,
                                               .over_range = false,
                                               .sample_loss = true};
    }
 
    SerializedPacket Vita49OutputSink::buildContextPacket(const std::uint32_t stream_id, const RealType simulation_time,
                                                          const bool stream_open, const bool stream_close)
    {
        if (!_packetizer)
        {
            throw std::logic_error("VITA packetizer is unavailable");
        }
        auto& state = stateFor(stream_id);
        const RealType context_time = simulation_time <= -1.0e200 ? 0.0 : simulation_time;
        const auto timestamp = timestampFromEpoch(_packetizer->epochUnixNanoseconds(), context_time);
        const ContextBuildRequest request{.stream = state.descriptor,
                                          .stream_id = stream_id,
                                          .simulation_name = _simulation_name,
                                          .adc_fullscale = _packetizer->adcFullscale(),
                                          .timestamp = timestamp,
                                          .packet_count = state.packet_counts.next(),
                                          .valid_data = true,
                                          .calibrated_time = true,
                                          .reference_lock = true,
                                          .over_range = state.over_range_pending,
                                          .sample_loss = state.sample_loss_pending,
                                          .stream_open = stream_open,
                                          .stream_close = stream_close};
        const auto context = Vita49ContextBuilder::build(request);
        auto packet = _packetizer->makeContextPacket(context);
        packet.first_sample_time = context_time;
        state.last_context_time = context_time;
        state.sample_loss_pending = false;
        state.over_range_pending = false;
        return packet;
    }
 
    void Vita49OutputSink::emitContext(const std::uint32_t stream_id, const RealType simulation_time,
                                       const bool stream_open, const bool stream_close)
    {
        if (!_pacing_started)
        {
            const RealType context_time = simulation_time <= -1.0e200 ? 0.0 : simulation_time;
            _pending_contexts.push_back(PendingContext{.stream_id = stream_id,
                                                       .simulation_time = context_time,
                                                       .stream_open = stream_open,
                                                       .stream_close = stream_close});
            stateFor(stream_id).last_context_time = context_time;
            return;
        }
        auto packet = buildContextPacket(stream_id, simulation_time, stream_open, stream_close);
        if (enqueuePacket(std::move(packet)))
        {
            ++stateFor(stream_id).stats.context_packets;
        }
    }
 
    void Vita49OutputSink::applyDropped(const DroppedDatagram& dropped)
    {
        if (dropped.stream_id == 0 || !_streams.contains(dropped.stream_id))
        {
            return;
        }
        auto& state = stateFor(dropped.stream_id);
        ++state.stats.packets_dropped;
        state.stats.samples_dropped += dropped.sample_count;
        if (dropped.data_packet || (!dropped.context_packet && dropped.sample_count > 0))
        {
            state.stats.packets_emitted -= std::min<std::uint64_t>(state.stats.packets_emitted, 1u);
            state.stats.samples_emitted -= std::min(state.stats.samples_emitted, dropped.sample_count);
        }
        else if (dropped.context_packet)
        {
            state.stats.context_packets -= std::min<std::uint64_t>(state.stats.context_packets, 1u);
        }
        state.sample_loss_pending = true;
    }
 
    std::unique_ptr<core::ReceiverOutputSink>
    makeVita49OutputSink(core::ReceiverOutputTelemetryCallback telemetry_callback)
    {
        return std::make_unique<Vita49OutputSink>(nullptr, std::move(telemetry_callback));
    }
}