#define PIN_LED_WIT 5
#define PIN_LED_ROODR 6
#define PIN_LED_ROODL 7
#define READ_PIN 4

// Statemachine CONTROLE
#define CONTROLE_Opstart 100
#define CONTROLE_CheckWit 110
#define CONTROLE_StandOnveilig 120
#define CONTROLE_CheckRood 130
#define CONTROLE_FunctieWacht 140

// Statemachine WITLICHT_
#define WITLICHT_Opstart 200
#define WITLICHT_BlijftUit 210
#define WITLICHT_FunctieStart 220
#define WITLICHT_GaatAan 230
#define WITLICHT_BlijftAan 240
#define WITLICHT_GaatUit 250
#define WITLICHT_wachtControle 260
#define WITLICHT_FunctieWacht 270

// Statemachine ROODLICHT_
#define ROODLICHT_Opstart 300
#define ROODLICHT_FunctieStart 310
#define ROODLICHTL_GaatAan 320
#define ROODLICHTL_BlijftAan 330
#define ROODLICHTLR_GaatUitAan 331
#define ROODLICHTLR_BlijftAan 332
#define ROODLICHTLR_GaatAanUit 333
#define ROODLICHTLR_BlijftUit 334
#define ROODLICHTL_GaatUit 340
#define ROODLICHT_BlijftUit 350
#define ROODLICHT_FunctieWacht 360

// Statemachine SLAGBOOM_
#define SLAGBOOM_Opstart 400
#define SLAGBOOM_StartSluiten 410
#define SLAGBOOM_Sluit 420
#define SLAGBOOM_StartOpen 430
#define SLAGBOOM_Open 440
#define SLAGBOOM_FunctieWacht 450

// Servo library.
#include <Servo.h>
Servo myServo1;
Servo myServo2;

// vars WITLICHT_
int stateWitLicht = WITLICHT_Opstart;
unsigned long witFadeTijd = 350;
unsigned long witAanTijd = 10;
unsigned long witUitTijd = 580;
unsigned long resetWitLicht = 0;
// gezien analogwrite max 254 kan zijn moet de 254/fadetijd
float witFadeStep =  254.0 / witFadeTijd;
int brightnessWit = 0;


// vars ROODLICHT_
int stateRoodLicht = ROODLICHT_Opstart;
unsigned long roodFadeTijd = 250;
unsigned long roodAanTijd = 100;
unsigned long resetRoodLicht = 0;
// gezien analogwrite max 254 kan zijn moet de 254/fadetijd
float roodFadeStep =  254.0 / roodFadeTijd;
int brightnessRoodL = 0;
int brightnessRoodR = 0;

// Vars SLAGBOOM_
int stateSlagboom = SLAGBOOM_FunctieWacht;
//unsigned long roodFadeTijd = 250;
unsigned long slagboomWachtTijd = 5000;
unsigned long resetSlagboomWacht = 0;
/*dit is de tijd voor 1 graden te bewegen
  unsigned long slagboomStap = (unsigned long)(3000 / 90) ; // x90
  unsigned long resetSlagboom = 0;
  int slagboomHoek;
*/
// dit is de tijd voor 1 graden te bewegen (Slagboom sluiten)
unsigned long slagboomStapSluiten = (unsigned long)(8000 / 90) ; // x90
unsigned long resetSlagboomSluiten = 0;
int slagboomHoekSluiten;
// dit is de tijd voor 1 graden te bewegen (Slagboom openen)
unsigned long slagboomStapOpenen = (unsigned long)(3000 / 90) ; // x90
unsigned long resetSlagboomOpenen = 0;
int slagboomHoekOpenen;

// Vars CONTROLE
int stateControle = CONTROLE_Opstart;
bool overwegOnveilig;

// Vars READ PIN
bool buttonState = 0;

// Vars helper-stukje
int oldstateWit = 0;
int oldstateRood = 0;
int oldstateSlagboom = 0;
int oldstateControle = 0;

void setup() {

  Serial.begin(115200);
  Serial.println(" Overweg versie zonder Sound ");

  pinMode(PIN_LED_WIT, OUTPUT);
  pinMode(PIN_LED_ROODL, OUTPUT);
  pinMode(PIN_LED_ROODR, OUTPUT);
  pinMode(READ_PIN, INPUT);
  buttonState = digitalRead(READ_PIN);

  myServo1.attach(A0);
  myServo2.attach(A1);
  slagboomHoekOpenen = 0;
  delay(100);
  myServo1.write(slagboomHoekOpenen);
  myServo2.write(slagboomHoekOpenen);
  delay (1000);

}

