#include <Servo.h>

/* *****  instellingen van tijden, zijn aan te passen indien nodig  *****  */

int rodeLedKnippersnelheid = 1000;                     // knipper snelheid rodeleds
int witteLedKnippersnelheid = 2000;                    // knipper snelheid witteleds
int servo1Min = 10;                                    // Servo 1 Minimum 
int servo2Min = 10;                                    // Servo 2 Minimum 
int servo1Max = 100;                                   // Servo 1 Maximum 
int servo2Max = 100;                                   // Servo 2 Maximum 
int sluitSnelheid = 15;                                 // 5 = snel, 25 = gemiddeld, 50 = langzaam
int tijdsDuur[5] = {100, 1000, 100, 1000, 100};        // ingestelde tijden
                                                // {knipperen, sluiten, wachten voor openen, openen, knipperen}

/* ***** aansluitpins randapparatuur + vaste code, vanaf hier niets wijzigen *****  */

int knipperLedWit = 6;                // aansluitpinnen verschillende componenten
int knipperLedRood1 = 5;              // rode led 1
int knipperLedRood2 = 7;              //rode led 2
int slagboomServo1 = 8;               //slagboom servo 1
int slagboomServo2 = 9;               //slagboom servo 2
int sensorPin1 = 2;                   // digitale uitganspin sensor 1    
int sensorPin2 = 4;                   // digitale uitganspin sensor 1 
int audioPin = 10;                    // aansluitpin om WTV020 audio player te resetten

/*     *******************  variabelen  *****************   */

long myTimer = 0;                        // Timer
long servoTimer = 0;                     // Timer der Servos
Servo myServo1;                          // Servo  1
Servo myServo2;                          // Servo  2
int myServoPosition[2] = {80, 80};       // schrijft de huidige servo positie weg
int myState = 0;                         // schrijft de toestand weg van de voortgang
// 0 = witte knipperled , 1 = rode knipper led, 2 = slagboom sluit, 3=slagboom gesloten, 4 = slagboom openen

void setup() {
 
  myServo1.attach(slagboomServo1);
  myServo2.attach(slagboomServo2);
   myServo1.write(80);
  myServo2.write(80);
  delay (200);
  myServo1.write(servo1Max);
  myServo2.write(servo2Max);
  myServoPosition[0] = servo1Min;
  myServoPosition[1] = servo2Min;
  //delay(200);                             
  pinMode(knipperLedRood1, OUTPUT);
  pinMode(knipperLedRood2, OUTPUT);
  pinMode(sensorPin1, INPUT);
  pinMode(sensorPin2, INPUT);
  pinMode(audioPin, OUTPUT);
}
void loop() {

  if ((digitalRead(sensorPin1) == 0) || (digitalRead(sensorPin2) == 0)) {
    if (myState == 0) {
      myState = 1;
      myTimer = millis();
    } else if ((myState == 5) || (myState == 6)) myState = 2;
  } else {
    if (myState == 3) {
      myState = 4;
      myTimer = millis();          // Timer resetten
    }
  }


     switch (myState) {
      case 0: // witte led knipperd
        witteLedKnipperd();
        break;
      case 1: // rode leds knipperen
      digitalWrite(audioPin, HIGH);
        rodeLedsKnipperen();             // rode led knipperen
        if (myTimer + tijdsDuur[0] < millis()) { // doorlooptijd
          myTimer = millis();          // Timer resetten
          myState = 2;                // ga naar volgende toestand
        }
        break;
      case 2: // slagbomen sluiten
        rodeLedsKnipperen();             // rode led knipperen
        slagboomSluiten();
        if (myTimer + tijdsDuur[1] < millis()) { // doorlooptijd
          myTimer = millis();          // Timer resetten
          myState = 3;                // ga naar volgende toestand
        }
        break;
      case 3: // slagbomen gesloten
        rodeLedsKnipperen();             // rode led knipperen
        break;
      case 4: // Schranke öffnen
        if (myTimer + tijdsDuur[2] < millis()) { // doorlooptijd
          myTimer = millis();          // Timer resetten
          myState = 5;                 // ga naar volgende toestand
        }        
        break;
      case 5: // slagbomen openen
        rodeLedsKnipperen();             // rode led knipperen
        slagboomOpenen();
        if (myTimer + tijdsDuur[3] < millis()) { // doorlooptijd
          myTimer = millis();          // Timer resetten
          myState = 6;                // ga naar volgende toestand
        }
        break;
      case 6: // rode leds knipperen
        rodeLedsKnipperen();             // rode led knipperen
        if (myTimer + tijdsDuur[4] < millis()) { // doorlooptijd
          myTimer = millis();          // Timer resetten
          myState = 0;                // ga naar volgende toestand
        }
        break;
    }
  }


  void rodeLedsKnipperen() {
    //digitalWrite(audioPin, HIGH);
  if (millis() % rodeLedKnippersnelheid * 2 < rodeLedKnippersnelheid) {
    digitalWrite(knipperLedWit, LOW );
    digitalWrite(knipperLedRood1, LOW);
    digitalWrite(knipperLedRood2, HIGH);
  } else {
    digitalWrite(knipperLedRood1, HIGH);
    digitalWrite(knipperLedRood2, LOW);
  }
}

    void witteLedKnipperd() {
  digitalWrite(knipperLedRood1, LOW);
  digitalWrite(knipperLedRood2, LOW);
  digitalWrite(audioPin, LOW);
  
 if (millis() % witteLedKnippersnelheid * 2 < witteLedKnippersnelheid) {
    digitalWrite(knipperLedWit, LOW );
    } else {
   digitalWrite(knipperLedWit, HIGH );
  }
}
    
   void slagboomSluiten() {
     
  if (servoTimer + sluitSnelheid < millis()) {
    if (myServoPosition[0] > servo1Min) myServoPosition[0]--;
    myServo1.write(myServoPosition[0]);
    if (myServoPosition[1] > servo2Min) myServoPosition[1]--;
    myServo2.write(myServoPosition[1]);
    servoTimer = millis();
  }
}
    
 void slagboomOpenen() {
  if (servoTimer + sluitSnelheid < millis()) {
    if (myServoPosition[0] < servo1Max) myServoPosition[0]++;
    myServo1.write(myServoPosition[0]);
    if (myServoPosition[1] < servo2Max) myServoPosition[1]++;
    myServo2.write(myServoPosition[1]);
    servoTimer = millis();
  }
  
}