CircularIndex: alternate index management for full capacity

Comparing head/tail indexes cannot distinguish between empty/full cases,
so we end up wasting one item in the circular buffer. This also limits
the smallest and efficient size choice to be 4.

If the circular buffer is large, there's no issue, however for the
motion planner the block size is significant, and I was planning to use
exactly a ring buffer of two: one busy block, plus one planned.

Modify the indexer to store the internal "index" (aka cursor) pointer to
be one extra bit deeper than the final index. Comparing the underlying
cursor allow to distinguish the empty/full case due to the extra bit,
while producing the final index requires simple masking.

This is just as efficient if the size is a power of two with
2-complement wrap-around logic, which is the optimized case. However
the implementation also works for non-power-of-two sizes.

Add tests for more failure cases in the CircularBuffer which is built on
top.

(tecnique described in the Art of Computer Programming by Knuth)

src/hal/circular_buffer.h

@@ -4,10 +4,10 @@
 #include <stddef.h>
 
 /// A generic circular index class which can be used to build circular buffers
-/// Can hold up to (size-1) elements
+/// Can hold up to size elements
 /// @param index_t data type of indices into array of elements
 ///   (recommended to keep uint8_fast8_t as single byte operations are atomical on the AVR)
-/// @param size number of index positions + 1.
+/// @param size number of index positions.
 ///   It is recommended to keep a power of 2 to allow for optimal code generation on the AVR (there is no HW modulo instruction)
 template <typename index_t = uint_fast8_t, size_t size = 16>
 class CircularIndex {
@@ -23,7 +23,9 @@ public:
 
     /// @returns true if full
     inline bool full() const {
-        return next(head) == tail;
+        // alternative without wrap-around logic:
+        //   return tail != head && mask(tail) == mask(head);
+        return (head - tail) % (size * 2) == size;
     }
 
     /// Advance the head index of the buffer.
@@ -41,29 +43,37 @@ public:
     /// @returns return the tail index from the buffer.
     /// Does not perform any range checks for performance reasons, should be preceeded by if(!empty()) in the user code
     inline index_t front() const {
-        return tail;
+        return mask(tail);
     }
 
     /// @returns return the head index from the buffer.
     /// Does not perform any range checks for performance reasons, should be preceeded by if(!empty()) in the user code
     inline index_t back() const {
-        return head;
+        return mask(head);
     }
 
 protected:
-    index_t tail; ///< index of element to read (pop/extract) from the buffer
-    index_t head; ///< index of an empty spot or element insertion (write)
+    index_t tail; ///< cursor of the element to read (pop/extract) from the buffer
+    index_t head; ///< cursor of the empty spot or element insertion (write)
 
-    /// @returns next index wrapped past the end of the array of elements
-    static index_t next(index_t index) { return (index + 1) % size; }
+    /// @return the index position given a cursor
+    static index_t mask(index_t cursor) { return cursor % size; }
+
+    /// @returns next cursor for internal comparisons
+    static index_t next(index_t cursor) {
+        // note: the modulo can be avoided if size is a power of two: we can do this
+        // relying on the optimizer eliding the following check at compile time.
+        static constexpr bool power2 = !(size & (size - 1));
+        return power2 ? (cursor + 1) : (cursor + 1) % (size * 2);
+    }
 };
 
 /// A generic circular buffer class
-/// Can hold up to (size-1) elements
+/// Can hold up to size elements
 /// @param T data type of stored elements
 /// @param index_t data type of indices into array of elements
 ///   (recommended to keep uint8_fast8_t as single byte operations are atomical on the AVR)
-/// @param size number of elements to store + 1.
+/// @param size number of elements to store
 ///   It is recommended to keep a power of 2 to allow for optimal code generation on the AVR (there is no HW modulo instruction)
 template <typename T = uint8_t, typename index_t = uint_fast8_t, size_t size = 16>
 class CircularBuffer {

tests/unit/hal/circular_buffer/test_circular_buffer.cpp

@@ -25,3 +25,100 @@ TEST_CASE("circular_buffer::basic", "[circular_buffer]") {
     CHECK(b == 1);
     CHECK(cb.empty());
 }
+
+TEST_CASE("circular_buffer::fill", "[circular_buffer]") {
+
+    static constexpr auto size = 4;
+    using CB = CircularBuffer<uint8_t, uint8_t, size>;
+
+    // start with an empty buffer
+    CB cb;
+    REQUIRE(cb.empty());
+
+    // ensure we can fill the buffer
+    for (auto i = 0; i != size; ++i) {
+        CHECK(!cb.full());
+        cb.push(i);
+    }
+    REQUIRE(cb.full());
+
+    // ensure another push fails
+    REQUIRE(!cb.push(0));
+
+    // retrieve all elements
+    for (auto i = 0; i != size; ++i) {
+        uint8_t v;
+        CHECK(cb.pop(v));
+        CHECK(v == i);
+    }
+    REQUIRE(cb.empty());
+}
+
+TEST_CASE("circular_buffer::wrap_around", "[circular_buffer]") {
+
+    static constexpr auto size = 4;
+    using CB = CircularBuffer<uint8_t, uint8_t, size>;
+
+    // start with an empty buffer
+    CB cb;
+    REQUIRE(cb.empty());
+
+    // test inverse logic
+    REQUIRE(!cb.full());
+
+    // add two elements to shift the internal offset
+    uint8_t v;
+    cb.push(size + 1);
+    cb.pop(v);
+    cb.push(size + 1);
+    cb.pop(v);
+    REQUIRE(cb.empty());
+
+    // loop to test the internal cursor wrap-around logic
+    // the number of loops needs to be equal or greater than the index type
+    for (auto loop = 0; loop != 256; ++loop) {
+        INFO("loop " << loop)
+
+        // ensure we can fill the buffer
+        for (auto i = 0; i != size; ++i) {
+            CHECK(!cb.full());
+            cb.push(i);
+            CHECK(!cb.empty());
+        }
+        REQUIRE(cb.full());
+        REQUIRE(!cb.empty());
+
+        // retrieve all elements
+        for (auto i = 0; i != size; ++i) {
+            uint8_t v;
+            CHECK(cb.pop(v));
+            CHECK(v == i);
+        }
+        REQUIRE(cb.empty());
+    }
+}
+
+TEST_CASE("circular_buffer::minimal_size", "[circular_buffer]") {
+
+    using CB = CircularBuffer<uint8_t, uint8_t, 1>;
+
+    // test a buffer with a minimal size (1 element)
+    CB cb;
+
+    // initial state
+    REQUIRE(cb.empty());
+    REQUIRE(!cb.full());
+
+    // push one element
+    REQUIRE(cb.push(1));
+    REQUIRE(cb.full());
+    REQUIRE(!cb.empty());
+    REQUIRE(!cb.push(2));
+
+    // retrieve the element
+    uint8_t v;
+    REQUIRE(cb.pop(v));
+    REQUIRE(v == 1);
+    REQUIRE(cb.empty());
+    REQUIRE(!cb.pop(v));
+}