a/util: Refactor HistoryBuffer to be more like a standard container.

src/xrt/auxiliary/math/m_relation_history.cpp

@@ -35,12 +35,12 @@ struct relation_history_entry
 	uint64_t timestamp;
 };
 
-#define leng 4096
+constexpr size_t BufLen = 4096;
-#define power2 12
+constexpr size_t power2 = 12;
 
 struct m_relation_history
 {
-	HistoryBuffer<struct relation_history_entry, leng> impl;
+	HistoryBuffer<struct relation_history_entry, BufLen> 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 we aren't empty, we can compare against the latest timestamp.
-	if ((!rh->has_first_sample) || (rhe.timestamp > rh->impl[0]->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->impl.empty() || at_timestamp_ns == 0) {
-	if (rh->has_first_sample == 0 || 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 oldest_in_buffer = rh->impl.front().timestamp;
-		uint64_t newest_in_buffer = rh->impl[0]->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++) {
+		for (size_t i = 0; i < rh->impl.size(); i++) {
-			if (rh->impl[i]->timestamp < at_timestamp_ns) {
+			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 before = rh->impl.get_at_age(idx)->relation;
-		struct xrt_space_relation after = rh->impl[idx - 1]->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_before = at_timestamp_ns - rh->impl.get_at_age(idx)->timestamp;
-		diff_after = rh->impl[idx - 1]->timestamp - at_timestamp_ns;
+		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_relation = rh->impl.back().relation;
-	*out_time_ns = rh->impl[0]->timestamp;
+	*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

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 <moses@collabora.com>
- * @ingroup drv_ht
+ * @author Ryan Pavlik <ryan.pavlik@collabora.com>
+ * @ingroup aux_util
  */
 
 #pragma once
 
 #include <algorithm>
+#include <array>
+#include <limits>
+#include <stdexcept>
 #include <assert.h>
 
 
@@ -19,55 +23,914 @@
 
 namespace xrt::auxiliary::util {
 
-template <typename T, int maxSize> class HistoryBuffer
+template <typename T, size_t MaxSize> class HistoryBuffer;
+
+namespace detail {
+	//! All the bookkeeping for adapting a fixed-size array to a ring buffer.
+	template <size_t MaxSize> class RingBufferHelper
+	{
+	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;
+		}
+
+	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 <size_t MaxSize> class RingBufferIteratorBase
+	{
+		static_assert(MaxSize < (std::numeric_limits<size_t>::max() >> 1), "Cannot use most significant bit");
+		// for internal use
+		RingBufferIteratorBase(const RingBufferHelper<MaxSize> *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<MaxSize> &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<MaxSize> &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<MaxSize> &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<size_t>(-1 * n) % MaxSize);
+				assert(ret < MaxSize);
+				return ret;
+			}
+			return static_cast<size_t>(n % MaxSize);
+		}
+		const RingBufferHelper<MaxSize> *buf_helper_{nullptr};
+		size_t index_{MaxSize + 1};
+	};
+
+	/*!
+	 * Equality comparison operator for @ref RingBufferIteratorBase, which is the base of all HistoryBuffer iterator
+	 * types.
+	 */
+	template <size_t MaxSize>
+	static inline bool
+	operator==(RingBufferIteratorBase<MaxSize> const &lhs, RingBufferIteratorBase<MaxSize> 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();
+	}
+
+	/*!
+	 * Inequality comparison operator for @ref RingBufferIteratorBase, which is the base of all HistoryBuffer
+	 * iterator types.
+	 */
+	template <size_t MaxSize>
+	static inline bool
+	operator!=(RingBufferIteratorBase<MaxSize> const &lhs, RingBufferIteratorBase<MaxSize> const &rhs) noexcept
+	{
+		return !(lhs == rhs);
+	}
+
+	template <typename T, size_t MaxSize> 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 <typename T, size_t MaxSize> class HistoryBufConstIterator : public RingBufferIteratorBase<MaxSize>
+	{
+		using base = RingBufferIteratorBase<MaxSize>;
+		// for use internally
+		HistoryBufConstIterator(const HistoryBuffer<T, MaxSize> *container,
+		                        RingBufferIteratorBase<MaxSize> &&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<T, MaxSize> &container,
+		                        const RingBufferHelper<MaxSize> &helper,
+		                        size_t index);
+
+		//! Implicit conversion from a non-const iterator
+		HistoryBufConstIterator(const HistoryBufIterator<T, MaxSize> &nonconst);
+
+		//! Construct the "past the end" iterator that can be decremented safely
+		static HistoryBufConstIterator
+		end(const HistoryBuffer<T, MaxSize> &container, const RingBufferHelper<MaxSize> &helper)
+		{
+			return {&container, RingBufferIteratorBase<MaxSize>::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.
+		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<base &>(*this) += n;
+
+			return *this;
+		}
+
+		HistoryBufConstIterator &
+		operator-=(std::ptrdiff_t n) noexcept
+		{
+			static_cast<base &>(*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<T, MaxSize> *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 <typename T, size_t MaxSize> class HistoryBufIterator : public RingBufferIteratorBase<MaxSize>
+	{
+		using base = RingBufferIteratorBase<MaxSize>;
+
+		// for use internally
+		HistoryBufIterator(HistoryBuffer<T, MaxSize> *container, RingBufferIteratorBase<MaxSize> &&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<T, MaxSize> &container,
+		                   const RingBufferHelper<MaxSize> &helper,
+		                   size_t index);
+
+		//! Construct the "past the end" iterator that can be decremented safely
+		static HistoryBufIterator
+		end(HistoryBuffer<T, MaxSize> &container, const RingBufferHelper<MaxSize> &helper)
+		{
+			return {&container, RingBufferIteratorBase<MaxSize>::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<base &>(*this) += n;
+
+			return *this;
+		}
+
+		HistoryBufIterator &
+		operator-=(std::ptrdiff_t n) noexcept
+		{
+			static_cast<base &>(*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<T, MaxSize> *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 <typename T, size_t MaxSize> class HistoryBuffer
 {
 private:
-	T internalBuffer[maxSize];
+	using container_t = std::array<T, MaxSize>;
-	int mTopIdx = 0;
+	container_t internalBuffer{};
-	int mLength = 0;
+	detail::RingBufferHelper<MaxSize> helper_;
 
+	static_assert(MaxSize < (std::numeric_limits<size_t>::max() >> 1), "Cannot use most significant bit");
 
 public:
-	// clang-format off
+	//! Is the buffer empty?
-	int topIdx() const noexcept { return mTopIdx; }
+	bool
-	int length() const noexcept { return mLength; }
+	empty() const noexcept
-	// clang-format on
+	{
+		return helper_.empty();
+	}
+	//! How many elements are in the buffer?
+	size_t
+	size() const noexcept
+	{
+		return helper_.size();
+	}
 
-	/* Put something at the top, overwrite whatever was at the back*/
+
+	/*!
+	 * @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(const T inElement)
+	push_back(const T &element)
 	{
-		mTopIdx++;
+		auto inner_index = helper_.push_back_location();
-		if (mTopIdx == maxSize) {
+		internalBuffer[inner_index] = element;
-			mTopIdx = 0;
 	}
 
-		memcpy(&internalBuffer[mTopIdx], &inElement, sizeof(T));
+	//! @overload
-		mLength++;
+	void
-		mLength = std::min(mLength, maxSize);
+	push_back(T &&element)
-	}
-
-	T * // Hack comment to fix clang-format
-	operator[](int inIndex)
 	{
-		if (mLength == 0) {
+		auto inner_index = helper_.push_back_location();
-			return NULL;
+		internalBuffer[inner_index] = std::move(element);
-		}
-		assert(inIndex <= maxSize);
-		assert(inIndex >= 0);
-
-		int index = mTopIdx - inIndex;
-		if (index < 0) {
-			index = maxSize + index;
 	}
 
-		assert(index >= 0);
+	/*!
-		if (index > maxSize) {
+	 * @brief Logically remove the oldest element from the buffer.
-			assert(false);
+	 *
+	 * 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();
 	}
 
-		return &internalBuffer[index];
+	/*!
+	 * @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<MaxSize> &
+	helper() const noexcept
+	{
+		return helper_;
+	}
+
+	using iterator = detail::HistoryBufIterator<T, MaxSize>;
+	using const_iterator = detail::HistoryBufConstIterator<T, MaxSize>;
+
+	//! 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 <size_t MaxSize>
+	inline bool
+	RingBufferIteratorBase<MaxSize>::valid() const noexcept
+	{
+		return index_ < MaxSize && buf_helper_ != nullptr && index_ < buf_helper_->size();
+	}
+
+	template <size_t MaxSize>
+	inline std::ptrdiff_t
+	detail::RingBufferIteratorBase<MaxSize>::operator-(const RingBufferIteratorBase<MaxSize> &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<std::ptrdiff_t>(index_) - static_cast<std::ptrdiff_t>(other.index_);
+		}
+		throw std::logic_error(
+		    "Tried to find the difference between an iterator that has no concrete index, and one that does.");
+	}
+
+	template <typename T, size_t MaxSize>
+	inline HistoryBufIterator<T, MaxSize>::HistoryBufIterator(HistoryBuffer<T, MaxSize> &container,
+	                                                          const RingBufferHelper<MaxSize> &helper,
+	                                                          size_t index)
+	    : base(helper, index), container_(&container)
+	{}
+
+	template <typename T, size_t MaxSize>
+	inline typename HistoryBufIterator<T, MaxSize>::reference
+	HistoryBufIterator<T, MaxSize>::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 <typename T, size_t MaxSize>
+	inline typename HistoryBufIterator<T, MaxSize>::pointer
+	HistoryBufIterator<T, MaxSize>::operator->() const noexcept
+	{
+		return container_->get_at_index(base::index());
+	}
+
+	template <typename T, size_t MaxSize>
+	inline HistoryBufConstIterator<T, MaxSize>::HistoryBufConstIterator(const HistoryBuffer<T, MaxSize> &container,
+	                                                                    const RingBufferHelper<MaxSize> &helper,
+	                                                                    size_t index)
+	    : base(helper, index), container_(&container)
+	{}
+
+	template <typename T, size_t MaxSize>
+	inline detail::HistoryBufConstIterator<T, MaxSize>::HistoryBufConstIterator(
+	    const HistoryBufIterator<T, MaxSize> &nonconst)
+	    : base(nonconst), container_(nonconst.container_)
+	{}
+
+	template <typename T, size_t MaxSize>
+	inline typename HistoryBufConstIterator<T, MaxSize>::reference
+	HistoryBufConstIterator<T, MaxSize>::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 <typename T, size_t MaxSize>
+	inline typename HistoryBufConstIterator<T, MaxSize>::pointer
+	HistoryBufConstIterator<T, MaxSize>::operator->() const noexcept
+	{
+		return container_->get_at_index(base::index());
+	}
+} // namespace detail
 } // namespace xrt::auxiliary::util

tests/tests_history_buf.cpp

@@ -8,6 +8,17 @@
 
 #include <iostream>
 #include <math/m_relation_history.h>
+#include <util/u_template_historybuf.hpp>
+using xrt::auxiliary::util::HistoryBuffer;
+
+template <size_t MaxSize>
+static inline std::ostream &
+operator<<(std::ostream &os, const xrt::auxiliary::util::detail::RingBufferIteratorBase<MaxSize> &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<int, 4> 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<int, 4> buffer;
+	buffer.push_back(0);
+	buffer.push_back(2);
+	buffer.push_back(4);
+	using namespace xrt::auxiliary::util;
+	using iterator = typename HistoryBuffer<int, 4>::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());
+	}
+}