void loop() {

  switch (stateControle) {

    case CONTROLE_Opstart: //100
      Serial.println("controle opstart  100");
      stateControle = CONTROLE_FunctieWacht;
      break;

    case CONTROLE_CheckWit: //110
      buttonState = digitalRead(READ_PIN);
      Serial.println("controle wit   110");
      if (buttonState == LOW) {
        stateWitLicht = WITLICHT_BlijftUit;
        stateControle = CONTROLE_FunctieWacht;
      } else {
        stateControle = CONTROLE_StandOnveilig;
        stateWitLicht = WITLICHT_FunctieWacht;
      }
      break;


    case CONTROLE_StandOnveilig: //120
      overwegOnveilig = true;
      stateRoodLicht = ROODLICHT_FunctieStart;
      stateControle = CONTROLE_FunctieWacht;
      if (slagboomHoekSluiten < 89) {
        stateSlagboom = SLAGBOOM_StartSluiten;
        resetSlagboomWacht = millis();
        Serial.println("Slagbomen sluiten");
      }
      break;

    case CONTROLE_CheckRood: //130
      buttonState = digitalRead(READ_PIN);
      Serial.println("controle rood");
      if (buttonState == HIGH) {
        stateRoodLicht = ROODLICHTLR_GaatUitAan;
        stateControle = CONTROLE_FunctieWacht;
      } else {
        stateSlagboom = SLAGBOOM_StartOpen;
        Serial.println("Slagbomen openen zich");
        stateControle = CONTROLE_FunctieWacht;
      }
      break;

    case CONTROLE_FunctieWacht: //140
      // Serial.println("controle wacht");
      break;

    default:
      Serial.println("Probleem Controle");
  }

  switch (stateWitLicht) {
    case WITLICHT_Opstart: // 200
      digitalWrite(PIN_LED_WIT, LOW);
      Serial.println("opstart witlicht  200");
      stateWitLicht = WITLICHT_BlijftUit;
      break;

    case WITLICHT_BlijftUit: //210
      analogWrite(PIN_LED_WIT, 0);
      if (millis() - resetWitLicht > witUitTijd) {
        resetWitLicht = millis();
        stateWitLicht = WITLICHT_FunctieStart;
      }
      break;

    case WITLICHT_FunctieStart: // 220
      Serial.println("Wit licht in actie");
      resetWitLicht = millis();
      stateWitLicht = WITLICHT_GaatAan;
      brightnessWit = 0;
      break;

    case WITLICHT_GaatAan: // 230
      if (millis() - resetWitLicht < witFadeTijd) {
        brightnessWit = (int)(witFadeStep * (millis() - resetWitLicht) );
        analogWrite(PIN_LED_WIT, brightnessWit);
      } else {
        resetWitLicht = millis();
        stateWitLicht = WITLICHT_BlijftAan;
      }
      break;

    case WITLICHT_BlijftAan: // 240
      analogWrite(PIN_LED_WIT, 254);
      if (millis() - resetWitLicht > witAanTijd) {
        resetWitLicht = millis();
        stateWitLicht = WITLICHT_GaatUit;
      }
      break;

    case WITLICHT_GaatUit: // 250
      if (millis() - resetWitLicht < witFadeTijd) {
        brightnessWit = 254 - (int)(witFadeStep * (millis() - resetWitLicht));
        analogWrite(PIN_LED_WIT, brightnessWit);
      } else {
        resetWitLicht = millis();
        stateWitLicht = WITLICHT_wachtControle;
      }
      break;

    case WITLICHT_wachtControle: //260
      analogWrite(PIN_LED_WIT, 0);
      stateControle = CONTROLE_CheckWit;
      break;

    case WITLICHT_FunctieWacht: // 270
      digitalWrite(PIN_LED_WIT, 0);
      //Serial.println("witlicht wacht");
      break;

    default:
      Serial.println("Probleem Witlicht");
  }

  switch (stateRoodLicht) {

    case ROODLICHT_Opstart: //300
      digitalWrite(PIN_LED_ROODL, LOW);
      digitalWrite(PIN_LED_ROODR, LOW);
      Serial.println("opstart roodlicht   300");
      stateRoodLicht = ROODLICHT_FunctieWacht;
      break;

    case ROODLICHT_FunctieStart: //310
      resetRoodLicht = millis();
      Serial.println("Rood licht in actie");
      stateRoodLicht = ROODLICHTL_GaatAan;
      brightnessRoodL = 0;
      brightnessRoodR = 0;
      resetSlagboomWacht = millis();
      break;

    case ROODLICHTL_GaatAan: //320
      if (millis() - resetRoodLicht < roodFadeTijd) {
        brightnessRoodL = (int)(roodFadeStep * (millis() - resetRoodLicht) );
        analogWrite(PIN_LED_ROODL, brightnessRoodL);
      } else {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHTL_BlijftAan;
      }
      break;

    case ROODLICHTL_BlijftAan: //330
      if (millis() - resetRoodLicht > roodAanTijd) {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHTLR_GaatUitAan;
      }
      break;

    case ROODLICHTLR_GaatUitAan: //331
      if (millis() - resetRoodLicht < roodFadeTijd) {
        brightnessRoodL = 254 - (int)(roodFadeStep * (millis() - resetRoodLicht));
        brightnessRoodR = (int)(roodFadeStep * (millis() - resetRoodLicht));
        analogWrite(PIN_LED_ROODL, brightnessRoodL);
        analogWrite(PIN_LED_ROODR, brightnessRoodR);
      } else {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHTLR_BlijftAan;
      }
      break;

    case ROODLICHTLR_BlijftAan: //332
      if (millis() - resetRoodLicht > roodAanTijd) {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHTLR_GaatAanUit;
      }
      break;

    case ROODLICHTLR_GaatAanUit: //333
      if (millis() - resetRoodLicht < roodFadeTijd) {
        brightnessRoodL = (int)(roodFadeStep * (millis() - resetRoodLicht));
        brightnessRoodR = 254 - (int)(roodFadeStep * (millis() - resetRoodLicht));
        analogWrite(PIN_LED_ROODL, brightnessRoodL);
        analogWrite(PIN_LED_ROODR, brightnessRoodR);
      } else {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHTLR_BlijftUit;
      }
      break;

    case ROODLICHTLR_BlijftUit: //334
      if (millis() - resetRoodLicht > roodAanTijd) {
        resetRoodLicht = millis();
        if (buttonState == LOW && overwegOnveilig == true) {
          stateRoodLicht = ROODLICHTLR_GaatUitAan;
        } else if (buttonState == LOW && overwegOnveilig == false) {
          stateRoodLicht = ROODLICHTL_GaatUit;
        } else {
          stateControle = CONTROLE_CheckRood;
        }
      }

      break;

    case ROODLICHTL_GaatUit: //340
      if (millis() - resetRoodLicht < roodFadeTijd) {
        brightnessRoodL = 254 - (int)(roodFadeStep * (millis() - resetRoodLicht));
        analogWrite(PIN_LED_ROODL, brightnessRoodL);
      } else {
        resetRoodLicht = millis();
        stateRoodLicht = ROODLICHT_BlijftUit;
      }
      break;

    case ROODLICHT_BlijftUit: //350
      analogWrite(PIN_LED_ROODL, 0);
      analogWrite(PIN_LED_ROODR, 0);
      if (millis() - resetRoodLicht > roodAanTijd) {
        resetRoodLicht = millis();
        stateWitLicht = WITLICHT_FunctieStart;
        stateRoodLicht = ROODLICHT_FunctieWacht;
      }
      break;

    case ROODLICHT_FunctieWacht: //360
      //Serial.println("roodlicht wacht");
      break;

    default:
      Serial.println("Probleem Roodlicht");
  }

  switch (stateSlagboom) {
    case SLAGBOOM_Opstart: // 400
      Serial.println("Opstart Slagbomen   400");
      stateSlagboom = SLAGBOOM_FunctieWacht;
      break;

    case SLAGBOOM_StartSluiten: // 410
      if (millis() - resetSlagboomWacht >= slagboomWachtTijd ) {
        slagboomHoekSluiten = 0;
        stateSlagboom = SLAGBOOM_Sluit;
      }
      //resetSlagboomWacht = millis();
      break;

    case SLAGBOOM_Sluit: // 420
      if (millis() - resetSlagboomSluiten > slagboomStapSluiten) {
        slagboomHoekSluiten ++ ;
        myServo1.write(slagboomHoekSluiten);
        myServo2.write(slagboomHoekSluiten);
        resetSlagboomSluiten = millis();
        if (slagboomHoekSluiten > 89) {
          stateSlagboom = SLAGBOOM_FunctieWacht;
        }
        //resetSlagboomSluiten = millis();
      }
      break;

    case SLAGBOOM_StartOpen: // 430
      slagboomHoekOpenen = 90;
      resetSlagboomOpenen = millis();
      stateSlagboom = SLAGBOOM_Open;
      break;

    case SLAGBOOM_Open: // 440
      if (millis() - resetSlagboomOpenen > slagboomStapOpenen) {
        slagboomHoekOpenen -- ;
        myServo1.write(slagboomHoekOpenen);
        myServo2.write(slagboomHoekOpenen);
        resetSlagboomOpenen = millis();
        if (slagboomHoekOpenen < 2) {
          overwegOnveilig = false;
          stateSlagboom = SLAGBOOM_FunctieWacht;
          //stateRoodLicht = ROODLICHTL_GaatUit;
        }
      }
      break;

    case SLAGBOOM_FunctieWacht: // 450
      // Serial.println("slagboom wacht");
      break;

    default:
      Serial.println("Probleem slagbomen");
  }

  // helper-stukje om de states te controleren als ze veranderen
  if (oldstateWit  != stateWitLicht) {
    Serial.println(stateWitLicht);
    oldstateWit  = stateWitLicht;
  }

  // helper-stukje om de states te controleren als ze veranderen
  if (oldstateRood  != stateRoodLicht) {
    Serial.println(stateRoodLicht);
    oldstateRood  = stateRoodLicht;
  }

  // helper-stukje om de states te controleren als ze veranderen
  if (oldstateSlagboom  != stateSlagboom) {
    Serial.println(stateSlagboom);
    oldstateSlagboom  = stateSlagboom;
  }

  // helper-stukje om de states te controleren als ze veranderen
  if (oldstateControle  != stateControle) {
    Serial.println(stateControle);
    oldstateControle = stateControle;
  }
}