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

#define VERSNEL 0
#define CRUISE 1
#define VERTRAAG 2
#define PAUZE 3
#define AANLOOPTIJD 10000 //aantal milliseconden dat de versnelfaze duurt;
#define REMTIJD 3000 //aantal milliseconden dat de remfaze duurt


// Variabelen
byte spoelen[4] = {Bm, Ap, Bp, Am};  //array
unsigned long step = 1;

bool richting = LOW; // Wijzerzin
float startTimer= 0;
unsigned long stepStart;
unsigned long aanloopTijd = 400; // steps
unsigned long cruiseTijd = 4000;
unsigned long uitloopTijd = 400; // steps
unsigned long pauzeTijd = 2000;
unsigned long pulsDuur = 250;//maximale tijd dat een puls mag duren
unsigned long pulsTijd = 0;//tijd dat de puls duurt
int startSnelheid = 10000; // in microseconden
int kruisSnelheid = 400; // in microseconden
byte motorStatus = VERSNEL;
float interval;


// Functies

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

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(Ap, OUTPUT);
  pinMode(Am, OUTPUT);
  pinMode(Bp, OUTPUT);
  pinMode(Bm, OUTPUT);
  pinMode(PIN_LED, OUTPUT);
  startTimer = millis();
  pulsTijd = millis();
  stepStart = 0;
}

void loop() {
  switch (motorStatus) {

    case VERSNEL: // 0
      // veranderSnelheid(aanloopTijd, CRUISE, 1 );
      if (millis() - startTimer < AANLOOPTIJD) {
      // puls start op 250 ms, na 3 seconden is de puls nog 10 ms
      pulsDuur = 130 + 120 * cos((millis() - startTimer) / AANLOOPTIJD * PI);
      Serial.print ("status = VERSNEL ");
      Serial.print( " tijdsduur = ");Serial.print((millis()-startTimer));
      Serial.print (" pulsduur = ");
      Serial.println(pulsDuur);
      doStep();
        }
      else {
        motorStatus = CRUISE;
        pulsDuur = 10;
        startTimer = millis();
        }
      break;

    case CRUISE: // 1
      if (millis() - startTimer < cruiseTijd) {
        if (micros() - stepStart > interval) {
          doStep();
          stepStart = micros();
        }
        //Serial.println("cruise");
      } else {
        motorStatus = VERTRAAG;
        startTimer = millis();
        step = 0;
      }
      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;
        startTimer = millis();
      }

      break;

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

    default:
      Serial.println("grote fout");
  }
}



void doStep() {
  if ((millis() - pulsTijd) > pulsDuur) {
    if (richting == LOW) {
      digitalWrite(spoelen[(step - 1) % 4], LOW);
      digitalWrite(spoelen[step % 4], HIGH);
    } else {
      digitalWrite(spoelen[3 - ((step - 1) % 4)], LOW);
      digitalWrite(spoelen[3 - (step % 4)], HIGH);
    }
    step++;
    pulsTijd = millis();
  }
}


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();
    }
    //Serial.println("versnel");
  } else {
    motorStatus = state;
    startTimer = millis();
  }
}