#define Bm 8
#define Bp 9
#define Ap 10
#define Am 11
#define PIN_LED 13

#define VERSNEL 0
#define CRUISE 1
#define VERTRAAG 2
#define PAUZE 3



// Variabelen
byte spoelen[4] = {Bm, Ap, Bp, Am};
unsigned long step = 1;
bool richting = HIGH;
unsigned long aanloopTijd = 2000;
unsigned long cruiseTijd = 5000;
unsigned long remtijd = 2000;
unsigned long pauze = 1000;
unsigned long startMoment;
byte vertrager = 10;
byte motorStatus = VERSNEL;

// Functions
void doStep();

void setup() {
  Serial.begin(9600);
  pinMode(Bm, OUTPUT); //8
  pinMode(Bp, OUTPUT); //9
  pinMode(Am, OUTPUT); //10
  pinMode(Ap, OUTPUT); //11
  pinMode(PIN_LED, OUTPUT); //13
  startMoment = millis();
}

void loop() {
  /*
    if ((millis() - startMoment) < aanloopTijd) {
      //aanloopmoment
      vertrager = 20;
      doStep ();
    } else {
      //rapdraaien
      vertrager = 5;
      doStep ();

    };
  */

  switch (motorStatus) {
    case VERSNEL: // 0
      if ((millis() - startMoment) < aanloopTijd) {
        doStep();
      } else {
        motorStatus = CRUISE;
        vertrager = 10;
        startMoment = millis();
      }
      break;
    case CRUISE :  // 1
      Serial.println("status = CRUISE");
      if (millis() - startMoment < cruiseTijd) {
        doStep();
      } else {
        motorStatus = VERTRAAG;
        vertrager = 100;
        startMoment = millis();
      }
      break;
    case VERTRAAG: //2
      if ((millis() - startMoment) < remtijd) {
        doStep();
      } else {
        motorStatus = PAUZE;
        vertrager = 100;
        startMoment = millis();
        break;
      case PAUZE: //3
        break;
      default:
        Serial.println("stomme fout");
      }





  }

  void doStep() {

    step++; // step = step + 1 / step += 1
    if (richting == LOW) {
      delay(vertrager);
      digitalWrite(spoelen[(step - 1) % 4], LOW) ;
      digitalWrite(spoelen[step % 4], HIGH);
    } else {
      delay(vertrager);
      digitalWrite(spoelen[3 - ((step - 1) % 4)], LOW) ;
      digitalWrite(spoelen[3 - (step % 4)], HIGH);
    }
  }
}