timestamped_ring_buffer.hpp

Go to the documentation of this file.

 
#pragma once
 
#include <commrat/platform/threading.hpp>
#include <commrat/platform/timestamp.hpp>
#include <commrat/messages.hpp>  // For TimsMessage
#include <sertial/containers/ring_buffer.hpp>
#include <optional>
#include <cmath>
#include <algorithm>
 
namespace commrat {
 
// ============================================================================
// Timestamp Accessor Trait (handles both direct .timestamp and .header.timestamp)
// ============================================================================
 
template<typename T>
struct TimestampAccessor {
    static uint64_t get(const T& msg) {
        return msg.timestamp;
    }
};
 
// Specialization for TimsMessage<T>
template<typename PayloadT>
struct TimestampAccessor<TimsMessage<PayloadT>> {
    static uint64_t get(const TimsMessage<PayloadT>& msg) {
        return msg.header.timestamp;
    }
};
 
enum class InterpolationMode {
    NEAREST,      
    BEFORE,       
    AFTER,        
    INTERPOLATE   
};
 
template<typename T, std::size_t MaxSize = 100>
class TimestampedRingBuffer {
    // Compile-time validation
    static_assert(MaxSize > 0, "MaxSize must be greater than 0");
    
public:
    using value_type = T;
    using size_type = std::size_t;
    
    // ========================================================================
    // Construction
    // ========================================================================
    
    explicit TimestampedRingBuffer(
        std::chrono::milliseconds default_tolerance = std::chrono::milliseconds(50)
    ) : default_tolerance_(default_tolerance) {}
    
    // ========================================================================
    // Capacity
    // ========================================================================
    
    size_type size() const {
        SharedLock lock(mutex_);
        return buffer_.size();
    }
    
    static constexpr size_type capacity() {
        return MaxSize;
    }
    
    bool empty() const {
        SharedLock lock(mutex_);
        return buffer_.empty();
    }
    
    bool full() const {
        SharedLock lock(mutex_);
        return buffer_.full();
    }
    
    void clear() {
        UniqueLockShared lock(mutex_);
        buffer_.clear();
        oldest_timestamp_ = 0;
        newest_timestamp_ = 0;
    }
    
    // ========================================================================
    // Modifiers
    // ========================================================================
    
    void push(const T& message) {
        UniqueLockShared lock(mutex_);
        
        // Validate timestamp ordering (debug only)
        #ifndef NDEBUG
        if (!buffer_.empty() && TimestampAccessor<T>::get(message) < newest_timestamp_) {
            // WARNING: Timestamps must be monotonically increasing
            // This is a logic error in the producer
        }
        #endif
        
        // Update timestamp bounds
        if (buffer_.empty()) {
            oldest_timestamp_ = TimestampAccessor<T>::get(message);
        } else if (buffer_.full()) {
            // Overwriting oldest - update lower bound
            oldest_timestamp_ = TimestampAccessor<T>::get(buffer_[1]);  // Second-oldest becomes oldest
        }
        newest_timestamp_ = TimestampAccessor<T>::get(message);
        
        buffer_.push_back(message);
    }
    
    void push(T&& message) {
        UniqueLockShared lock(mutex_);
        
        #ifndef NDEBUG
        if (!buffer_.empty() && TimestampAccessor<T>::get(message) < newest_timestamp_) {
            // WARNING: Timestamp order violation
        }
        #endif
        
        if (buffer_.empty()) {
            oldest_timestamp_ = TimestampAccessor<T>::get(message);
        } else if (buffer_.full()) {
            oldest_timestamp_ = TimestampAccessor<T>::get(buffer_[1]);
        }
        newest_timestamp_ = TimestampAccessor<T>::get(message);
        
        buffer_.push_back(std::move(message));
    }
    
    // ========================================================================
    // Timestamp-Based Lookup (Primary Feature for Phase 6)
    // ========================================================================
    
