// pins connected
#define Bm 8              // B- line
#define Bp 9              // B+ line
#define Ap 10             // A+ line
#define Am 11             // A- line
#define pinLed 13

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

// Variabelen
byte spoelen[4] = {Bm, Ap, Bp, Am}; //arry van 4 spoelen
unsigned long step = 1;
bool richting = LOW;                // Wijzerzin
float interval;

unsigned long aanloopTijd = 2000;  // dit zijn step's
unsigned long cruiseTijd = 5000;
unsigned long uitloopTijd = 2000;
unsigned long pauzeTijd = 2000;
int startSnelheid = 10000;  // in micro seconden
int kruisSnelheid = 400;   // in micro sec
unsigned long resetTijd;
unsigned long stepStart;
byte motorStatus = VERSNEL;

// Functies
void doStep();                     //declareren van functie
void veranderSnelheid();

void setup() {

  Serial.begin(9600);
  pinMode(Bm, OUTPUT);
  pinMode(Bp, OUTPUT);
  pinMode(Ap, OUTPUT);
  pinMode(Am, OUTPUT);
  pinMode(pinLed, OUTPUT);
  resetTijd = millis();
  stepStart = 0;
}

void loop() {

  switch (motorStatus) {

    case VERSNEL:      //0
      veranderSnelheid(aanloopTijd, CRUISE, 1);
      /*
        if (step < aanloopTijd) {
          //interval = step;
          interval = (cos(float(step) / aanloopTijd * PI) + 1) / 2 * (startSnelheid - kruisSnelheid) + kruisSnelheid;
          if ( micros() - stepStart > interval) {
            doStep();
            stepStart = micros();
          }
        } else {
          motorStatus = CRUISE;
          resetTijd = millis();
          // Serial.println(" naar CRUISE");
        }
      */
      break;

    case CRUISE:       //1
      if (millis() - resetTijd < cruiseTijd) {
        if ( micros() - stepStart > interval) {
          doStep();
          stepStart = micros();
          // Serial.println("cruise");
        }
      } else {
        motorStatus = VERTRAAG;
        resetTijd = millis();
        step = 0;
        // Serial.println(" naar VERTRAAG");
      }
      break;

    case VERTRAAG:     // 2
      veranderSnelheid(uitloopTijd, PAUZE, -1);
      /*
        if (step < uitloopTijd) {
          interval = (-cos(float(step) / aanloopTijd * PI) + 1) / 2 * (startSnelheid - kruisSnelheid) + kruisSnelheid;
          if ( micros() - stepStart > interval) {
            doStep();
            stepStart = micros();
          }
          // Serial.println("vertraag");
        } else {
          motorStatus = PAUZE;
          resetTijd = millis();
          //Serial.println(" naar PAUZE");
        }
      */
      break;

    case PAUZE:        // 3
      if (millis() - resetTijd > pauzeTijd) {
        motorStatus = VERSNEL;
        resetTijd = millis();
        // Serial.println(" naar VERSNEL");
        richting = !richting;   // richting is niet richting
        step = 0;
      }
      //Serial.println("pauze");
      break;

    default:
      //Serial.println(" vreemde fout");
      break;
  }
}
void doStep() {
  // Serial.println(spoelen[step % 4]); // modulo met getal 4 om 0, 1, 2, 3 te krijgen

  if (richting == LOW) {
    digitalWrite(spoelen[(step - 1) % 4], LOW) ;
    digitalWrite(spoelen[step % 4], HIGH);
  } else {
    digitalWrite(spoelen[3 - ((step - 1) % 4)], LOW) ; // 3- geeft een andere volgorde ipv 0,1,2,3 nu 3,2,1,0
    digitalWrite(spoelen[3 - (step % 4)], HIGH);
  }
  step++;  // stap + 1  (ofwel andere schrijfwijze- step += 1)
  //Serial.println(interval);
}
//Serial.println(cos(float(step) / 100));   // cosinus functie, zie grafiek serial monitor

void veranderSnelheid(int aantalSteps, byte state, int x) {

  if (step < aantalSteps) {
    interval = (x * cos(float(step) / aantalSteps * PI) + 1) / 2 * (startSnelheid - kruisSnelheid) + kruisSnelheid;
    if ( micros() - stepStart > interval) {
      doStep();
      stepStart = micros();
    }
  } else {
    motorStatus = state;
    resetTijd = millis();
    // Serial.println(" naar CRUISE");
  }
}