[wpimath] Add generic circular buffer class to Java

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

@@ -6,7 +6,7 @@
 
 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 @@
  * 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 @@
  * 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,19 +38,22 @@ public int size() {
    *
    * @return value at front of buffer
    */
-  public double getFirst() {
+  public T getFirst() {
     return m_data[m_front];
   }
 
   /**
    * 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 double getLast() {
    *
    * @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 void addFirst(double value) {
    *
    * @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 void addLast(double value) {
    * 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 double removeFirst() {
    * 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 double removeLast() {
    *
    * @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 void resize(int size) {
 
   /** 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 void clear() {
    * @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) {