diff --git a/wpiutil/src/main/native/include/wpi/static_circular_buffer.h b/wpiutil/src/main/native/include/wpi/static_circular_buffer.h new file mode 100644 index 0000000000..8329917050 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/static_circular_buffer.h @@ -0,0 +1,276 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +namespace wpi { + +/** + * This is a simple circular buffer so we don't need to "bucket brigade" copy + * old values. + */ +template +class static_circular_buffer { + public: + static_assert(N > 0, "The circular buffer size shouldn't be zero."); + + constexpr static_circular_buffer() = default; + + class iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = T*; + using reference = T&; + + iterator(static_circular_buffer* buffer, size_t index) + : m_buffer(buffer), m_index(index) {} + + iterator& operator++() { + ++m_index; + return *this; + } + iterator operator++(int) { + iterator retval = *this; + ++(*this); + return retval; + } + bool operator==(const iterator& other) const { + return m_buffer == other.m_buffer && m_index == other.m_index; + } + bool operator!=(const iterator& other) const { return !(*this == other); } + reference operator*() { return (*m_buffer)[m_index]; } + + private: + static_circular_buffer* m_buffer; + size_t m_index; + }; + + class const_iterator { + public: + using iterator_category = std::forward_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = T*; + using const_reference = const T&; + + const_iterator(const static_circular_buffer* buffer, size_t index) + : m_buffer(buffer), m_index(index) {} + + const_iterator& operator++() { + ++m_index; + return *this; + } + const_iterator operator++(int) { + const_iterator retval = *this; + ++(*this); + return retval; + } + bool operator==(const const_iterator& other) const { + return m_buffer == other.m_buffer && m_index == other.m_index; + } + bool operator!=(const const_iterator& other) const { + return !(*this == other); + } + const_reference operator*() const { return (*m_buffer)[m_index]; } + + private: + const static_circular_buffer* m_buffer; + size_t m_index; + }; + + iterator begin() { return iterator(this, 0); } + iterator end() { + return iterator(this, ::wpi::static_circular_buffer::size()); + } + + const_iterator begin() const { return const_iterator(this, 0); } + const_iterator end() const { + return const_iterator(this, ::wpi::static_circular_buffer::size()); + } + + const_iterator cbegin() const { return const_iterator(this, 0); } + const_iterator cend() const { + return const_iterator(this, ::wpi::static_circular_buffer::size()); + } + + /** + * Returns number of elements in buffer + */ + size_t size() const { return m_length; } + + /** + * Returns value at front of buffer + */ + T& front() { return (*this)[0]; } + + /** + * Returns value at front of buffer + */ + const T& front() const { return (*this)[0]; } + + /** + * Returns value at back of buffer + * + * If there are no elements in the buffer, calling this function results in + * undefined behavior. + */ + T& back() { return m_data[(m_front + m_length - 1) % N]; } + + /** + * Returns value at back of buffer + * + * If there are no elements in the buffer, calling this function results in + * undefined behavior. + */ + const T& back() const { return m_data[(m_front + m_length - 1) % N]; } + + /** + * Push a new value onto the front of the buffer. + * + * The value at the back is overwritten if the buffer is full. + */ + void push_front(T value) { + m_front = ModuloDec(m_front); + + m_data[m_front] = value; + + if (m_length < N) { + m_length++; + } + } + + /** + * Push a new value onto the back of the buffer. + * + * The value at the front is overwritten if the buffer is full. + */ + void push_back(T value) { + m_data[(m_front + m_length) % N] = value; + + if (m_length < N) { + m_length++; + } else { + // Increment front if buffer is full to maintain size + m_front = ModuloInc(m_front); + } + } + + /** + * Push a new value onto the front of the buffer that is constructed with the + * provided constructor arguments. + * + * The value at the back is overwritten if the buffer is full. + */ + template + void emplace_front(Args&&... args) { + m_front = ModuloDec(m_front); + + m_data[m_front] = T{args...}; + + if (m_length < N) { + m_length++; + } + } + + /** + * Push a new value onto the back of the buffer that is constructed with the + * provided constructor arguments. + * + * The value at the front is overwritten if the buffer is full. + */ + template + void emplace_back(Args&&... args) { + m_data[(m_front + m_length) % N] = T{args...}; + + if (m_length < N) { + m_length++; + } else { + // Increment front if buffer is full to maintain size + m_front = ModuloInc(m_front); + } + } + + /** + * Pop value at front of buffer. + * + * If there are no elements in the buffer, calling this function results in + * undefined behavior. + */ + T pop_front() { + T& temp = m_data[m_front]; + m_front = ModuloInc(m_front); + m_length--; + return temp; + } + + /** + * Pop value at back of buffer. + * + * If there are no elements in the buffer, calling this function results in + * undefined behavior. + */ + T pop_back() { + m_length--; + return m_data[(m_front + m_length) % N]; + } + + /** + * Empties internal buffer. + */ + void reset() { + m_front = 0; + m_length = 0; + } + + /** + * @return Element at index starting from front of buffer. + */ + T& operator[](size_t index) { return m_data[(m_front + index) % N]; } + + /** + * @return Element at index starting from front of buffer. + */ + const T& operator[](size_t index) const { + return m_data[(m_front + index) % N]; + } + + private: + std::array m_data; + + // Index of element at front of buffer + size_t m_front = 0; + + // Number of elements used in buffer + size_t m_length = 0; + + /** + * Increment an index modulo the length of the buffer. + * + * @return The result of the modulo operation. + */ + size_t ModuloInc(size_t index) { return (index + 1) % N; } + + /** + * Decrement an index modulo the length of the buffer. + * + * @return The result of the modulo operation. + */ + size_t ModuloDec(size_t index) { + if (index == 0) { + return N - 1; + } else { + return index - 1; + } + } +}; + +} // namespace wpi diff --git a/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp b/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp new file mode 100644 index 0000000000..72da38b176 --- /dev/null +++ b/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp @@ -0,0 +1,151 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "wpi/static_circular_buffer.h" // NOLINT(build/include_order) + +#include + +#include "gtest/gtest.h" + +static const std::array values = { + {751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, + 421.125, 799.913}}; + +static const std::array pushFrontOut = { + {799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}}; + +static const std::array pushBackOut = { + {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}}; + +TEST(StaticCircularBufferTest, PushFrontTest) { + wpi::static_circular_buffer queue; + + for (auto& value : values) { + queue.push_front(value); + } + + for (size_t i = 0; i < pushFrontOut.size(); ++i) { + EXPECT_EQ(pushFrontOut[i], queue[i]); + } +} + +TEST(StaticCircularBufferTest, PushBackTest) { + wpi::static_circular_buffer queue; + + for (auto& value : values) { + queue.push_back(value); + } + + for (size_t i = 0; i < pushBackOut.size(); ++i) { + EXPECT_EQ(pushBackOut[i], queue[i]); + } +} + +TEST(StaticCircularBufferTest, EmplaceFrontTest) { + wpi::static_circular_buffer queue; + + for (auto& value : values) { + queue.emplace_front(value); + } + + for (size_t i = 0; i < pushFrontOut.size(); ++i) { + EXPECT_EQ(pushFrontOut[i], queue[i]); + } +} + +TEST(StaticCircularBufferTest, EmplaceBackTest) { + wpi::static_circular_buffer queue; + + for (auto& value : values) { + queue.emplace_back(value); + } + + for (size_t i = 0; i < pushBackOut.size(); ++i) { + EXPECT_EQ(pushBackOut[i], queue[i]); + } +} + +TEST(StaticCircularBufferTest, PushPopTest) { + wpi::static_circular_buffer queue; + + // Insert three elements into the buffer + queue.push_back(1.0); + queue.push_back(2.0); + queue.push_back(3.0); + + EXPECT_EQ(1.0, queue[0]); + EXPECT_EQ(2.0, queue[1]); + EXPECT_EQ(3.0, queue[2]); + + /* + * The buffer is full now, so pushing subsequent elements will overwrite the + * front-most elements. + */ + + queue.push_back(4.0); // Overwrite 1 with 4 + + // The buffer now contains 2, 3 and 4 + EXPECT_EQ(2.0, queue[0]); + EXPECT_EQ(3.0, queue[1]); + EXPECT_EQ(4.0, queue[2]); + + queue.push_back(5.0); // Overwrite 2 with 5 + + // The buffer now contains 3, 4 and 5 + EXPECT_EQ(3.0, queue[0]); + EXPECT_EQ(4.0, queue[1]); + EXPECT_EQ(5.0, queue[2]); + + EXPECT_EQ(5.0, queue.pop_back()); // 5 is removed + + // The buffer now contains 3 and 4 + EXPECT_EQ(3.0, queue[0]); + EXPECT_EQ(4.0, queue[1]); + + EXPECT_EQ(3.0, queue.pop_front()); // 3 is removed + + // Leaving only one element with value == 4 + EXPECT_EQ(4.0, queue[0]); +} + +TEST(StaticCircularBufferTest, ResetTest) { + wpi::static_circular_buffer queue; + + for (size_t i = 1; i < 6; ++i) { + queue.push_back(i); + } + + queue.reset(); + + EXPECT_EQ(queue.size(), size_t{0}); +} + +TEST(StaticCircularBufferTest, IteratorTest) { + wpi::static_circular_buffer queue; + + queue.push_back(1.0); + queue.push_back(2.0); + queue.push_back(3.0); + queue.push_back(4.0); // Overwrite 1 with 4 + + // The buffer now contains 2, 3 and 4 + const std::array values = {2.0, 3.0, 4.0}; + + // iterator + int i = 0; + for (auto& elem : queue) { + EXPECT_EQ(values[i], elem); + ++i; + } + + // const_iterator + i = 0; + for (const auto& elem : queue) { + EXPECT_EQ(values[i], elem); + ++i; + } +}