#include <Servo.h>
#include <MobaTools.h>

Servo Weiche1;                //Servo Kreuzweiche OL
Servo Weiche2;                //Servo Kreuzweiche OR
Servo Weiche3;                //Servo Kreuzweiche UL
Servo Weiche4;                //Servo Kreuzweiche UR
Servo Schranke;               //Einzelservo Oben
int Grad;

int T_W = 53;                   //Taster weiß Frahrstrecke an Kreuzweiche von OL-> UR
int T_O = 52;                   //Taster orange Frahrstrecke an Kreuzweiche von OL-> OR & UL-> UR
int T_G1 = 51;                  //Taster grün Frahrstrecke an Kreuzweiche von UL-> OR
const byte T_S = 50;              // Taster rot Schranke an Pin 50 und GND
const long interval = 500;        // Blinkintervall
const byte entprellzeit = 20;     // Zeitintervall der Tasterabfrage
boolean taster_gedrueckt = 0;     // Speichervariable für den Tasterzustand
boolean last_tasterzustand = 0;   // zur Erkennung der Flanke
MoToTimer Entprellen;
bool SchrankeValue = true;
// Leds je 2 Leds an 1 Pin -> Sterung mit LOW/HIGH
int Pin_LED2_Rot = A3;            // LED Gerade an PIN A0 high
int Pin_LED3_Gruen = A0;          // LED Gerade an PIN A0 low
int Pin_LED6_Gruen = A1;          // LED Gerade an PIN A1 high
int Pin_LED7_Rot = A2;            // LED Gerade an PIN A1 low
int Pin_LED4_Rot = A2;            // LED Gerade an PIN A0 high
int Pin_LED5_Gruen = A1;          // LED Gerade an PIN A0 low
int Pin_LED8_Gruen = A0;          // LED Gerade an PIN A1 high
int Pin_LED9_Rot = A3;            // LED Gerade an PIN A1 low

