001/*----------------------------------------------------------------------------*/
002/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
003/* Open Source Software - may be modified and shared by FRC teams. The code   */
004/* must be accompanied by the FIRST BSD license file in the root directory of */
005/* the project.                                                               */
006/*----------------------------------------------------------------------------*/
007
008package edu.wpi.first.wpilibj;
009
010/**
011 * This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
012 */
013public class CircularBuffer {
014  private double[] m_data;
015
016  // Index of element at front of buffer
017  private int m_front = 0;
018
019  // Number of elements used in buffer
020  private int m_length = 0;
021
022  /**
023   * Create a CircularBuffer with the provided size.
024   *
025   * @param size The size of the circular buffer.
026   */
027  public CircularBuffer(int size) {
028    m_data = new double[size];
029    for (int i = 0; i < m_data.length; i++) {
030      m_data[i] = 0.0;
031    }
032  }
033
034  /**
035   * Returns number of elements in buffer.
036   *
037   * @return number of elements in buffer
038   */
039  double size() {
040    return m_length;
041  }
042
043  /**
044   * Get value at front of buffer.
045   *
046   * @return value at front of buffer
047   */
048  double getFirst() {
049    return m_data[m_front];
050  }
051
052  /**
053   * Get value at back of buffer.
054   *
055   * @return value at back of buffer
056   */
057  double getLast() {
058    // If there are no elements in the buffer, do nothing
059    if (m_length == 0) {
060      return 0.0;
061    }
062
063    return m_data[(m_front + m_length - 1) % m_data.length];
064  }
065
066  /**
067   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
068   * full.
069   */
070  public void addFirst(double value) {
071    if (m_data.length == 0) {
072      return;
073    }
074
075    m_front = moduloDec(m_front);
076
077    m_data[m_front] = value;
078
079    if (m_length < m_data.length) {
080      m_length++;
081    }
082  }
083
084  /**
085   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
086   * full.
087   */
088  public void addLast(double 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   */
108  public double removeFirst() {
109    // If there are no elements in the buffer, do nothing
110    if (m_length == 0) {
111      return 0.0;
112    }
113
114    double temp = m_data[m_front];
115    m_front = moduloInc(m_front);
116    m_length--;
117    return temp;
118  }
119
120
121  /**
122   * Pop 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  void resize(int size) {
140    double[] newBuffer = new double[size];
141    m_length = Math.min(m_length, size);
142    for (int i = 0; i < m_length; i++) {
143      newBuffer[i] = m_data[(m_front + i) % m_data.length];
144    }
145    m_data = newBuffer;
146    m_front = 0;
147  }
148
149  /**
150   * Sets internal buffer contents to zero.
151   */
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   * @return Element at index starting from front of buffer.
164   */
165  public double get(int index) {
166    return m_data[(m_front + index) % m_data.length];
167  }
168
169  /**
170   * Increment an index modulo the length of the m_data buffer.
171   */
172  private int moduloInc(int index) {
173    return (index + 1) % m_data.length;
174  }
175
176  /**
177   * Decrement an index modulo the length of the m_data buffer.
178   */
179  private int moduloDec(int index) {
180    if (index == 0) {
181      return m_data.length - 1;
182    } else {
183      return index - 1;
184    }
185  }
186}