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

enum  {
      UNBEKANNT,
      NACHLINKS,
      LINKS,
      NACHRECHTS,
      RECHTS
} Weichenstatus;

enum {
    WEICHE1,
    WEICHE2,
    WEICHE3,
    WEICHE4
} WeichenNummer;

enum {
    GERADE,
    ABBIEGEN
} WeichenStatus;

enum {
    OBEN,
    UNTEN
} SchrankenStatus;


struct WeichenTyp {
       Servo   servo;
       int     Abbiegen    = 90;
       int     Gerade      =  0;
       int     servoPin;
       boolean isAttached = false;
       unsigned long SchaltDelay = 200;
       unsigned long lastSchaltTime ;
};

struct SchrankenTyp {
       Servo   servo;
       int     Unten    = 90;
       int     Oben     =  0;
       int     Zustand  = 90;
       int     servoPin;
       boolean isAttached = false;
       unsigned long SchaltDelay = 200;
       unsigned long lastSchaltTime ;
       boolean Schliessen = false;
       boolean Oeffnen = false;
};

SchrankenTyp Schranke;

const int WeichenZahl = 4;
WeichenTyp Weiche[WeichenZahl];

//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 = A0;            // 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 = A1;            // LED Gerade an PIN A1 low
int Pin_LED4_Rot = A2;            // LED Gerade an PIN A0 high
int Pin_LED5_Gruen = A2;          // LED Gerade an PIN A0 low
int Pin_LED8_Gruen = A3;          // LED Gerade an PIN A1 high
int Pin_LED9_Rot = A3;            // LED Gerade an PIN A1 low
// Blinkleds
int ledState = LOW;                       // Led-Status
uint8_t TasterValue = 0;                  // Taster-Status
boolean SchrankeIstUnten = true;
unsigned long previousMillis = 0;


enum  {
     WEISS,
     ORANGE,
     GRUEN,
     ROT
} Tastenfarbe;

const int TastenDelay = 50;
int TastenPin[4] = {53,52,51,50};

struct Tastentyp{
        boolean ausloesen = false;
        boolean gedrueckt = false;
        boolean zeitsetzen   = true;
        unsigned long lastTastenDruck;
};

Tastentyp Taste[4];

//___________FUNKTION "Taste Gedrueckt"________________
boolean TasteGedrueckt(int Farbe){
    boolean status = !digitalRead(TastenPin[Farbe]);
    if (status != Taste[Farbe].gedrueckt && Taste[Farbe].zeitsetzen){
       Taste[Farbe].zeitsetzen = false;
       Taste[Farbe].lastTastenDruck = millis();
    }  
    if(millis()-Taste[Farbe].lastTastenDruck > TastenDelay){
       Taste[Farbe].ausloesen = (!Taste[Farbe].gedrueckt && status);
       Taste[Farbe].gedrueckt = status;
       Taste[Farbe].zeitsetzen = true;                    
    }
    
  return Taste[Farbe].gedrueckt;  
};


//___________FUNKTION "schalten ohne delay"________________
void schaltenOhneDelay (int Nr, int Winkel)
{
  if (!Weiche[Nr].isAttached){   // Falls der Pin noch belegt ist ....
    Weiche[Nr].servo.attach(Weiche[Nr].servoPin);
    Weiche[Nr].isAttached = true;
    Weiche[Nr].lastSchaltTime = millis();
    Weiche[Nr].servo.write(Winkel);
  }  else {
    Serial.print(Nr);Serial.println("\t Weichennummer");
    Serial.print(Weiche[Nr].servoPin);Serial.println("\t ist noch belegt!");
    }
}
void SchrankeSchaltenOhneDelay (SchrankenTyp schranke)
{
  if (!Schranke.isAttached){   // Falls der Pin noch belegt ist ....
    Schranke.servo.attach(Schranke.servoPin);
    Schranke.isAttached = true;
    Schranke.lastSchaltTime = millis();
    Schranke.servo.write(schranke.Zustand);
    if (Schranke.Zustand == Schranke.Oben) digitalWrite(LED_BUILTIN, LOW);
  }  else {
    Serial.println("Schranke");
    Serial.print(Schranke.servoPin);Serial.println("\t ist noch belegt!");
  }
}

void SchrankeLangsam(){
  static unsigned long previousTime = 0;
   if (Schranke.Oeffnen && Schranke.Zustand > Schranke.Oben){
     if (millis() - previousTime > 250) {
          previousTime = millis();
          Schranke.Schliessen = false;
          Schranke.Zustand -= 10;
          if (Schranke.Zustand < Schranke.Oben) {
              Schranke.Zustand = Schranke.Oben;
              Schranke.Oeffnen = false;
              };
          SchrankeSchaltenOhneDelay(Schranke);
      }
   }
   if (Schranke.Schliessen && Schranke.Zustand < Schranke.Unten){
      if (millis() - previousTime > 250) {
           previousTime = millis();
          Schranke.Zustand += 10;
          Schranke.Oeffnen = false;
          if (Schranke.Zustand > Schranke.Unten) {
              Schranke.Zustand = Schranke.Unten;
              Schranke.Schliessen = false;
              };
          SchrankeSchaltenOhneDelay(Schranke);
      }
   }

}

