/*
 * 1. Under Arduino Loop function 
 *    a) select the wheel you want to test.
 *    b) in case 1 and case 2 set the motor you want to test AIN1, AIN2 or BIN1, BIN2
 * 2. Switch Robot to PROG (Program)
 * 3. In the Arduino IDE menu bar set Tool > Port to Arxterra 3DoT
 * 3. Upload the program.
 * 4. Switch Robot to RUN
 * 5. In the Arduino IDE menu bar set Tool > Port to Arxterra 3DoT (note: it will be a different port number than in step 3).
 * 6. In the Arduino IDE menu bar Tool > Serial Monitor
 * 7. Enter the number of degrees you want to turn and press the Send button on the right of the text entry field.
 * 8. When test is over the serial monitor will display the number of steps and the turning error in degress.
 */

// 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 = 72;  // (360 degrees/rev)/ (5 degrees / pin change) = 72 pin change / rev)

uint32_t stopwatch_pressed;        // reset and start the stopwatch
uint32_t elapsed_time; 
uint8_t current_value;             // current wheel encoder output value
uint32_t time_min;
uint32_t time_max;
  
const int16_t encoder_wheel = 5;   // 5 degrees per slice
int16_t turn_angle = 180;          // turn motor this many degrees

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);

    digitalWrite(NSLEEP, HIGH);
    state = 0;               // FSM reset state 0
 
    time_min = 100;          // initial test is true
    time_max = 0;
}

void loop()
{ 
  uint8_t wheelA = digitalRead(EncoderA);
  uint8_t wheelB = digitalRead(EncoderB); // HIGH or LOW
  uint8_t test_this_wheel = wheelB;       // *********************

  static int error = 0;
  
  switch (state)
  {
     case 0:  // initialize test parameters
        if (Serial.available() > 0){ 
           turn_angle = Serial.parseInt();
           pulse_count = turn_angle / encoder_wheel;
           state++;
           Serial.print("pulse count = ");
           Serial.println(pulse_count);
        }

        break;
        
      case 1: // start test *********************
        analogWrite(AIN1, 0);     // left motor (simply a convention) is wired to encoder B
        analogWrite(AIN2, 255);   // standard is 230
        state++;
        break;
      
      case 2: // watch encoder
        if (pin_change(test_this_wheel)){
          pulse_count--;                    // decrement pulse counter
          if (pulse_count <= 0){
            state++;
          }
        }
        break;
    case 3: // end the test  *********************
        /* Switch from brake to coast (AIN2:AIN1 = 00) to watch 
         * the effect on stopping the motors.
         */
        digitalWrite(AIN1, 1);  // stop the motor(s) 
        digitalWrite(AIN2, 1);  // left motor (simply a convention) is wired to encoder B
        stopwatch_pressed = millis();   // start the stop watch
        state++;
        break;
    case 4: // report test result
        if (pin_change(test_this_wheel)) error++;   // increment the error counter

        // delay to insure motor(s) have come to a full and complete stop
        elapsed_time = millis() - stopwatch_pressed;
        if (elapsed_time > 1000){          // 1 second should do it
          Serial.print("turning error = ");
          Serial.print(error * 5);
          Serial.println(" degrees");
          state = 0;   // enter another angle
        }
       break;

    default:

        break;
    } // switch-case

}  // loop

// simulate with polling a pin change interrupt
bool pin_change(uint8_t value){       // value may be HIGH or LOw
  static uint8_t last_value = value; // initalize to current value
  bool trigger = false; 
  if (value != last_value){
    last_value = value;              // update to current value
    trigger = true; 
  }
  return trigger;
}


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)  
}