// Motor A
const int AIN1 = 6;
const int AIN2 = 9;

// Motor B
const int BIN1 = 5;
const int BIN2 = 10; 

const int NSLEEP = 4;

// Wheel Encoder 
const int EncoderA = 14; // MISO
const int EncoderB = 16; // MOSI 

// const int window_size = 40;  // (360 degrees/rev)/ (9 degrees / pin change) = 40 pin change / rev)
const int window_size = 72;  // (360 degrees/rev)/ (5 degrees / pin change) = 72 pin change / rev)
// const int window_size = 120;  // (360 degrees/rev)/ (3 degrees / pin change) = 120 pin change / rev)


/*
 * ArxRobot Sample Arduino Code
 * Basic Example
 */
  /* one-half revolution for 9 degree encoder is 20 */
  uint32_t stopwatch_pressed;        // reset and start the stopwatch 
  uint8_t current_value;             // current wheel encoder output value
  uint32_t time_min;
  uint32_t time_max;

  int16_t pulse_count;               // count down clock 
  uint8_t state;                     // FSM
  
void setup()
{
    Serial.begin(9600);

    // Wheel Encoder  
    pinMode(EncoderA, INPUT);  // pull-up included on wheel encoder
    pinMode(EncoderB, INPUT);
    
    // Motors
    pinMode(AIN1, OUTPUT);
    pinMode(AIN2, OUTPUT);
    pinMode(BIN1, OUTPUT);
    pinMode(BIN2, OUTPUT);
    pinMode(NSLEEP, OUTPUT);

    // paperBot forward (cw)
    analogWrite(AIN1, 0);     // left motor (simply a convention) monitored by encoder B
    analogWrite(AIN2, 255); // standard is 230
    analogWrite(BIN1, 0);     // right motor monitored by encoder A
    analogWrite(BIN2, 255);   // standard is 230

    digitalWrite(NSLEEP, HIGH);

    state = 0;               // FSM reset state
    pulse_count = 72;        // number of pulses until statistics are recorded (5 degree wheel)
    
    time_min = 100;          // initial test is true
    time_max = 0;
}

void loop()
{ 
  uint8_t wheelA = digitalRead(EncoderA); // right motor ==> encoder A
  uint8_t wheelB = digitalRead(EncoderB); // left  motor ==> encoder B 
  uint8_t test_this_wheel = wheelA;       // *********************
  
  if (test_this_wheel != current_value){
    current_value = test_this_wheel;       // update to current value

    switch (state)
    {
    case 0: // let system reach steady-state
        pulse_count--;                    // decrement pulse counter
        if (pulse_count < 0){
          // initialize statistic package
          stopwatch_pressed = millis();   // start the stop watch
          state = 1;
        }
        break;
    case 1: // statistics package
        uint32_t elapsed_time = millis() - stopwatch_pressed; // uint32_t time difference cast to a 16-bit value
        stopwatch_pressed = millis();          // reset and start the stopwatch

        uint16_t time_avg = runningAverage((uint16_t) elapsed_time);

        if (time_max < elapsed_time) time_max = elapsed_time;
        if (elapsed_time < time_min) time_min = elapsed_time;

        static uint32_t neg_edge;
        static uint32_t pos_edge;  
        if (current_value == 0) neg_edge = elapsed_time;
        else pos_edge = elapsed_time;

        // in place of " " you can also use "\t"
        // time cast to a 16-bit value

        Serial.print((uint16_t) neg_edge);       // blue
        Serial.print(" ");
        Serial.print((uint16_t) pos_edge);       // red
        Serial.print(" ");
        Serial.print((uint16_t) time_max);       // green
        Serial.print(" ");
        Serial.print((uint16_t) time_min);       // orange
        Serial.print(" ");
        Serial.print((uint16_t) time_avg);       // purple 
        Serial.print(" ");
        Serial.println((uint16_t) elapsed_time); // gray
          
        break;
    }
  }
}

uint16_t runningAverage(uint16_t val)
{
  static uint16_t window[window_size] = {0};   // window into the data stream
  static int index = 0;          // index pointer into the circular buffer
  static uint16_t sum = 0;       // sum of the readings in the buffer

  uint16_t A_0 = window[index];  // oldest value
  uint16_t A_1 = val;            // newest value
  sum += A_1 - A_0;              // subtract out the oldest reading and
                                 // replace with the newest reading.
  // update
  window[index] = A_1;           // update the buffer
  index++;                       // move pointer
  if(index >= window_size){
   index = 0;
  }
  return sum/window_size;        // integer division (round down)  
}