#include <Servo.h>

Servo Weiche1;
Servo Weiche2;
Servo Weiche3;
Servo Weiche4;
Servo Schranke;

int Grad;

const int T_W = 53;
const int T_O = 52;
const int T_G1 = 51;

const byte T_S= 50;                 // Taster an Pin 1 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

boolean SchrankeOben = false;
boolean AlterSchrankenStatus = false;


// Leds
const int Pin_LED2_Rot = A0;         // LED Gerade an PIN A0 high
const int Pin_LED3_Gruen = A0;         // LED Gerade an PIN A0 low

const int Pin_LED6_Gruen = A1;         // LED Gerade an PIN A1 high
const int Pin_LED7_Rot = A1;         // LED Gerade an PIN A1 low

const int Pin_LED4_Rot = A2;         // LED Gerade an PIN A0 high
const int Pin_LED5_Gruen = A2;         // LED Gerade an PIN A0 low

const int Pin_LED8_Gruen = A3;         // LED Gerade an PIN A1 high
const int Pin_LED9_Rot = A3;         // LED Gerade an PIN A1 low

// Blinkleds
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;   
unsigned long BlinkStartZeit = 0;
unsigned long BlinkDauer     = 3000;  // drei Sekunden blinken lassen

//___________________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(LED_BUILTIN, OUTPUT);           // Led Pin13 

// 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, LOW);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED4_Rot, OUTPUT);
  digitalWrite(Pin_LED4_Rot, HIGH);     // 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, LOW);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet
  pinMode(Pin_LED9_Rot, OUTPUT);
  digitalWrite(Pin_LED9_Rot, HIGH);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus

// Schrankenstatus initiieren
  SchrankeOben = false;
  AlterSchrankenStatus = false;
}

//___________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=offen - 1=geschlossen
{               
  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() 
{

  while(digitalRead(T_W) == LOW)        //Start Weiche 1 zu 3
    {
      W1(1);
      digitalWrite(Pin_LED3_Gruen, HIGH);
      digitalWrite(Pin_LED8_Gruen, LOW);
      W2(0);
      digitalWrite(Pin_LED4_Rot, HIGH);
      digitalWrite(Pin_LED7_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_LED5_Gruen, LOW);
      digitalWrite(Pin_LED8_Gruen, LOW);
    }
  
  while(digitalRead(T_G1) == LOW)       //Start Weiche 3 zu 4
    {
      W2(1);
      digitalWrite(Pin_LED2_Rot, LOW);
      digitalWrite(Pin_LED9_Rot, HIGH);
      W1(0);
      digitalWrite(Pin_LED5_Gruen, LOW);
      digitalWrite(Pin_LED6_Gruen, HIGH);
    }

  if (SchrankeOben) {
    while(digitalRead(T_S) == LOW)      //Start Schranke schließen
    {
      S(0);
      SchrankeOben = false;
      BlinkStartZeit = millis();
    }
  } 
  if (!SchrankeOben) {
    while(digitalRead(T_S) == LOW)       //Start Schranke öffnen
     {
       S(1);
       SchrankeOben = true;
       BlinkStartZeit = millis();
    }
  }

    // Blinkled
  

  if (AlterSchrankenStatus != SchrankeOben){
    AlterSchrankenStatus = SchrankeOben;
    BlinkValue = HIGH;
    BlinkStartZeit = millis();
  };


  
  // BlinkWithoutDelay....
  unsigned long currentMillis = millis();
  if (BlinkValue == HIGH) 
  {                   
    if (currentMillis - previousMillis >= interval) 
    {
      previousMillis = currentMillis; 
      if (ledState == LOW)
        ledState = HIGH;
      else
        ledState = LOW;
      digitalWrite(LED_BUILTIN, ledState);
    }
   if (currentMillis - BlinkStartZeit >= BlinkDauer){
       BlinkValue = LOW;
   } 

  }
  else
    digitalWrite(LED_BUILTIN, LOW);            
  // Ende BlinkWithoutDelay....




 /*  
  TasterValue = digitalRead (T_S);        // Tasterstatus einlesen
  if (status != TasterValue)                 // Aenderung des Taster ?
  {
    if (TasterValue)                     // geschaltet
    {                          
      BlinkValue = !BlinkValue;                 // ...und wird gewechselt.
    }
    status = TasterValue;                       // Status merken
  }
*/
  
}