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 {
009  private double[] 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  public CircularBuffer(int size) {
023    m_data = new double[size];
024    for (int i = 0; i < m_data.length; i++) {
025      m_data[i] = 0.0;
026    }
027  }
028
029  /**
030   * Returns number of elements in buffer.
031   *
032   * @return number of elements in buffer
033   */
034  double size() {
035    return m_length;
036  }
037
038  /**
039   * Get value at front of buffer.
040   *
041   * @return value at front of buffer
042   */
043  double getFirst() {
044    return m_data[m_front];
045  }
046
047  /**
048   * Get value at back of buffer.
049   *
050   * @return value at back of buffer
051   */
052  double getLast() {
053    // If there are no elements in the buffer, do nothing
054    if (m_length == 0) {
055      return 0.0;
056    }
057
058    return m_data[(m_front + m_length - 1) % m_data.length];
059  }
060
061  /**
062   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
063   * full.
064   *
065   * @param value The value to push.
066   */
067  public void addFirst(double value) {
068    if (m_data.length == 0) {
069      return;
070    }
071
072    m_front = moduloDec(m_front);
073
074    m_data[m_front] = value;
075
076    if (m_length < m_data.length) {
077      m_length++;
078    }
079  }
080
081  /**
082   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
083   * full.
084   *
085   * @param value The value to push.
086   */
087  public void addLast(double value) {
088    if (m_data.length == 0) {
089      return;
090    }
091
092    m_data[(m_front + m_length) % m_data.length] = value;
093
094    if (m_length < m_data.length) {
095      m_length++;
096    } else {
097      // Increment front if buffer is full to maintain size
098      m_front = moduloInc(m_front);
099    }
100  }
101
102  /**
103   * Pop value at front of buffer.
104   *
105   * @return value at front of buffer
106   */
107  public double removeFirst() {
108    // If there are no elements in the buffer, do nothing
109    if (m_length == 0) {
110      return 0.0;
111    }
112
113    double temp = m_data[m_front];
114    m_front = moduloInc(m_front);
115    m_length--;
116    return temp;
117  }
118
119  /**
120   * Pop value at back of buffer.
121   *
122   * @return value at back of buffer
123   */
124  public double removeLast() {
125    // If there are no elements in the buffer, do nothing
126    if (m_length == 0) {
127      return 0.0;
128    }
129
130    m_length--;
131    return m_data[(m_front + m_length) % m_data.length];
132  }
133
134  /**
135   * Resizes internal buffer to given size.
136   *
137   * <p>A new buffer is allocated because arrays are not resizable.
138   *
139   * @param size New buffer size.
140   */
141  void resize(int size) {
142    double[] newBuffer = new double[size];
143    m_length = Math.min(m_length, size);
144    for (int i = 0; i < m_length; i++) {
145      newBuffer[i] = m_data[(m_front + i) % m_data.length];
146    }
147    m_data = newBuffer;
148    m_front = 0;
149  }
150
151  /** Sets internal buffer contents to zero. */
152  public void clear() {
153    for (int i = 0; i < m_data.length; i++) {
154      m_data[i] = 0.0;
155    }
156    m_front = 0;
157    m_length = 0;
158  }
159
160  /**
161   * Get the element at the provided index relative to the start of the buffer.
162   *
163   * @param index Index into the buffer.
164   * @return Element at index starting from front of buffer.
165   */
166  public double get(int index) {
167    return m_data[(m_front + index) % m_data.length];
168  }
169
170  /**
171   * Increment an index modulo the length of the m_data buffer.
172   *
173   * @param index Index into the buffer.
174   */
175  private int moduloInc(int index) {
176    return (index + 1) % m_data.length;
177  }
178
179  /**
180   * Decrement an index modulo the length of the m_data buffer.
181   *
182   * @param index Index into the buffer.
183   */
184  private int moduloDec(int index) {
185    if (index == 0) {
186      return m_data.length - 1;
187    } else {
188      return index - 1;
189    }
190  }
191}