//___________FUNKTION "Detach nach Intervall"________________
void DetachNachZeit() {
  for (int i = WEICHE1; i <= WEICHE4; i++) {
    if(Weiche[i].isAttached){
      if(millis()-Weiche[i].lastSchaltTime > Weiche[i].SchaltDelay){
         Weiche[i].servo.detach();
         Weiche[i].isAttached = false;
      }
    }
  }
  if (Schranke.isAttached){
      if(millis()-Schranke.lastSchaltTime > Schranke.SchaltDelay){
         Schranke.servo.detach();
         Schranke.isAttached = false;
      }
  } 
}

//___________FUNKTION "Bewege Weiche Nr. x in Richtung"___________________
void schalte(int Nr, int Richtung)                //GERADE oder ABBIEGEN
{ 
  int Grad;
  if (Richtung == GERADE) {Grad = Weiche[Nr].Gerade;
  }                 else  {Grad = Weiche[Nr].Abbiegen;};
  schaltenOhneDelay(Nr, Grad);
}


//___________FUNKTION "Weichen und Schranke initialisieren"________________
void WeichenUndSchrankeInit(){
   Weiche[WEICHE1].servoPin = 2;
   Weiche[WEICHE2].servoPin = 3;
   Weiche[WEICHE3].servoPin = 3;
   Weiche[WEICHE4].servoPin = 2;
   Schranke.servoPin        = 4;
}

//___________________VOID SETUP______________
void setup()
{
  Serial.begin(115200);
  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_LED2_Rot, OUTPUT);
  digitalWrite(Pin_LED2_Rot, LOW);     // Initialisieren der LED auf Grundstellung, LED Abzweig ist aus

  pinMode(Pin_LED3_Gruen, OUTPUT);
  digitalWrite(Pin_LED3_Gruen, HIGH);    // 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_LED5_Gruen, OUTPUT);
  digitalWrite(Pin_LED5_Gruen, LOW);    // Initialisieren der LED auf Grundstellung, LED Gerade leuchtet

  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

  WeichenUndSchrankeInit();
}

//___________FUNKTION "Taste WEISS"___________________
void TasteWeiss(){
      if (!Taste[WEISS].ausloesen) return;
      Taste[WEISS].ausloesen = false;
      schalte(WEICHE1,ABBIEGEN);
      schalte(WEICHE2,GERADE);
      digitalWrite(Pin_LED3_Gruen, HIGH);
      digitalWrite(Pin_LED4_Rot, HIGH);
      digitalWrite(Pin_LED7_Rot, LOW);
      digitalWrite(Pin_LED8_Gruen, LOW);
};
//___________FUNKTION "Taste ORANGE"___________________
void TasteOrange(){
      if (!Taste[ORANGE].ausloesen) return;
      Taste[ORANGE].ausloesen = false;
      schalte(WEICHE1,GERADE);
      schalte(WEICHE2,GERADE);
      digitalWrite(Pin_LED3_Gruen, HIGH);
      digitalWrite(Pin_LED5_Gruen, LOW);
      digitalWrite(Pin_LED6_Gruen, HIGH);
      digitalWrite(Pin_LED8_Gruen, LOW);
};
//___________FUNKTION "Taste GRUEN"___________________
void TasteGruen(){
      if (!Taste[GRUEN].ausloesen) return;
      Taste[GRUEN].ausloesen = false;
      schalte(WEICHE2,ABBIEGEN);
      schalte(WEICHE1,GERADE);
      digitalWrite(Pin_LED2_Rot, LOW);
      digitalWrite(Pin_LED5_Gruen, LOW);
      digitalWrite(Pin_LED6_Gruen, HIGH);
      digitalWrite(Pin_LED9_Rot, HIGH);
};

//___________FUNKTION "Taste ROT"___________________
void TasteRot(){
    {     
      if (!Taste[ROT].ausloesen) return;
      Taste[ROT].ausloesen = false;
      Schranke.Schliessen = false;
      Schranke.Oeffnen    = false;
      if (Schranke.Zustand == Schranke.Oben) {Schranke.Zustand = Schranke.Unten;
                                      } else {Schranke.Zustand = Schranke.Oben;}
      SchrankeSchaltenOhneDelay(Schranke);
    }
};
//___________FUNKTION "Blinken"___________________
void Blinken(){
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= interval)
      {
        previousMillis = currentMillis;
        ledState = !ledState; 
        digitalWrite(LED_BUILTIN, ledState);
      };
  }

void SerialIn(){
   if (Serial.available()){
  char c = Serial.read();
     if (c == 'w') {Taste[WEISS].ausloesen = true;TasteWeiss();};
     if (c == 'o') {Taste[ORANGE].ausloesen = true;TasteOrange();};
     if (c == 'g') {Taste[GRUEN].ausloesen = true;TasteGruen();};
     if (c == 'r') {Taste[ROT].ausloesen = true;TasteRot();};
     if (c == 'O') {Schranke.Oeffnen = true;};
     if (c == 'S') {Schranke.Schliessen = true;};

   }
}


//_____________VOID LOOP______________________
void loop()
{
  SerialIn();
  if (TasteGedrueckt(WEISS))  TasteWeiss();  //Start Weiche 1 zu 3
  if (TasteGedrueckt(ORANGE)) TasteOrange(); //Start Weiche 1/2 und 3/4
  if (TasteGedrueckt(GRUEN))  TasteGruen(); //Start Weiche 3 zu 4
  if (TasteGedrueckt(ROT))    TasteRot(); // Schranke bewegen
  if (Schranke.Zustand != Schranke.Oben) Blinken();
  DetachNachZeit();                      // Falls Servos attached sind ..
  SchrankeLangsam();
}