From 90c74578d18ff3d1ee5a6729fa79c9fe6984148e Mon Sep 17 00:00:00 2001 From: Ryan Pavlik Date: Tue, 23 Nov 2021 14:34:24 -0600 Subject: [PATCH] a/util: Refactor HistoryBuffer to be more like a standard container. --- src/xrt/auxiliary/math/m_relation_history.cpp | 69 +- .../auxiliary/util/u_template_historybuf.hpp | 937 +++++++++++++++++- tests/tests_history_buf.cpp | 181 ++++ 3 files changed, 1119 insertions(+), 68 deletions(-) diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp index ae050bc85..686926501 100644 --- a/src/xrt/auxiliary/math/m_relation_history.cpp +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -35,12 +35,12 @@ struct relation_history_entry uint64_t timestamp; }; -#define leng 4096 -#define power2 12 +constexpr size_t BufLen = 4096; +constexpr size_t power2 = 12; struct m_relation_history { - HistoryBuffer impl; + HistoryBuffer impl; bool has_first_sample; struct os_mutex mutex; }; @@ -71,11 +71,11 @@ m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation rhe.timestamp = timestamp; bool ret = false; os_mutex_lock(&rh->mutex); - // Don't evaluate the second condition if the length is 0 - rh->impl[0] will be NULL! - if ((!rh->has_first_sample) || (rhe.timestamp > rh->impl[0]->timestamp)) { + // if we aren't empty, we can compare against the latest timestamp. + if (rh->impl.empty() || rhe.timestamp > rh->impl.back().timestamp) { // Everything explodes if the timestamps in relation_history aren't monotonically increasing. If we get // a timestamp that's before the most recent timestamp in the buffer, just don't put it in the history. - rh->impl.push(rhe); + rh->impl.push_back(rhe); ret = true; } rh->has_first_sample = true; @@ -89,15 +89,14 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, XRT_TRACE_MARKER(); os_mutex_lock(&rh->mutex); m_relation_history_result ret = M_RELATION_HISTORY_RESULT_INVALID; - - if (rh->has_first_sample == 0 || at_timestamp_ns == 0) { + if (rh->impl.empty() || at_timestamp_ns == 0) { // Do nothing. You push nothing to the buffer you get nothing from the buffer. goto end; } { - uint64_t oldest_in_buffer = rh->impl[rh->impl.length() - 1]->timestamp; - uint64_t newest_in_buffer = rh->impl[0]->timestamp; + uint64_t oldest_in_buffer = rh->impl.front().timestamp; + uint64_t newest_in_buffer = rh->impl.back().timestamp; if (at_timestamp_ns > newest_in_buffer) { // The desired timestamp is after what our buffer contains. @@ -108,7 +107,7 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, U_LOG_T("Extrapolating %f s after the head of the buffer!", delta_s); - m_predict_relation(&rh->impl[0]->relation, delta_s, out_relation); + m_predict_relation(&rh->impl.back().relation, delta_s, out_relation); ret = M_RELATION_HISTORY_RESULT_PREDICTED; goto end; @@ -119,33 +118,37 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, diff_prediction_ns = at_timestamp_ns - oldest_in_buffer; double delta_s = time_ns_to_s(diff_prediction_ns); U_LOG_T("Extrapolating %f s before the tail of the buffer!", delta_s); - m_predict_relation(&rh->impl[rh->impl.length() - 1]->relation, delta_s, out_relation); + m_predict_relation(&rh->impl.front().relation, delta_s, out_relation); ret = M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED; - goto end; } U_LOG_T("Interpolating within buffer!"); #if 0 // Very slow - O(n) - but easier to read - int idx = 0; + size_t idx = 0; - for (int i = 0; i < rh->impl.length(); i++) { - if (rh->impl[i]->timestamp < at_timestamp_ns) { + for (size_t i = 0; i < rh->impl.size(); i++) { + auto ts = rh->impl.get_at_age(i)->timestamp; + if (ts == at_timestamp_ns) { + *out_relation = rh->impl.get_at_age(i)->relation; + ret = M_RELATION_HISTORY_RESULT_EXACT; + goto end; + } + + if (ts < at_timestamp_ns) { // If the entry we're looking at is before the input time idx = i; break; } } - U_LOG_T("Correct answer is %i", idx); + U_LOG_T("Correct answer is %li", idx); #else // Fast - O(log(n)) - but hard to read - int idx = leng / 2; // 2048 + int idx = BufLen / 2; // 2048 int step = idx; for (int i = power2 - 2; i >= -1; i--) { - uint64_t ts_after = rh->impl[idx - 1]->timestamp; - uint64_t ts_before = rh->impl[idx]->timestamp; // This is a little hack because any power of two looks like 0b0001000 (with the 1 in a // different place for each power). Bit-shift it and it either doubles or halves. In our case it @@ -154,11 +157,15 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, assert(step == (int)pow(2, i)); - if (idx >= rh->impl.length()) { + if (idx >= (int)rh->impl.size()) { // We'd be looking at an uninitialized value. Go back closer to the head of the buffer. idx -= step; continue; } + assert(idx > 0); + + uint64_t ts_after = rh->impl.get_at_age(idx - 1)->timestamp; + uint64_t ts_before = rh->impl.get_at_age(idx)->timestamp; if (ts_before == at_timestamp_ns || ts_after == at_timestamp_ns) { // exact match break; @@ -186,24 +193,24 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, #endif // Do the thing. - struct xrt_space_relation before = rh->impl[idx]->relation; - struct xrt_space_relation after = rh->impl[idx - 1]->relation; + struct xrt_space_relation before = rh->impl.get_at_age(idx)->relation; + struct xrt_space_relation after = rh->impl.get_at_age(idx - 1)->relation; - if (rh->impl[idx]->timestamp == at_timestamp_ns) { + if (rh->impl.get_at_age(idx)->timestamp == at_timestamp_ns) { // exact match: before *out_relation = before; ret = M_RELATION_HISTORY_RESULT_EXACT; goto end; } - if (rh->impl[idx - 1]->timestamp == at_timestamp_ns) { + if (rh->impl.get_at_age(idx - 1)->timestamp == at_timestamp_ns) { // exact match: after *out_relation = after; ret = M_RELATION_HISTORY_RESULT_EXACT; goto end; } int64_t diff_before, diff_after = 0; - diff_before = at_timestamp_ns - rh->impl[idx]->timestamp; - diff_after = rh->impl[idx - 1]->timestamp - at_timestamp_ns; + diff_before = at_timestamp_ns - rh->impl.get_at_age(idx)->timestamp; + diff_after = rh->impl.get_at_age(idx - 1)->timestamp - at_timestamp_ns; float amount_to_lerp = (float)diff_before / (float)(diff_before + diff_after); @@ -234,12 +241,12 @@ m_relation_history_get_latest(struct m_relation_history *rh, struct xrt_space_relation *out_relation) { os_mutex_lock(&rh->mutex); - if (rh->impl.length() == 0) { + if (rh->impl.empty()) { os_mutex_unlock(&rh->mutex); return false; } - *out_relation = rh->impl[0]->relation; - *out_time_ns = rh->impl[0]->timestamp; + *out_relation = rh->impl.back().relation; + *out_time_ns = rh->impl.back().timestamp; os_mutex_unlock(&rh->mutex); return true; } @@ -247,7 +254,7 @@ m_relation_history_get_latest(struct m_relation_history *rh, uint32_t m_relation_history_get_size(const struct m_relation_history *rh) { - return (uint32_t)rh->impl.length(); + return (uint32_t)rh->impl.size(); } void diff --git a/src/xrt/auxiliary/util/u_template_historybuf.hpp b/src/xrt/auxiliary/util/u_template_historybuf.hpp index b9dcbb50e..5d1684dbc 100644 --- a/src/xrt/auxiliary/util/u_template_historybuf.hpp +++ b/src/xrt/auxiliary/util/u_template_historybuf.hpp @@ -4,12 +4,16 @@ * @file * @brief Ringbuffer implementation for keeping track of the past state of things * @author Moses Turner - * @ingroup drv_ht + * @author Ryan Pavlik + * @ingroup aux_util */ #pragma once #include +#include +#include +#include #include @@ -19,55 +23,914 @@ namespace xrt::auxiliary::util { -template class HistoryBuffer -{ -private: - T internalBuffer[maxSize]; - int mTopIdx = 0; - int mLength = 0; +template class HistoryBuffer; - -public: - // clang-format off - int topIdx() const noexcept { return mTopIdx; } - int length() const noexcept { return mLength; } - // clang-format on - - /* Put something at the top, overwrite whatever was at the back*/ - void - push(const T inElement) +namespace detail { + //! All the bookkeeping for adapting a fixed-size array to a ring buffer. + template class RingBufferHelper { - mTopIdx++; - if (mTopIdx == maxSize) { - mTopIdx = 0; + private: + size_t latest_idx_ = 0; + size_t length_ = 0; + + //! Get the inner index of the front value: assumes not empty! + size_t + front_impl_() const noexcept + { + assert(!empty()); + // length will not exceed MaxSize, so this will not underflow + return (latest_idx_ + MaxSize - length_ + 1) % MaxSize; } - memcpy(&internalBuffer[mTopIdx], &inElement, sizeof(T)); - mLength++; - mLength = std::min(mLength, maxSize); + public: + //! Get the inner index for a given age (if possible) + bool + age_to_inner_index(size_t age, size_t &out_inner_idx) const noexcept + { + + if (empty()) { + return false; + } + if (age >= length_) { + return false; + } + // latest_idx_ is the same as latest_idx_ + MaxSize (when modulo MaxSize) so we add it to handle + // underflow with unsigned values + out_inner_idx = (latest_idx_ + MaxSize - age) % MaxSize; + return true; + } + + //! Get the inner index for a given index (if possible) + bool + index_to_inner_index(size_t index, size_t &out_inner_idx) const noexcept + { + + if (empty()) { + return false; + } + if (index >= length_) { + return false; + } + // Just add to the front index and take modulo MaxSize + out_inner_idx = (front_impl_() + index) % MaxSize; + return true; + } + + //! Is the buffer empty? + bool + empty() const noexcept + { + return length_ == 0; + } + + //! How many elements are in the buffer? + size_t + size() const noexcept + { + return length_; + } + + /*! + * @brief Update internal state for pushing an element to the back, and return the inner index to store + * the element at. + */ + size_t + push_back_location() noexcept + { + latest_idx_ = (latest_idx_ + 1) % MaxSize; + length_ = std::min(length_ + 1, MaxSize); + return latest_idx_; + } + + /*! + * @brief Record the logical removal of the front element, if any. + * + * Does nothing if the buffer is empty. + */ + void + pop_front() noexcept + { + if (!empty()) { + length_--; + } + } + + //! Get the inner index of the front value, or MaxSize if empty. + size_t + front_inner_index() const noexcept + { + if (empty()) { + return MaxSize; + } + return front_impl_(); + } + + //! Get the inner index of the back (latest) value, or MaxSize if empty. + size_t + back_inner_index() const noexcept + { + if (empty()) { + return MaxSize; + } + return latest_idx_; + } + }; + + /*! + * @brief Base class used by iterators and const_iterators of HistoryBuffer, providing substantial parts of the + * functionality. + * + * Using inheritance instead of composition here to more easily satisfy the C++ standard's requirements for + * iterators (e.g. that const iterators and iterators are comparable, etc.) + * + * All invalid instances will compare as equal, but they are not all equivalent: the "past the end" iterator is + * invalid to dereference, but is allowed to be decremented. + */ + template class RingBufferIteratorBase + { + static_assert(MaxSize < (std::numeric_limits::max() >> 1), "Cannot use most significant bit"); + // for internal use + RingBufferIteratorBase(const RingBufferHelper *helper, size_t index) + : buf_helper_(*helper), index_(index) + {} + + public: + //! Construct the "past the end" iterator that can be decremented safely + static RingBufferIteratorBase + end(const RingBufferHelper &helper) + { + return RingBufferIteratorBase(helper, helper.size()); + } + + //! Is this iterator valid? + bool + valid() const noexcept; + + //! Is this iterator valid? + explicit operator bool() const noexcept + { + return valid(); + } + + //! What is the index stored by this iterator? + size_t + index() const noexcept + { + return index_; + } + + /*! + * @brief True if this iterator is "irrecoverably" invalid (aka, cleared/default constructed). + * + * Implies !valid() + */ + bool + is_cleared() const noexcept + { + return index_ > MaxSize; + } + + protected: + //! default constructor + RingBufferIteratorBase() = default; + + //! Constructor from a helper and index + explicit RingBufferIteratorBase(const RingBufferHelper &helper, size_t index) + : buf_helper_(&helper), index_(index % MaxSize) + {} + + + /*! + * Clear member variables so we are bytewise identical to a default constructed instance, and thus + * irrecoverably invalid. + */ + void + clear() noexcept + { + *this = {}; + } + + /*! + * @brief Increment the internal index, given appropriate conditions. + * + * If we were already out of range (invalid), the members are cleared so we are irrecoverably invalid. + * However, you can reach the "past-the-end" element using this function and still decrement away from + * it. + */ + void + increment() noexcept + { + if (valid()) { + index_++; + } else { + // make sure we can't "back out" of invalid + clear(); + } + } + + /*! + * @brief Decrement the internal index, given appropriate conditions. + * + * If our index is not 0 and not our sentinel (MaxSize), we decrement. + * (This is a broader condition than "valid()": we want to be able to decrement() on the "past-the-end" + * iterator to get the last element, etc.) If we were already out of range (invalid) the members are + * cleared so we are irrecoverably invalid. + */ + void + decrement() noexcept + { + if (index_ > 0 && !is_cleared()) { + index_--; + } else { + // make sure we can't "back out" of invalid + clear(); + } + } + + public: + /*! + * @brief Compute the difference between two iterators. + * + * - If both are invalid, the result is 0. + * - If both are either valid or the "past-the-end" iterator, the result is the difference in (logical, + * not inner) index. + * + * @throws std::logic_error if one of the above conditions is not met. + */ + std::ptrdiff_t + operator-(const RingBufferIteratorBase &other) const; + + /*! + * @brief Increment by an arbitrary value. + */ + RingBufferIteratorBase & + operator+=(std::ptrdiff_t n) + { + if (is_cleared()) { + return *this; + } + + size_t n_clean = remap_to_range(n); + index_ = (index_ + n_clean) % MaxSize; + return *this; + } + + /*! + * @brief Decrement by an arbitrary value. + */ + RingBufferIteratorBase & + operator-=(std::ptrdiff_t n) + { + if (is_cleared()) { + return *this; + } + size_t n_clean = remap_to_range(n); + index_ = (index_ + MaxSize - n_clean) % MaxSize; + return *this; + } + /*! + * @brief Add a count. + */ + RingBufferIteratorBase + operator+(std::ptrdiff_t n) const + { + RingBufferIteratorBase ret = *this; + ret += n; + return ret; + } + + /*! + * @brief Subtract a count. + */ + RingBufferIteratorBase + operator-(std::ptrdiff_t n) const + { + RingBufferIteratorBase ret = *this; + ret -= n; + return ret; + } + + private: + //! Returns a value in [0, MaxSize) + static size_t + remap_to_range(std::ptrdiff_t n) + { + if (n < 0) { + size_t ret = MaxSize - (static_cast(-1 * n) % MaxSize); + assert(ret < MaxSize); + return ret; + } + return static_cast(n % MaxSize); + } + const RingBufferHelper *buf_helper_{nullptr}; + size_t index_{MaxSize + 1}; + }; + + /*! + * Equality comparison operator for @ref RingBufferIteratorBase, which is the base of all HistoryBuffer iterator + * types. + */ + template + static inline bool + operator==(RingBufferIteratorBase const &lhs, RingBufferIteratorBase const &rhs) noexcept + { + const bool lhs_valid = lhs.valid(); + const bool rhs_valid = rhs.valid(); + if (!lhs_valid && !rhs_valid) { + // all invalid iterators compare equal. + return true; + } + if (lhs_valid != rhs_valid) { + // valid is never equal to invalid. + return false; + } + // OK, so both are valid. Now, we look at the index + return lhs.index() == rhs.index(); } - T * // Hack comment to fix clang-format - operator[](int inIndex) + /*! + * Inequality comparison operator for @ref RingBufferIteratorBase, which is the base of all HistoryBuffer + * iterator types. + */ + template + static inline bool + operator!=(RingBufferIteratorBase const &lhs, RingBufferIteratorBase const &rhs) noexcept { - if (mLength == 0) { - return NULL; - } - assert(inIndex <= maxSize); - assert(inIndex >= 0); + return !(lhs == rhs); + } - int index = mTopIdx - inIndex; - if (index < 0) { - index = maxSize + index; + template class HistoryBufIterator; + + /** + * @brief Class template for const_iterator for HistoryBuffer + * + * @tparam T Container element type - must match HistoryBuffer + * @tparam MaxSize Maximum number of elements - must match HistoryBuffer + */ + template class HistoryBufConstIterator : public RingBufferIteratorBase + { + using base = RingBufferIteratorBase; + // for use internally + HistoryBufConstIterator(const HistoryBuffer *container, + RingBufferIteratorBase &&iter_base) + : base(std::move(iter_base)), container_(container) + {} + + public: + using iterator_category = std::random_access_iterator_tag; + using value_type = const T; + using difference_type = std::ptrdiff_t; + using pointer = const T *; + using reference = const T &; + //! Default-construct an (invalid) iterator. + HistoryBufConstIterator() = default; + + //! Construct from a container, its helper, and an index: mostly for internal use. + HistoryBufConstIterator(const HistoryBuffer &container, + const RingBufferHelper &helper, + size_t index); + + //! Implicit conversion from a non-const iterator + HistoryBufConstIterator(const HistoryBufIterator &nonconst); + + //! Construct the "past the end" iterator that can be decremented safely + static HistoryBufConstIterator + end(const HistoryBuffer &container, const RingBufferHelper &helper) + { + return {&container, RingBufferIteratorBase::end(helper)}; } - assert(index >= 0); - if (index > maxSize) { - assert(false); + //! Is this iterator valid? + bool + valid() const noexcept + { + return container_ != nullptr && base::valid(); } - return &internalBuffer[index]; + //! Is this iterator valid? + explicit operator bool() const noexcept + { + return valid(); + } + + //! Dereference operator: throws std::out_of_range if invalid + reference + operator*() const; + + //! Smart pointer operator: returns nullptr if invalid + pointer + operator->() const noexcept; + + //! Pre-increment: Advance, then return self. + HistoryBufConstIterator & + operator++() + { + base::increment(); + return *this; + } + + //! Post-increment: return a copy of initial state after incrementing self + HistoryBufConstIterator + operator++(int) + { + HistoryBufConstIterator tmp = *this; + base::increment(); + return tmp; + } + + //! Pre-decrement: Subtract, then return self. + HistoryBufConstIterator & + operator--() + { + base::decrement(); + return *this; + } + + //! Post-decrement: return a copy of initial state after decrementing self + HistoryBufConstIterator + operator--(int) + { + HistoryBufConstIterator tmp = *this; + base::decrement(); + return tmp; + } + + + using base::operator-; + + HistoryBufConstIterator & + operator+=(std::ptrdiff_t n) noexcept + { + static_cast(*this) += n; + + return *this; + } + + HistoryBufConstIterator & + operator-=(std::ptrdiff_t n) noexcept + { + static_cast(*this) -= n; + + return *this; + } + + //! Increment a copy of the iterator a given number of steps. + HistoryBufConstIterator + operator+(std::ptrdiff_t n) const noexcept + { + HistoryBufConstIterator ret(*this); + ret += n; + return ret; + } + + //! Decrement a copy of the iterator a given number of steps. + HistoryBufConstIterator + operator-(std::ptrdiff_t n) const noexcept + { + HistoryBufConstIterator ret(*this); + ret -= n; + return ret; + } + + private: + const HistoryBuffer *container_{nullptr}; + }; + + + /** + * @brief Class template for iterator for HistoryBuffer + * + * @see HistoryBufConstIterator for the const_iterator version + * + * @tparam T Container element type - must match HistoryBuffer + * @tparam MaxSize Maximum number of elements - must match HistoryBuffer + */ + template class HistoryBufIterator : public RingBufferIteratorBase + { + using base = RingBufferIteratorBase; + + // for use internally + HistoryBufIterator(HistoryBuffer *container, RingBufferIteratorBase &&iter_base) + : base(std::move(iter_base)), container_(container) + {} + + public: + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = T *; + using reference = T &; + + //! Default-construct an (invalid) iterator. + HistoryBufIterator() = default; + + //! Construct from a container, its helper, and an index: mostly for internal use. + HistoryBufIterator(HistoryBuffer &container, + const RingBufferHelper &helper, + size_t index); + + //! Construct the "past the end" iterator that can be decremented safely + static HistoryBufIterator + end(HistoryBuffer &container, const RingBufferHelper &helper) + { + return {&container, RingBufferIteratorBase::end(helper)}; + } + + //! Is this iterator valid? + bool + valid() const noexcept + { + return container_ != nullptr && base::valid(); + } + + //! Is this iterator valid? + explicit operator bool() const noexcept + { + return valid(); + } + + //! Dereference operator: throws std::out_of_range if invalid + reference + operator*() const; + + //! Smart pointer operator: returns nullptr if invalid + pointer + operator->() const noexcept; + + //! Pre-increment: Advance, then return self. + HistoryBufIterator & + operator++() + { + base::increment(); + return *this; + } + + //! Post-increment: return a copy of initial state after incrementing self + HistoryBufIterator + operator++(int) + { + HistoryBufIterator tmp = *this; + base::increment(); + return tmp; + } + + //! Pre-decrement: Subtract, then return self. + HistoryBufIterator & + operator--() + { + base::decrement(); + return *this; + } + + //! Post-decrement: return a copy of initial state after decrementing self + HistoryBufIterator + operator--(int) + { + HistoryBufIterator tmp = *this; + base::decrement(); + return tmp; + } + using base::operator-; + + HistoryBufIterator & + operator+=(std::ptrdiff_t n) noexcept + { + static_cast(*this) += n; + + return *this; + } + + HistoryBufIterator & + operator-=(std::ptrdiff_t n) noexcept + { + static_cast(*this) -= n; + + return *this; + } + + //! Increment a copy of the iterator a given number of steps. + HistoryBufIterator + operator+(std::ptrdiff_t n) const noexcept + { + HistoryBufIterator ret(*this); + ret += n; + return ret; + } + + //! Decrement a copy of the iterator a given number of steps. + HistoryBufIterator + operator-(std::ptrdiff_t n) const noexcept + { + HistoryBufIterator ret(*this); + ret -= n; + return ret; + } + + private: + HistoryBuffer *container_{nullptr}; + }; + + +} // namespace detail + +/*! + * @brief Stores some number of values in a ring buffer, overwriting the earliest-pushed-remaining element if out of + * room. + * + * Note this should only store value types, since there's no way to destroy elements other than overwriting them, and + * all elements are default-initialized upon construction of the container. + */ +template class HistoryBuffer +{ +private: + using container_t = std::array; + container_t internalBuffer{}; + detail::RingBufferHelper helper_; + + static_assert(MaxSize < (std::numeric_limits::max() >> 1), "Cannot use most significant bit"); + +public: + //! Is the buffer empty? + bool + empty() const noexcept + { + return helper_.empty(); + } + //! How many elements are in the buffer? + size_t + size() const noexcept + { + return helper_.size(); + } + + + /*! + * @brief Put something at the back, overwriting whatever was at the front if necessary. + * + * This is permitted to invalidate iterators. They won't be poisoned, but they will return something you don't + * expect. + */ + void + push_back(const T &element) + { + auto inner_index = helper_.push_back_location(); + internalBuffer[inner_index] = element; + } + + //! @overload + void + push_back(T &&element) + { + auto inner_index = helper_.push_back_location(); + internalBuffer[inner_index] = std::move(element); + } + + /*! + * @brief Logically remove the oldest element from the buffer. + * + * The value still remains in the backing container until overwritten, it just isn't accessible anymore. + * + * This invalidates iterators. They won't be poisoned, but they will return something you don't expect. + */ + void + pop_front() + { + helper_.pop_front(); + } + + /*! + * @brief Access something at a given age, where age 0 is the most recent value, age 1 precedes it, etc. + * + * Out of bounds accesses will return nullptr. + */ + T * + get_at_age(size_t age) noexcept + { + size_t inner_index = 0; + if (helper_.age_to_inner_index(age, inner_index)) { + return &internalBuffer[inner_index]; + } + return nullptr; + } + + + //! @overload + const T * + get_at_age(size_t age) const noexcept + { + size_t inner_index = 0; + if (helper_.age_to_inner_index(age, inner_index)) { + return &internalBuffer[inner_index]; + } + return nullptr; + } + + /*! + * @brief Access something at a given index, where 0 is the least-recent value still stored, index 1 follows it, + * etc. + * + * Out of bounds accesses will return nullptr. + */ + T * + get_at_index(size_t index) noexcept + { + size_t inner_index = 0; + if (helper_.index_to_inner_index(index, inner_index)) { + return &internalBuffer[inner_index]; + } + return nullptr; + } + + //! @overload + const T * + get_at_index(size_t index) const noexcept + { + size_t inner_index = 0; + if (helper_.index_to_inner_index(index, inner_index)) { + return &internalBuffer[inner_index]; + } + return nullptr; + } + + + + //! Access the ring buffer helper, mostly for implementation usage only. + const detail::RingBufferHelper & + helper() const noexcept + { + return helper_; + } + + using iterator = detail::HistoryBufIterator; + using const_iterator = detail::HistoryBufConstIterator; + + //! Get an iterator for the least-recent element. + iterator + begin() noexcept + { + return iterator(*this, helper_, 0); + } + + //! Get a "past the end" iterator + iterator + end() noexcept + { + return iterator::end(*this, helper_); + } + + //! Get a const iterator for the least-recent element. + const_iterator + cbegin() const noexcept + { + return const_iterator(*this, helper_, 0); + } + + //! Get a "past the end" const iterator + const_iterator + cend() const noexcept + { + return const_iterator::end(*this, helper_); + } + + //! Get a const iterator for the least-recent element. + const_iterator + begin() const noexcept + { + return cbegin(); + } + + //! Get a "past the end" const iterator + const_iterator + end() const noexcept + { + return cend(); + } + + /*! + * @brief Gets a reference to the front (least-recent) element in the buffer. + * @throws std::logic_error if buffer is empty + */ + T & + front() + { + if (empty()) { + throw std::logic_error("Cannot get the front of an empty buffer"); + } + return internalBuffer[helper_.front_inner_index()]; + } + //! @overload + const T & + front() const + { + if (empty()) { + throw std::logic_error("Cannot get the front of an empty buffer"); + } + return internalBuffer[helper_.front_inner_index()]; + } + + /*! + * @brief Gets a reference to the back (most-recent) element in the buffer. + * @throws std::logic_error if buffer is empty + */ + T & + back() + { + if (empty()) { + throw std::logic_error("Cannot get the back of an empty buffer"); + } + return internalBuffer[helper_.back_inner_index()]; + } + + //! @overload + const T & + back() const + { + if (empty()) { + throw std::logic_error("Cannot get the back of an empty buffer"); + } + return internalBuffer[helper_.back_inner_index()]; } }; +namespace detail { + + template + inline bool + RingBufferIteratorBase::valid() const noexcept + { + return index_ < MaxSize && buf_helper_ != nullptr && index_ < buf_helper_->size(); + } + + template + inline std::ptrdiff_t + detail::RingBufferIteratorBase::operator-(const RingBufferIteratorBase &other) const + { + const bool self_valid = valid(); + const bool other_valid = other.valid(); + if (!self_valid && !other_valid) { + return 0; + } + if (index_ < MaxSize && other.index_ < MaxSize) { + return static_cast(index_) - static_cast(other.index_); + } + throw std::logic_error( + "Tried to find the difference between an iterator that has no concrete index, and one that does."); + } + + template + inline HistoryBufIterator::HistoryBufIterator(HistoryBuffer &container, + const RingBufferHelper &helper, + size_t index) + : base(helper, index), container_(&container) + {} + + template + inline typename HistoryBufIterator::reference + HistoryBufIterator::operator*() const + { + auto ptr = container_->get_at_index(base::index()); + if (ptr == nullptr) { + throw std::out_of_range("Iterator index out of range"); + } + return *ptr; + } + + template + inline typename HistoryBufIterator::pointer + HistoryBufIterator::operator->() const noexcept + { + return container_->get_at_index(base::index()); + } + + template + inline HistoryBufConstIterator::HistoryBufConstIterator(const HistoryBuffer &container, + const RingBufferHelper &helper, + size_t index) + : base(helper, index), container_(&container) + {} + + template + inline detail::HistoryBufConstIterator::HistoryBufConstIterator( + const HistoryBufIterator &nonconst) + : base(nonconst), container_(nonconst.container_) + {} + + template + inline typename HistoryBufConstIterator::reference + HistoryBufConstIterator::operator*() const + { + auto ptr = container_->get_at_index(base::index()); + if (ptr == nullptr) { + throw std::out_of_range("Iterator index out of range"); + } + return *ptr; + } + + template + inline typename HistoryBufConstIterator::pointer + HistoryBufConstIterator::operator->() const noexcept + { + return container_->get_at_index(base::index()); + } +} // namespace detail } // namespace xrt::auxiliary::util diff --git a/tests/tests_history_buf.cpp b/tests/tests_history_buf.cpp index cf211fa8e..08ab4cbfc 100644 --- a/tests/tests_history_buf.cpp +++ b/tests/tests_history_buf.cpp @@ -8,6 +8,17 @@ #include #include +#include +using xrt::auxiliary::util::HistoryBuffer; + +template +static inline std::ostream & +operator<<(std::ostream &os, const xrt::auxiliary::util::detail::RingBufferIteratorBase &iter_base) +{ + os << "Iterator@[" << iter_base.index() << "]"; + return os; +} + #include "catch/catch.hpp" @@ -191,3 +202,173 @@ TEST_CASE("RelationHistory") CHECK(out_relation.pose.position.x > 2.f); } } + +TEST_CASE("u_template_historybuf") +{ + HistoryBuffer buffer; + SECTION("behavior when empty") + { + CHECK(buffer.empty()); + CHECK(0 == buffer.size()); + CHECK_FALSE(buffer.begin().valid()); + CHECK_FALSE(buffer.end().valid()); + CHECK(buffer.begin() == buffer.end()); + } + SECTION("behavior with one") + { + buffer.push_back(0); + CHECK_FALSE(buffer.empty()); + CHECK(buffer.size() == 1); + + // check iterators + CHECK(buffer.begin().valid()); + CHECK_FALSE(buffer.end().valid()); + CHECK_FALSE(buffer.begin() == buffer.end()); + { + auto it = buffer.end(); + // should be permanently cleared + ++it; + CHECK_FALSE(it.valid()); + --it; + CHECK_FALSE(it.valid()); + } + CHECK(buffer.begin() == buffer.cbegin()); + CHECK(buffer.end() == buffer.cend()); + + // can we decrement our past-the-end iterator to get the begin iterator? + CHECK(buffer.begin() == --(buffer.end())); + + // make sure post-decrement works right + CHECK_FALSE(buffer.begin() == (buffer.end())--); + + // make sure post-increment works right + CHECK(buffer.begin() == buffer.begin()++); + + // make sure pre-increment works right + CHECK_FALSE(buffer.begin() == ++(buffer.begin())); + + + // check contents + CHECK_NOTHROW(buffer.get_at_index(0)); + CHECK_FALSE(buffer.get_at_index(0) == nullptr); + CHECK(*buffer.get_at_index(0) == 0); + CHECK_FALSE(buffer.get_at_age(0) == nullptr); + CHECK(*buffer.get_at_age(0) == 0); + CHECK_NOTHROW(buffer.front()); + CHECK(buffer.front() == 0); + CHECK_NOTHROW(buffer.back()); + CHECK(buffer.back() == 0); + + CHECK(*buffer.begin() == buffer.front()); + } + + SECTION("behavior with two") + { + buffer.push_back(0); + buffer.push_back(1); + CHECK_FALSE(buffer.empty()); + CHECK(buffer.size() == 2); + SECTION("check iterators") + { + // check iterators + CHECK(buffer.begin().valid()); + CHECK_FALSE(buffer.end().valid()); + CHECK_FALSE(buffer.begin() == buffer.end()); + { + auto it = buffer.end(); + // should be permanently cleared + ++it; + CHECK_FALSE(it.valid()); + --it; + CHECK_FALSE(it.valid()); + } + CHECK(buffer.begin() == buffer.cbegin()); + CHECK(buffer.end() == buffer.cend()); + + // can we decrement our past-the-end iterator to get the begin iterator? + CHECK(buffer.begin() == --(--(buffer.end()))); + + // make sure post-decrement works right + CHECK_FALSE(buffer.begin() == (buffer.end())--); + + // make sure post-increment works right + CHECK(buffer.begin() == buffer.begin()++); + + // make sure pre-increment works right + CHECK_FALSE(buffer.begin() == ++(buffer.begin())); + } + SECTION("check contents") + { + // check contents + CHECK_NOTHROW(buffer.get_at_index(0)); + CHECK_FALSE(buffer.get_at_index(0) == nullptr); + CHECK(*buffer.get_at_index(0) == 0); + CHECK_FALSE(buffer.get_at_index(1) == nullptr); + CHECK(*buffer.get_at_index(1) == 1); + CHECK(buffer.get_at_index(2) == nullptr); + + CHECK_NOTHROW(buffer.get_at_age(0)); + CHECK_FALSE(buffer.get_at_age(0) == nullptr); + CHECK(*buffer.get_at_age(0) == 1); + CHECK_FALSE(buffer.get_at_age(1) == nullptr); + CHECK(*buffer.get_at_age(1) == 0); + + CHECK(buffer.get_at_age(2) == nullptr); + + CHECK_NOTHROW(buffer.front()); + CHECK(buffer.front() == 0); + + CHECK_NOTHROW(buffer.back()); + CHECK(buffer.back() == 1); + + CHECK(*buffer.begin() == buffer.front()); + CHECK(buffer.back() == *(--buffer.end())); + } + } + + SECTION("algorithm behavior with 3") + { + buffer.push_back(0); + buffer.push_back(2); + buffer.push_back(4); + CHECK_FALSE(buffer.empty()); + CHECK(buffer.size() == 3); + CHECK(buffer.begin() == std::find(buffer.begin(), buffer.end(), 0)); + CHECK(++(buffer.begin()) == std::find(buffer.begin(), buffer.end(), 2)); + CHECK(buffer.end() == std::find(buffer.begin(), buffer.end(), 5)); + + CHECK(++(buffer.begin()) == std::lower_bound(buffer.begin(), buffer.end(), 1)); + } +} + +TEST_CASE("IteratorBase") +{ + + HistoryBuffer buffer; + buffer.push_back(0); + buffer.push_back(2); + buffer.push_back(4); + using namespace xrt::auxiliary::util; + using iterator = typename HistoryBuffer::iterator; + iterator default_constructed{}; + iterator begin_constructed = buffer.begin(); + iterator end_constructed = buffer.end(); + + SECTION("Check default constructed") + { + CHECK_FALSE(default_constructed.valid()); + CHECK(default_constructed.is_cleared()); + } + SECTION("Check begin constructed") + { + CHECK(begin_constructed.valid()); + CHECK_FALSE(begin_constructed.is_cleared()); + CHECK((--begin_constructed).is_cleared()); + } + SECTION("Check end constructed") + { + CHECK_FALSE(end_constructed.valid()); + CHECK_FALSE(end_constructed.is_cleared()); + CHECK((++end_constructed).is_cleared()); + } +}