// Blinkleds
int Pin_LED1_Rot = A4;
int ledState = LOW;                       // Led-Status
uint8_t TasterValue = 0;                  // Taster-Status
uint8_t BlinkValue = 0;                   // Blink-Status
uint8_t status = 0;
unsigned long previousMillis = 0;
//___________________VOID SETUP______________
void setup()
{
  pinMode(T_W, INPUT_PULLUP);
  pinMode(T_O, INPUT_PULLUP);
  pinMode(T_G1, INPUT_PULLUP);
  pinMode(T_S, INPUT_PULLUP);
  // BlinkLed
  pinMode(Pin_LED1_Rot, OUTPUT);           // Led A4
  // Leds definieren
  pinMode(Pin_LED3_Gruen, OUTPUT);
  digitalWrite(Pin_LED3_Gruen, HIGH);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED2_Rot, OUTPUT);
  digitalWrite(Pin_LED2_Rot, LOW);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus
  pinMode(Pin_LED5_Gruen, OUTPUT);
  digitalWrite(Pin_LED5_Gruen, HIGH);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED4_Rot, OUTPUT);
  digitalWrite(Pin_LED4_Rot, LOW);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus
  pinMode(Pin_LED6_Gruen, OUTPUT);
  digitalWrite(Pin_LED6_Gruen, HIGH);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED7_Rot, OUTPUT);
  digitalWrite(Pin_LED7_Rot, LOW);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus
  pinMode(Pin_LED8_Gruen, OUTPUT);
  digitalWrite(Pin_LED8_Gruen, HIGH);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED9_Rot, OUTPUT);
  digitalWrite(Pin_LED9_Rot, LOW);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus
}
//___________FUNKTION "schalten"________________
void schalten (Servo Weiche, int PIN, int Winkel)
{
  Weiche.attach(PIN);
  Weiche.write(Winkel);
  delay(200);
  Weiche.detach();
}
//___________FUNKTION "W1"___________________
void W1(int Stellung)                //0=gerade - 1=abbiegen
{ if (Stellung == 0) {                //Stellung gerade aus
    Grad = 0;                         //Position in Grad
  }
  if (Stellung == 1) {                //Stellung abbiegend
    Grad = 90;                        //Position in Grad
  }
  schalten(Weiche1, 2, Grad);         //(Servo, Anschlusspin, Position)
}
//___________FUNKTION "W2"___________________
void W2(int Stellung)               //0=gerade - 1=abbiegen
{ if (Stellung == 0) {                //Stellung gerade aus
    Grad = 0;                         //Position in Grad
  }
  if (Stellung == 1) {                //Stellung abbiegend
    Grad = 90;                        //Position in Grad
  }
  schalten(Weiche2, 3, Grad);         //(Servo, Anschlusspin, Position)
}
//___________FUNKTION "W3"___________________
void W3(int Stellung)                //0=gerade - 1=abbiegen
{ if (Stellung == 0) {                //Stellung gerade aus
    Grad = 0;                         //Position in Grad
  }
  if (Stellung == 1) {                //Stellung abbiegend
    Grad = 90;                        //Position in Grad
  }
  schalten(Weiche3, 2, Grad);         //(Servo, Anschlusspin, Position)
}
//___________FUNKTION "W4"___________________
void W4(int Stellung)                //0=gerade - 1=abbiegen
{ if (Stellung == 0) {                //Stellung gerade aus
    Grad = 0;                         //Position in Grad
  }
  if (Stellung == 1) {                //Stellung abbiegend
    Grad = 90;                        //Position in Grad
  }
  schalten(Weiche4, 3, Grad);         //(Servo, Anschlusspin, Position)
}
//___________FUNKTION "Schranke"___________________
void S(int Stellung)   //void S(int Stellung) //0=gerade - 1=abbiegen
{
  if (Stellung == 0) {                //Stellung gerade aus
    Grad = 0;                         //Position in Grad
  }
  if (Stellung == 1) {                //Stellung abbiegend
    Grad = 90;                        //Position in Grad
  }
  schalten(Schranke, 4, Grad);        //(Servo, Anschlusspin, Position)
}
//_____________VOID LOOP______________________
void loop()
{
  // Entprellen des Tasters
  if ( !Entprellen.running() )
  { // Der Eingang wird nur alle 'entprellzeit' ms eingelesen
    Entprellen.setTime( entprellzeit );                   // Entprellzeit neu setzen
    taster_gedrueckt = ! digitalRead(T_S);                // invertiert mit ! wegen INPUT_PULLUP ( Taster schaltet gegen Gnd )
    if ( taster_gedrueckt == 0 ) last_tasterzustand = 0;  // Zur Flankenerkennung 'gedrückt'

    while (digitalRead(T_W) == LOW) {      //Start Weiche 1 zu 3
      W1(1);
      digitalWrite(Pin_LED3_Gruen, HIGH);
     digitalWrite(Pin_LED5_Gruen, LOW);
      W2(0);
      digitalWrite(Pin_LED4_Rot, HIGH);
      digitalWrite(Pin_LED2_Rot, LOW);
    }
    while (digitalRead(T_O) == LOW) {      //Start Weiche 1/2 und 3/4
      W1(0);
      digitalWrite(Pin_LED3_Gruen, HIGH);
      digitalWrite(Pin_LED6_Gruen, HIGH);
      W2(0);
      digitalWrite(Pin_LED2_Rot, LOW);
      digitalWrite(Pin_LED4_Rot, LOW);
    }
    while (digitalRead(T_G1) == LOW) {     //Start Weiche2 zu 4
      W2(1);
      digitalWrite(Pin_LED2_Rot, HIGH);
      digitalWrite(Pin_LED3_Gruen, LOW);
      W1(0);
      digitalWrite(Pin_LED5_Gruen, HIGH);
      digitalWrite(Pin_LED4_Rot, LOW);
    }
    if (digitalRead(T_S) == LOW) {     //Start Schranke runter
      SchrankeValue = !SchrankeValue;
      S(SchrankeValue ? 1 : 0);
      delay(50); // Debounce
      while (digitalRead(T_S) == LOW) { }
    }
    // Blinkled BlinkWithoutDelay....
    unsigned long currentMillis = millis();
    if (SchrankeValue == HIGH)
    {
      if (currentMillis - previousMillis >= interval)
      {
        previousMillis = currentMillis;
        if (ledState == LOW)
          ledState = HIGH;
        else
          ledState = LOW;
        digitalWrite(Pin_LED1_Rot, ledState);
      }
    }
    else
      digitalWrite(Pin_LED1_Rot, LOW);
    // Ende BlinkWithoutDelay....
  }
}