Implemented CircularBuffer resizing (#224)

wpilibc/shared/include/CircularBuffer.inc

@@ -81,6 +81,59 @@ T CircularBuffer<T>::PopBack() {
   return m_data[(m_front + m_length) % m_data.size()];
 }
 
+/**
+ * Resizes internal buffer to given size.
+ */
+template <class T>
+void CircularBuffer<T>::Resize(size_t size) {
+  if (size > m_data.size()) {
+    // Find end of buffer
+    size_t insertLocation = (m_front + m_length) % m_data.size();
+
+    // If insertion location precedes front of buffer, push front index back
+    if (insertLocation <= m_front) {
+      m_front += size - m_data.size();
+    }
+
+    // Add elements to end of buffer
+    m_data.insert(m_data.begin() + insertLocation, size - m_data.size(), 0);
+  } else if (size < m_data.size()) {
+    /* 1) Shift element block start at "front" left as many blocks as were
+     *    removed up to but not exceeding buffer[0]
+     * 2) Shrink buffer, which will remove even more elements automatically if
+     *    necessary
+     */
+    size_t elemsToRemove = m_data.size() - size;
+    auto frontIter = m_data.begin() + m_front;
+    if (m_front < elemsToRemove) {
+      /* Remove elements from end of buffer before shifting start of element
+       * block. Doing so saves a few copies.
+       */
+      m_data.erase(frontIter + size, m_data.end());
+
+      // Shift start of element block to left
+      m_data.erase(m_data.begin(), frontIter);
+
+      // Update metadata
+      m_front = 0;
+    } else {
+      // Shift start of element block to left
+      m_data.erase(frontIter - elemsToRemove, frontIter);
+
+      // Update metadata
+      m_front -= elemsToRemove;
+    }
+
+    /* Length only changes during a shrink if all unused spaces have been
+     * removed. Length decreases as used spaces are removed to meet the
+     * required size.
+     */
+    if (m_length > size) {
+      m_length = size;
+    }
+  }
+}
+
 /**
  * Sets internal buffer contents to zero.
  */