001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.util;
006
007/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
008public class CircularBuffer<T> {
009  private T[] m_data;
010
011  // Index of element at front of buffer
012  private int m_front;
013
014  // Number of elements used in buffer
015  private int m_length;
016
017  /**
018   * Create a CircularBuffer with the provided size.
019   *
020   * @param size The size of the circular buffer.
021   */
022  @SuppressWarnings("unchecked")
023  public CircularBuffer(int size) {
024    m_data = (T[]) new Object[size];
025  }
026
027  /**
028   * Returns number of elements in buffer.
029   *
030   * @return number of elements in buffer
031   */
032  public int size() {
033    return m_length;
034  }
035
036  /**
037   * Get value at front of buffer.
038   *
039   * @return value at front of buffer
040   */
041  public T getFirst() {
042    return m_data[m_front];
043  }
044
045  /**
046   * Get value at back of buffer.
047   *
048   * @return value at back of buffer
049   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
050   *     size())
051   */
052  @SuppressWarnings("unchecked")
053  public T getLast() {
054    // If there are no elements in the buffer, do nothing
055    if (m_length == 0) {
056      throw new IndexOutOfBoundsException("getLast() called on an empty container");
057    }
058
059    return m_data[(m_front + m_length - 1) % m_data.length];
060  }
061
062  /**
063   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
064   * full.
065   *
066   * @param value The value to push.
067   */
068  public void addFirst(T value) {
069    if (m_data.length == 0) {
070      return;
071    }
072
073    m_front = moduloDec(m_front);
074
075    m_data[m_front] = value;
076
077    if (m_length < m_data.length) {
078      m_length++;
079    }
080  }
081
082  /**
083   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
084   * full.
085   *
086   * @param value The value to push.
087   */
088  public void addLast(T value) {
089    if (m_data.length == 0) {
090      return;
091    }
092
093    m_data[(m_front + m_length) % m_data.length] = value;
094
095    if (m_length < m_data.length) {
096      m_length++;
097    } else {
098      // Increment front if buffer is full to maintain size
099      m_front = moduloInc(m_front);
100    }
101  }
102
103  /**
104   * Pop value at front of buffer.
105   *
106   * @return value at front of buffer
107   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
108   *     size())
109   */
110  @SuppressWarnings("unchecked")
111  public T removeFirst() {
112    // If there are no elements in the buffer, do nothing
113    if (m_length == 0) {
114      throw new IndexOutOfBoundsException("removeFirst() called on an empty container");
115    }
116
117    T temp = m_data[m_front];
118    m_front = moduloInc(m_front);
119    m_length--;
120    return temp;
121  }
122
123  /**
124   * Pop value at back of buffer.
125   *
126   * @return value at back of buffer
127   * @throws IndexOutOfBoundsException if the index is out of range (index &lt; 0 || index &gt;=
128   *     size())
129   */
130  @SuppressWarnings("unchecked")
131  public T removeLast() {
132    // If there are no elements in the buffer, do nothing
133    if (m_length == 0) {
134      throw new IndexOutOfBoundsException("removeLast() called on an empty container");
135    }
136
137    m_length--;
138    return m_data[(m_front + m_length) % m_data.length];
139  }
140
141  /**
142   * Resizes internal buffer to given size.
143   *
144   * <p>A new buffer is allocated because arrays are not resizable.
145   *
146   * @param size New buffer size.
147   */
148  @SuppressWarnings("unchecked")
149  public void resize(int size) {
150    var newBuffer = (T[]) new Object[size];
151    m_length = Math.min(m_length, size);
152    for (int i = 0; i < m_length; i++) {
153      newBuffer[i] = m_data[(m_front + i) % m_data.length];
154    }
155    m_data = newBuffer;
156    m_front = 0;
157  }
158
159  /** Sets internal buffer contents to zero. */
160  public void clear() {
161    m_front = 0;
162    m_length = 0;
163  }
164
165  /**
166   * Get the element at the provided index relative to the start of the buffer.
167   *
168   * @param index Index into the buffer.
169   * @return Element at index starting from front of buffer.
170   */
171  public T get(int index) {
172    return m_data[(m_front + index) % m_data.length];
173  }
174
175  /**
176   * Increment an index modulo the length of the buffer.
177   *
178   * @param index Index into the buffer.
179   * @return The incremented index.
180   */
181  private int moduloInc(int index) {
182    return (index + 1) % m_data.length;
183  }
184
185  /**
186   * Decrement an index modulo the length of the buffer.
187   *
188   * @param index Index into the buffer.
189   * @return The decremented index.
190   */
191  private int moduloDec(int index) {
192    if (index == 0) {
193      return m_data.length - 1;
194    } else {
195      return index - 1;
196    }
197  }
198}