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
007import java.util.Arrays;
008
009/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
010public class CircularBuffer {
011  private double[] m_data;
012
013  // Index of element at front of buffer
014  private int m_front;
015
016  // Number of elements used in buffer
017  private int m_length;
018
019  /**
020   * Create a CircularBuffer with the provided size.
021   *
022   * @param size The size of the circular buffer.
023   */
024  public CircularBuffer(int size) {
025    m_data = new double[size];
026    Arrays.fill(m_data, 0.0);
027  }
028
029  /**
030   * Returns number of elements in buffer.
031   *
032   * @return number of elements in buffer
033   */
034  public int 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  public 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  public 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  public 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    Arrays.fill(m_data, 0.0);
154    m_front = 0;
155    m_length = 0;
156  }
157
158  /**
159   * Get the element at the provided index relative to the start of the buffer.
160   *
161   * @param index Index into the buffer.
162   * @return Element at index starting from front of buffer.
163   */
164  public double get(int index) {
165    return m_data[(m_front + index) % m_data.length];
166  }
167
168  /**
169   * Increment an index modulo the length of the m_data buffer.
170   *
171   * @param index Index into the buffer.
172   */
173  private int moduloInc(int index) {
174    return (index + 1) % m_data.length;
175  }
176
177  /**
178   * Decrement an index modulo the length of the m_data buffer.
179   *
180   * @param index Index into the buffer.
181   */
182  private int moduloDec(int index) {
183    if (index == 0) {
184      return m_data.length - 1;
185    } else {
186      return index - 1;
187    }
188  }
189}