001/*----------------------------------------------------------------------------*/
002/* Copyright (c) FIRST 2015-2017. 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   * @param size The size of the circular buffer.
024   */
025  public CircularBuffer(int size) {
026    m_data = new double[size];
027    for (int i = 0; i < m_data.length; i++) {
028      m_data[i] = 0.0;
029    }
030  }
031
032  /**
033   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
034   * full.
035   */
036  public void pushFront(double value) {
037    if (m_data.length == 0) {
038      return;
039    }
040
041    m_front = moduloDec(m_front);
042
043    m_data[m_front] = value;
044
045    if (m_length < m_data.length) {
046      m_length++;
047    }
048  }
049
050  /**
051   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
052   * full.
053   */
054  public void pushBack(double value) {
055    if (m_data.length == 0) {
056      return;
057    }
058
059    m_data[(m_front + m_length) % m_data.length] = value;
060
061    if (m_length < m_data.length) {
062      m_length++;
063    } else {
064      // Increment front if buffer is full to maintain size
065      m_front = moduloInc(m_front);
066    }
067  }
068
069  /**
070   * Pop value at front of buffer.
071   *
072   * @return value at front of buffer
073   */
074  public double popFront() {
075    // If there are no elements in the buffer, do nothing
076    if (m_length == 0) {
077      return 0.0;
078    }
079
080    double temp = m_data[m_front];
081    m_front = moduloInc(m_front);
082    m_length--;
083    return temp;
084  }
085
086
087  /**
088   * Pop value at back of buffer.
089   */
090  public double popBack() {
091    // If there are no elements in the buffer, do nothing
092    if (m_length == 0) {
093      return 0.0;
094    }
095
096    m_length--;
097    return m_data[(m_front + m_length) % m_data.length];
098  }
099
100  /**
101   * Resizes internal buffer to given size.
102   *
103   * <p>A new buffer is allocated because arrays are not resizable.
104   */
105  void resize(int size) {
106    double[] newBuffer = new double[size];
107    m_length = Math.min(m_length, size);
108    for (int i = 0; i < m_length; i++) {
109      newBuffer[i] = m_data[(m_front + i) % m_data.length];
110    }
111    m_data = newBuffer;
112    m_front = 0;
113  }
114
115  /**
116   * Sets internal buffer contents to zero.
117   */
118  public void reset() {
119    for (int i = 0; i < m_data.length; i++) {
120      m_data[i] = 0.0;
121    }
122    m_front = 0;
123    m_length = 0;
124  }
125
126  /**
127   * @return Element at index starting from front of buffer.
128   */
129  public double get(int index) {
130    return m_data[(m_front + index) % m_data.length];
131  }
132
133  /**
134   * Increment an index modulo the length of the m_data buffer.
135   */
136  private int moduloInc(int index) {
137    return (index + 1) % m_data.length;
138  }
139
140  /**
141   * Decrement an index modulo the length of the m_data buffer.
142   */
143  private int moduloDec(int index) {
144    if (index == 0) {
145      return m_data.length - 1;
146    } else {
147      return index - 1;
148    }
149  }
150}