[wpimath] Add generic circular buffer class to Java (#5969)

The original is now called DoubleCircularBuffer.

wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java

@@ -6,7 +6,7 @@ package edu.wpi.first.math.filter;
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.util.CircularBuffer;
+import edu.wpi.first.util.DoubleCircularBuffer;
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
 
@@ -48,8 +48,8 @@ import org.ejml.simple.SimpleMatrix;
  * to make sure calculate() gets called at the desired, constant frequency!
  */
 public class LinearFilter {
-  private final CircularBuffer m_inputs;
-  private final CircularBuffer m_outputs;
+  private final DoubleCircularBuffer m_inputs;
+  private final DoubleCircularBuffer m_outputs;
   private final double[] m_inputGains;
   private final double[] m_outputGains;
 
@@ -62,8 +62,8 @@ public class LinearFilter {
    * @param fbGains The "feedback" or IIR gains.
    */
   public LinearFilter(double[] ffGains, double[] fbGains) {
-    m_inputs = new CircularBuffer(ffGains.length);
-    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputs = new DoubleCircularBuffer(ffGains.length);
+    m_outputs = new DoubleCircularBuffer(fbGains.length);
     m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
     m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
 

wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java

@@ -4,7 +4,7 @@
 
 package edu.wpi.first.math.filter;
 
-import edu.wpi.first.util.CircularBuffer;
+import edu.wpi.first.util.DoubleCircularBuffer;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
@@ -15,7 +15,7 @@ import java.util.List;
  * processing, LIDAR, or ultrasonic sensors).
  */
 public class MedianFilter {
-  private final CircularBuffer m_valueBuffer;
+  private final DoubleCircularBuffer m_valueBuffer;
   private final List<Double> m_orderedValues;
   private final int m_size;
 
@@ -26,7 +26,7 @@ public class MedianFilter {
    */
   public MedianFilter(int size) {
     // Circular buffer of values currently in the window, ordered by time
-    m_valueBuffer = new CircularBuffer(size);
+    m_valueBuffer = new DoubleCircularBuffer(size);
     // List of values currently in the window, ordered by value
     m_orderedValues = new ArrayList<>(size);
     // Size of rolling window

wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java

@@ -4,11 +4,9 @@
 
 package edu.wpi.first.util;
 
-import java.util.Arrays;
-
 /** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
-public class CircularBuffer {
-  private double[] m_data;
+public class CircularBuffer<T> {
+  private T[] m_data;
 
   // Index of element at front of buffer
   private int m_front;
@@ -21,9 +19,9 @@ public class CircularBuffer {
    *
    * @param size The size of the circular buffer.
    */
+  @SuppressWarnings("unchecked")
   public CircularBuffer(int size) {
-    m_data = new double[size];
-    Arrays.fill(m_data, 0.0);
+    m_data = (T[]) new Object[size];
   }
 
   /**
@@ -40,7 +38,7 @@ public class CircularBuffer {
    *
    * @return value at front of buffer
    */
-  public double getFirst() {
+  public T getFirst() {
     return m_data[m_front];
   }
 
@@ -48,11 +46,14 @@ public class CircularBuffer {
    * Get value at back of buffer.
    *
    * @return value at back of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double getLast() {
+  @SuppressWarnings("unchecked")
+  public T getLast() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("getLast() called on an empty container");
     }
 
     return m_data[(m_front + m_length - 1) % m_data.length];
@@ -64,7 +65,7 @@ public class CircularBuffer {
    *
    * @param value The value to push.
    */
-  public void addFirst(double value) {
+  public void addFirst(T value) {
     if (m_data.length == 0) {
       return;
     }
@@ -84,7 +85,7 @@ public class CircularBuffer {
    *
    * @param value The value to push.
    */
-  public void addLast(double value) {
+  public void addLast(T value) {
     if (m_data.length == 0) {
       return;
     }
@@ -103,14 +104,17 @@ public class CircularBuffer {
    * Pop value at front of buffer.
    *
    * @return value at front of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double removeFirst() {
+  @SuppressWarnings("unchecked")
+  public T removeFirst() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("removeFirst() called on an empty container");
     }
 
-    double temp = m_data[m_front];
+    T temp = m_data[m_front];
     m_front = moduloInc(m_front);
     m_length--;
     return temp;
@@ -120,11 +124,14 @@ public class CircularBuffer {
    * Pop value at back of buffer.
    *
    * @return value at back of buffer
+   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
+   *     size())
    */
-  public double removeLast() {
+  @SuppressWarnings("unchecked")
+  public T removeLast() {
     // If there are no elements in the buffer, do nothing
     if (m_length == 0) {
-      return 0.0;
+      throw new IndexOutOfBoundsException("removeLast() called on an empty container");
     }
 
     m_length--;
@@ -138,8 +145,9 @@ public class CircularBuffer {
    *
    * @param size New buffer size.
    */
+  @SuppressWarnings("unchecked")
   public void resize(int size) {
-    double[] newBuffer = new double[size];
+    var newBuffer = (T[]) new Object[size];
     m_length = Math.min(m_length, size);
     for (int i = 0; i < m_length; i++) {
       newBuffer[i] = m_data[(m_front + i) % m_data.length];
@@ -150,7 +158,6 @@ public class CircularBuffer {
 
   /** Sets internal buffer contents to zero. */
   public void clear() {
-    Arrays.fill(m_data, 0.0);
     m_front = 0;
     m_length = 0;
   }
@@ -161,23 +168,25 @@ public class CircularBuffer {
    * @param index Index into the buffer.
    * @return Element at index starting from front of buffer.
    */
-  public double get(int index) {
+  public T get(int index) {
     return m_data[(m_front + index) % m_data.length];
   }
 
   /**
-   * Increment an index modulo the length of the m_data buffer.
+   * Increment an index modulo the length of the buffer.
    *
    * @param index Index into the buffer.
+   * @return The incremented index.
    */
   private int moduloInc(int index) {
     return (index + 1) % m_data.length;
   }
 
   /**
-   * Decrement an index modulo the length of the m_data buffer.
+   * Decrement an index modulo the length of the buffer.
    *
    * @param index Index into the buffer.
+   * @return The decremented index.
    */
   private int moduloDec(int index) {
     if (index == 0) {

wpiutil/src/main/java/edu/wpi/first/util/DoubleCircularBuffer.java

@@ -0,0 +1,191 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.util.Arrays;
+
+/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
+public class DoubleCircularBuffer {
+  private double[] m_data;
+
+  // Index of element at front of buffer
+  private int m_front;
+
+  // Number of elements used in buffer
+  private int m_length;
+
+  /**
+   * Create a CircularBuffer with the provided size.
+   *
+   * @param size The size of the circular buffer.
+   */
+  public DoubleCircularBuffer(int size) {
+    m_data = new double[size];
+    Arrays.fill(m_data, 0.0);
+  }
+
+  /**
+   * Returns number of elements in buffer.
+   *
+   * @return number of elements in buffer
+   */
+  public int size() {
+    return m_length;
+  }
+
+  /**
+   * Get value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double getFirst() {
+    return m_data[m_front];
+  }
+
+  /**
+   * Get value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  public double getLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    return m_data[(m_front + m_length - 1) % m_data.length];
+  }
+
+  /**
+   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addFirst(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_front = moduloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addLast(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.length] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = moduloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double removeFirst() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    double temp = m_data[m_front];
+    m_front = moduloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+  /**
+   * Pop value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  public double removeLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    m_length--;
+    return m_data[(m_front + m_length) % m_data.length];
+  }
+
+  /**
+   * Resizes internal buffer to given size.
+   *
+   * <p>A new buffer is allocated because arrays are not resizable.
+   *
+   * @param size New buffer size.
+   */
+  public void resize(int size) {
+    double[] newBuffer = new double[size];
+    m_length = Math.min(m_length, size);
+    for (int i = 0; i < m_length; i++) {
+      newBuffer[i] = m_data[(m_front + i) % m_data.length];
+    }
+    m_data = newBuffer;
+    m_front = 0;
+  }
+
+  /** Sets internal buffer contents to zero. */
+  public void clear() {
+    Arrays.fill(m_data, 0.0);
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * Get the element at the provided index relative to the start of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return Element at index starting from front of buffer.
+   */
+  public double get(int index) {
+    return m_data[(m_front + index) % m_data.length];
+  }
+
+  /**
+   * Increment an index modulo the length of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return The incremented index.
+   */
+  private int moduloInc(int index) {
+    return (index + 1) % m_data.length;
+  }
+
+  /**
+   * Decrement an index modulo the length of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return The decremented index.
+   */
+  private int moduloDec(int index) {
+    if (index == 0) {
+      return m_data.length - 1;
+    } else {
+      return index - 1;
+    }
+  }
+}

wpiutil/src/main/native/include/wpi/circular_buffer.h

@@ -268,16 +268,18 @@ class circular_buffer {
   size_t m_length = 0;
 
   /**
-   * Increment an index modulo the length of the buffer.
+   * Increment an index modulo the size of the buffer.
    *
-   * @return The result of the modulo operation.
+   * @param index Index into the buffer.
+   * @return The incremented index.
    */
   size_t ModuloInc(size_t index) { return (index + 1) % m_data.size(); }
 
   /**
-   * Decrement an index modulo the length of the buffer.
+   * Decrement an index modulo the size of the buffer.
    *
-   * @return The result of the modulo operation.
+   * @param index Index into the buffer.
+   * @return The decremented index.
    */
   size_t ModuloDec(size_t index) {
     if (index == 0) {

wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java

@@ -21,7 +21,7 @@ class CircularBufferTest {
 
   @Test
   void addFirstTest() {
-    CircularBuffer queue = new CircularBuffer(8);
+    var queue = new CircularBuffer<Double>(8);
 
     for (double value : m_values) {
       queue.addFirst(value);
@@ -34,7 +34,7 @@ class CircularBufferTest {
 
   @Test
   void addLastTest() {
-    CircularBuffer queue = new CircularBuffer(8);
+    var queue = new CircularBuffer<Double>(8);
 
     for (double value : m_values) {
       queue.addLast(value);
@@ -47,7 +47,7 @@ class CircularBufferTest {
 
   @Test
   void pushPopTest() {
-    CircularBuffer queue = new CircularBuffer(3);
+    var queue = new CircularBuffer<Double>(3);
 
     // Insert three elements into the buffer
     queue.addLast(1.0);
@@ -91,22 +91,20 @@ class CircularBufferTest {
 
   @Test
   void resetTest() {
-    CircularBuffer queue = new CircularBuffer(5);
+    var queue = new CircularBuffer<Double>(5);
 
     for (int i = 0; i < 6; i++) {
-      queue.addLast(i);
+      queue.addLast((double) i);
     }
 
     queue.clear();
 
-    for (int i = 0; i < 5; i++) {
-      assertEquals(0.0, queue.get(i), 0.00005);
-    }
+    assertEquals(0, queue.size());
   }
 
   @Test
   void resizeTest() {
-    CircularBuffer queue = new CircularBuffer(5);
+    var queue = new CircularBuffer<Double>(5);
 
     /* Buffer contains {1, 2, 3, _, _}
      *                  ^ front

wpiutil/src/test/java/edu/wpi/first/util/DoubleCircularBufferTest.java

@@ -0,0 +1,213 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DoubleCircularBufferTest {
+  private final double[] m_values = {
+    751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+  private final double[] m_addFirstOut = {
+    799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657
+  };
+  private final double[] m_addLastOut = {
+    342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+
+  @Test
+  void addFirstTest() {
+    var queue = new DoubleCircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addFirst(value);
+    }
+
+    for (int i = 0; i < m_addFirstOut.length; i++) {
+      assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void addLastTest() {
+    var queue = new DoubleCircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addLast(value);
+    }
+
+    for (int i = 0; i < m_addLastOut.length; i++) {
+      assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void pushPopTest() {
+    var queue = new DoubleCircularBuffer(3);
+
+    // Insert three elements into the buffer
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    /*
+     * The buffer is full now, so pushing subsequent elements will overwrite the
+     * front-most elements.
+     */
+
+    queue.addLast(4.0); // Overwrite 1 with 4
+
+    // The buffer now contains 2, 3, and 4
+    assertEquals(2.0, queue.get(0), 0.00005);
+    assertEquals(3.0, queue.get(1), 0.00005);
+    assertEquals(4.0, queue.get(2), 0.00005);
+
+    queue.addLast(5.0); // Overwrite 2 with 5
+
+    // The buffer now contains 3, 4, and 5
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+    assertEquals(5.0, queue.get(2), 0.00005);
+
+    assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed
+
+    // The buffer now contains 3 and 4
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+
+    assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed
+
+    // Leaving only one element with value == 4
+    assertEquals(4.0, queue.get(0), 0.00005);
+  }
+
+  @Test
+  void resetTest() {
+    var queue = new DoubleCircularBuffer(5);
+
+    for (int i = 0; i < 6; i++) {
+      queue.addLast(i);
+    }
+
+    queue.clear();
+
+    for (int i = 0; i < 5; i++) {
+      assertEquals(0.0, queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void resizeTest() {
+    var queue = new DoubleCircularBuffer(5);
+
+    /* Buffer contains {1, 2, 3, _, _}
+     *                  ^ front
+     */
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, 1, 2, 3, _}
+     *                     ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, _, 1, 2, 3}
+     *                        ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {3, _, _, 1, 2}
+     *                           ^ front
+     */
+    queue.addLast(3.0);
+    queue.addFirst(2.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {2, 3, _, _, 1}
+     *                              ^ front
+     */
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    // Test addLast() after resize
+    queue.addLast(3.0);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    // Test addFirst() after resize
+    queue.addFirst(4.0);
+    assertEquals(4.0, queue.get(0), 0.00005);
+    assertEquals(1.0, queue.get(1), 0.00005);
+    assertEquals(2.0, queue.get(2), 0.00005);
+    assertEquals(3.0, queue.get(3), 0.00005);
+  }
+}