#include <Arduino.h>
#include "4_Mecanum_Wheel_Vehicle.h"     //Use Class for Omni Vehicles

//************SET UP OF VEHICLE*********************//
four_mecanum_wheel_vehicle vehicle = four_mecanum_wheel_vehicle();    //CREATE AN INSTANCE CALLED VEHICLE OF TYPE OMNI_VEHICLE AND USE THE DIGITAL PIN FOR MICROSTEPPING AS AN INPUT
//***********END OF VEHICLE SETUP*******************//

unsigned long previousMillis = 0;

unsigned long forwardInterval = 2000;   //DELAY FOR 2 SEC
unsigned long leftInterval = 2000;
unsigned long backwardInterval = 1000;
unsigned long rightInterval = 3000;

int state = 0;

void setup() {
  Serial.begin(9600);
  pinMode(8, OUTPUT);
  vehicle.setAllMotorsSpeed(1000); //500?
}

void loop() {

  switch (state) {
    case 0:
      if (myDelay(previousMillis, forwardInterval)) {
        vehicle.run_forward(100);
        previousMillis = millis();  //to account for time during running
        state++;
      }
    break;

    case 1:
      if (myDelay(previousMillis, leftInterval)) {
        vehicle.run_left(100);
        previousMillis = millis();  //to account for time during running
        state++;
      }
    break;

    case 2:
      if (myDelay(previousMillis, backwardInterval)) {
        vehicle.run_backward(100);
        previousMillis = millis();  //to account for time during running
        state++;
      }
    break;

    case 3:
      if (myDelay(previousMillis, rightInterval)) {
        vehicle.run_right(100);
        previousMillis = millis();  //to account for time during running
        state = 0;
      }
    break;

    default:
      state = 0;
    break;
  }   

}

boolean myDelay(unsigned long &previousMillis, unsigned long interval) {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    return true;
  }
  else return false;
}