    std::optional<T> getData(
        uint64_t timestamp,
        std::chrono::milliseconds tolerance = std::chrono::milliseconds(-1),
        InterpolationMode mode = InterpolationMode::NEAREST
    ) const {
        SharedLock lock(mutex_);
        
        if (buffer_.empty()) {
            return std::nullopt;
        }
        
        // Use default tolerance if not specified
        if (tolerance.count() < 0) {
            tolerance = default_tolerance_;
        }
        
        // Convert tolerance from milliseconds to nanoseconds
        // Timestamps are in nanoseconds (from Time::now()), so tolerance must match
        uint64_t tolerance_ns = static_cast<uint64_t>(tolerance.count()) * 1'000'000ULL;
        
        // Quick bounds check (handle underflow for low timestamps)
        uint64_t lower_bound = (timestamp >= tolerance_ns) ? (timestamp - tolerance_ns) : 0;
        uint64_t upper_bound = timestamp + tolerance_ns;
        if (upper_bound < timestamp) {  // Overflow check
            upper_bound = UINT64_MAX;
        }
        
        if (upper_bound < oldest_timestamp_ || lower_bound > newest_timestamp_) {
            return std::nullopt;  // Requested timestamp outside buffer range
        }
        
        // Dispatch to mode-specific implementation
        switch (mode) {
            case InterpolationMode::NEAREST:
                return getData_nearest(timestamp, tolerance_ns);
            case InterpolationMode::BEFORE:
                return getData_before(timestamp, tolerance_ns);
            case InterpolationMode::AFTER:
                return getData_after(timestamp, tolerance_ns);
            case InterpolationMode::INTERPOLATE:
                // Future: Linear interpolation between messages
                // For now, fall back to NEAREST
                return getData_nearest(timestamp, tolerance_ns);
        }
        
        return std::nullopt;
    }
    
    std::pair<uint64_t, uint64_t> getTimestampRange() const {
        SharedLock lock(mutex_);
        if (buffer_.empty()) {
            return {0, 0};
        }
        return {oldest_timestamp_, newest_timestamp_};
    }
    
private:
    // ========================================================================
    // Internal Lookup Implementations
    // ========================================================================
    
    std::optional<T> getData_nearest(uint64_t timestamp, uint64_t tolerance_ns) const {
        if (buffer_.empty()) {
            return std::nullopt;
        }
        
        size_type best_idx = 0;
        uint64_t best_diff = std::abs(static_cast<int64_t>(TimestampAccessor<T>::get(buffer_[0]) - timestamp));
        
        // Linear search for minimum time difference
        for (size_type i = 1; i < buffer_.size(); ++i) {
            uint64_t diff = std::abs(static_cast<int64_t>(TimestampAccessor<T>::get(buffer_[i]) - timestamp));
            if (diff < best_diff) {
                best_diff = diff;
                best_idx = i;
            }
        }
        
        if (best_diff <= tolerance_ns) {
            return buffer_[best_idx];
        }
        
        return std::nullopt;
    }
    
    std::optional<T> getData_before(uint64_t timestamp, uint64_t tolerance_units) const {
        if (buffer_.empty()) {
            return std::nullopt;
        }
        
        // Search backwards from newest to oldest
        for (size_type i = buffer_.size(); i > 0; --i) {
            size_type idx = i - 1;
            if (TimestampAccessor<T>::get(buffer_[idx]) <= timestamp) {
                uint64_t diff = timestamp - TimestampAccessor<T>::get(buffer_[idx]);
                if (diff <= tolerance_units) {
                    return buffer_[idx];
                }
                break;  // Found newest candidate, but out of tolerance
            }
        }
        
        return std::nullopt;
    }
    
    std::optional<T> getData_after(uint64_t timestamp, uint64_t tolerance_units) const {
        if (buffer_.empty()) {
            return std::nullopt;
        }
        
        // Search forwards from oldest to newest
        for (size_type i = 0; i < buffer_.size(); ++i) {
            if (TimestampAccessor<T>::get(buffer_[i]) >= timestamp) {
                uint64_t diff = TimestampAccessor<T>::get(buffer_[i]) - timestamp;
                if (diff <= tolerance_units) {
                    return buffer_[i];
                }
                break;  // Found oldest candidate, but out of tolerance
            }
        }
        
        return std::nullopt;
    }
    
    // ========================================================================
    // Member Variables
    // ========================================================================
    
    mutable SharedMutex mutex_;                 
    sertial::RingBuffer<T, MaxSize> buffer_;    
    
    // Cached timestamp bounds (avoid scanning on every getData)
    uint64_t oldest_timestamp_{0};  
    uint64_t newest_timestamp_{0};  
    
    std::chrono::milliseconds default_tolerance_;  
};
 
} // namespace commrat