/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj;

import org.junit.BeforeClass;
import org.junit.Test;

import static org.junit.Assert.assertEquals;

public class CircularBufferTest {
  private double[] m_values = {751.848, 766.366, 342.657, 234.252, 716.126,
      132.344, 445.697, 22.727, 421.125, 799.913};
  private double[] m_addFirstOut = {799.913, 421.125, 22.727, 445.697, 132.344,
      716.126, 234.252, 342.657};
  private double[] m_addLastOut = {342.657, 234.252, 716.126, 132.344, 445.697,
      22.727, 421.125, 799.913};

  @BeforeClass
  public static void before() {
    UnitTestUtility.setupMockBase();
  }

  @Test
  public void addFirstTest() {
    CircularBuffer queue = new CircularBuffer(8);

    for (double value : m_values) {
      queue.addFirst(value);
    }

    for (int i = 0; i < m_addFirstOut.length; i++) {
      assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
    }
  }

  @Test
  public void addLastTest() {
    CircularBuffer queue = new CircularBuffer(8);

    for (double value : m_values) {
      queue.addLast(value);
    }

    for (int i = 0; i < m_addLastOut.length; i++) {
      assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
    }
  }

  @Test
  public void pushPopTest() {
    CircularBuffer queue = new CircularBuffer(3);

    // Insert three elements into the buffer
    queue.addLast(1.0);
    queue.addLast(2.0);
    queue.addLast(3.0);

    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);
    assertEquals(3.0, queue.get(2), 0.00005);

    /*
     * The buffer is full now, so pushing subsequent elements will overwrite the
     * front-most elements.
     */

    queue.addLast(4.0); // Overwrite 1 with 4

    // The buffer now contains 2, 3, and 4
    assertEquals(2.0, queue.get(0), 0.00005);
    assertEquals(3.0, queue.get(1), 0.00005);
    assertEquals(4.0, queue.get(2), 0.00005);

    queue.addLast(5.0); // Overwrite 2 with 5

    // The buffer now contains 3, 4, and 5
    assertEquals(3.0, queue.get(0), 0.00005);
    assertEquals(4.0, queue.get(1), 0.00005);
    assertEquals(5.0, queue.get(2), 0.00005);

    assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed

    // The buffer now contains 3 and 4
    assertEquals(3.0, queue.get(0), 0.00005);
    assertEquals(4.0, queue.get(1), 0.00005);

    assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed

    // Leaving only one element with value == 4
    assertEquals(4.0, queue.get(0), 0.00005);
  }

  @Test
  public void resetTest() {
    CircularBuffer queue = new CircularBuffer(5);

    for (int i = 0; i < 6; i++) {
      queue.addLast(i);
    }

    queue.clear();

    for (int i = 0; i < 5; i++) {
      assertEquals(0.0, queue.get(i), 0.00005);
    }
  }

  @Test
  public void resizeTest() {
    CircularBuffer queue = new CircularBuffer(5);

    /* Buffer contains {1, 2, 3, _, _}
     *                  ^ front
     */
    queue.addLast(1.0);
    queue.addLast(2.0);
    queue.addLast(3.0);

    queue.resize(2);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.resize(5);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.clear();

    /* Buffer contains {_, 1, 2, 3, _}
     *                     ^ front
     */
    queue.addLast(0.0);
    queue.addLast(1.0);
    queue.addLast(2.0);
    queue.addLast(3.0);
    queue.removeFirst();

    queue.resize(2);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.resize(5);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.clear();

    /* Buffer contains {_, _, 1, 2, 3}
     *                        ^ front
     */
    queue.addLast(0.0);
    queue.addLast(0.0);
    queue.addLast(1.0);
    queue.addLast(2.0);
    queue.addLast(3.0);
    queue.removeFirst();
    queue.removeFirst();

    queue.resize(2);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.resize(5);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.clear();

    /* Buffer contains {3, _, _, 1, 2}
     *                           ^ front
     */
    queue.addLast(3.0);
    queue.addFirst(2.0);
    queue.addFirst(1.0);

    queue.resize(2);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.resize(5);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.clear();

    /* Buffer contains {2, 3, _, _, 1}
     *                              ^ front
     */
    queue.addLast(2.0);
    queue.addLast(3.0);
    queue.addFirst(1.0);

    queue.resize(2);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    queue.resize(5);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);

    // Test addLast() after resize
    queue.addLast(3.0);
    assertEquals(1.0, queue.get(0), 0.00005);
    assertEquals(2.0, queue.get(1), 0.00005);
    assertEquals(3.0, queue.get(2), 0.00005);

    // Test addFirst() after resize
    queue.addFirst(4.0);
    assertEquals(4.0, queue.get(0), 0.00005);
    assertEquals(1.0, queue.get(1), 0.00005);
    assertEquals(2.0, queue.get(2), 0.00005);
    assertEquals(3.0, queue.get(3), 0.00005);
  }
}