#include <SPI.h>         // Arduino SPI library
#include <ESP32Servo.h>  // Servo library
#include <TFT_eSPI.h>    // Hardware-specific library

// Definitionen
#define SPEED_MIN (1000)      // Set the Minimum Speed in microseconds
#define SPEED_MAX (2000)      // Set the Minimum Speed in microseconds
#define Drehzahl_max (25000)  // max. Drehzahl für Anzeige
//
#include "Free_Fonts.h"  // freie Fonts für Display
Servo ESC;               // Objekt ESC als Servo definiert

TFT_eSPI tft = TFT_eSPI();  // Invoke custom library

int hintergrund[7] = { 0x001F, 0xF800, 0x07E0, 0xF81F, 0xFFE0, 0x03E0, 0x7BE0 }; //keine Ahnung wofür, trau mich nicht das zu löschen

uint16_t Farbe_E;  // Var. Statusfarbe EStop
uint16_t Farbe_X;  // Var. Statusfarbe ES_X
uint16_t Farbe_Y;  // Var. Statusfarbe ES_Y
uint16_t Farbe_Z;  // Var. Statusfarbe ES_Z
uint16_t Farbe_T;  // Var. Statusfarbe Taster
uint16_t Farbe_W;  // Var. Statusfarbe Wkz-Laenge
uint16_t Farbe_S;  // Var. Statusfarbe Steuerung

unsigned long intervall = 2000;   // Länge der Anzeige Status
unsigned long intervall1 = 4000;  // Länge der Pause für Bereitschaft Fahrtenregler
unsigned long neuMillis = 0;      // Speicher für akt. Zeit
unsigned long altMillis = 0;      // Speicher für alte Zeit
unsigned long neuMillis1 = 0;     // Speicher für akt. Zeit
unsigned long altMillis1 = 0;     // Speicher für alte Zeit
int Status_Motor = 0;             // Var. für Statuszustand Spindel_in

String Status_Stg;           //Status Enable
String Status_ES_X;          //Status Endschalter X
String Status_ES_Y;          //Status Endschalter Y
String Status_ES_Z;          //Status Endschalter Z
String Status_3D;            //Status 3D-Taster
String Status_Wkz_Laenge;    //Status WKZ-Länge
String Status_Spindel;       //Status Spindel
String Status_EStop;         //Status E-Stop
String Achse;                //Var. für Achse
String DZ;                   //Var. für Drehzahl
unsigned long Farbe;         // Farbe= Schriftfarbe
int Dz_in = A0;              // Eingang Drehzahl-Spindel 0-10V
int Spindel_in = 12;         // Eingang Spindel_an Eingang
int Es_X_in = 39;            // Eingang Endschalter X
int Es_Y_in = 34;            // Eingang Endschalter Y
int Es_Z_in = 35;            // Eingang Endschalter Z
int Taster_in = 26;          // Eingang 3D Taster
int Wkz_in = 27;             // Eingang WKZ-Länge
int EStop_in = 32;           // Eingang Notaus
int Ena_in = 33;             // Eingang Enable
int Reserve_in = 14;         // Eingang Reserve Relais
int Spindel_out = 16;        // Ausgang Spindel_an
int Reserve_Relais_out = 4;  // Ausgang Reserve Relais
int Reserve_out = 4;         // Ausgang Reserve
int ESC_out = 13;            // Ausgang PWM ESC
int TFT_BL = 15;             // Ausgang Beleuchtung TFT
unsigned int Drehzahl = 0;   // Var. Drehzahl (Drehzahl 0)
int oESC;                    // Variable for the speed sent to the ESC


void setup() {
  pinMode(Es_X_in, INPUT);                 // PIN Es_X_in Eingang Pullup
  pinMode(Es_Y_in, INPUT);                 // PIN Es_Y_in Eingang Pullup
  pinMode(Es_Z_in, INPUT);                 // PIN Es_Z_in Eingang Pullup
  pinMode(Spindel_in, INPUT);              // PIN Spindel_in Eingang Pullup
  pinMode(Ena_in, INPUT);                  // Pin Enable_in Eingang Pullup
  pinMode(Taster_in, INPUT);               // Pin 3D-Taster Eingang Pullup
  pinMode(Wkz_in, INPUT);                  // Pin WKZ-Laenge-Taster Eingang Pullup
  pinMode(EStop_in, INPUT);                // Pin EStop Eingang Pullup
  pinMode(ESC_out, OUTPUT);                // PIN ESC_out Ausgang
  pinMode(TFT_BL, OUTPUT);                 // PIN TFT_BL Beleuchtung TFT Display
  pinMode(Spindel_out, OUTPUT);            // PIN Spindel Relais out (Relaisausgang)
  pinMode(Reserve_Relais_out, OUTPUT);     // PIN Reserve Relais out (Relaisausgang)
  pinMode(Reserve_out, OUTPUT);            // PIN Reserve out
  digitalWrite(Spindel_out, HIGH);         // Spindel_out auf High
  digitalWrite(Reserve_Relais_out, HIGH);  // Reserve_Relais auf High
  Serial.begin(9600);

  //Startbildschirm (Übernahme aus Vorschlag aus dem Forum)
  digitalWrite(TFT_BL, HIGH);
  tft.init();
  tft.setRotation(3);
  tft.fillScreen(TFT_BLACK);
  tft.setTextColor(TFT_RED, TFT_BLACK);
  tft.setFreeFont(FF24);
  tft.drawString("Systemstart!", 10, 2);
  tft.setFreeFont(FF19);
  tft.setTextColor(TFT_GREEN, TFT_BLACK);
  tft.drawString("Steuerung wird", 15, 60);
  tft.drawString("aktiviert!", 15, 100);
  tft.setTextColor(TFT_ORANGE, TFT_BLACK);
  for (byte i = 0; i <= 4; i++) {
    tft.setTextColor(TFT_ORANGE, TFT_BLACK);
    tft.drawString("Bitte warten!", 60, 160);
    delay(1000);
    tft.fillRect(0, 159, 318, 30, TFT_BLACK);
    delay(500);
  }
  //

  tft.fillScreen(TFT_BLACK);                 // Display schwarz
  ESC.attach(ESC_out, SPEED_MIN, SPEED_MAX); // ESC scharf schalten
  Serial.println();
}


//Hauptschleife
void loop() {
  Status();       // Fehler und Aktion wird angezeigt
  Anzeige();      // Statusanzeige wird aktualisiert
  Motor();        // Motor ein aus schalten
  Umdrehungen();  // Aktualisierung Drehzahl
}

//Motor an/aus
void Motor() {
  if (digitalRead(Spindel_in) == LOW && Status_Motor == 0) {  // Wenn Spindeleingang LOW und Status_Motor 0
    digitalWrite(Spindel_out, LOW);                           // ESC 5V einschalten
    ESC.write(SPEED_MIN);                                     // Drehzahl Motor 0, Anfangssignal für ESC
    Status_Motor = 1;                                         // Status_Motor wird 1 gesetzt
    altMillis1 = millis();                                    // akt. Zeit in altMillis1 überommen
    Serial.println("Motor einschalten");
    Serial.print("Status Motor=");
    Serial.println(Status_Motor);
  }

  if (digitalRead(Spindel_in) == HIGH && Status_Motor == 2) {            // wenn Spindeleingang HIGH
    for (oESC = Drehzahl; oESC >= 0; oESC -= 10) {  // von Drehzahl zu SPEED_MIN
      ESC.write(oESC);                              // tell ESC to go to the oESC speed value
      delay(10);                                    // waits 10ms for the ESC to reach speed
    }
    ESC.write(SPEED_MIN);
    digitalWrite(Spindel_out, HIGH);  // ESC 5V ausschalten
    Status_Motor = 0;                 // Status_Motor 0
    Serial.println("Motor ausgeschaltet");
    Serial.print("Status Motor=");
    Serial.println(Status_Motor);
  }
}

//Drehzahlreglung
void Umdrehungen() {
  Drehzahl = map(analogRead(Dz_in), 0, 4095, -7.2, 180);                      // umwandeln 0 - 1V in 0 - 180GRAD
  if (Drehzahl > 180) Drehzahl= 0;                                            // "unsigned"-> neg. Zahlen sollen 0 dargestellt werden
  unsigned int Drehz = map(analogRead(Dz_in), 0, 4095, -1000, Drehzahl_max);  // umwandeln 0 - 1V in 0 - 25000 U/min
  if (Drehz > Drehzahl_max) Drehz = 0;                                        // "unsigned"-> neg. Zahlen sollen 0 dargestellt werden
  Drehz = Drehz / 1000;                                                       // Drehzahl / 1000; Ergebnis -> Stellen hinter dem Komma weg
  Drehz = Drehz * 1000;                                                       // Drehzahl * 1000; Ergebnis letzte 3 Stellen immer 0
  DZ = Drehz;                                                                 // DZ für Anzeige als string übernehmen
  Serial.print("Drehzahl=");
  Serial.println(Drehz);
  if (Status_Motor == 1) {                            // wenn Spindel_in LOW
    neuMillis1 = millis();                            // akt. Zeit übernehmen
    if ((neuMillis1 - altMillis1) >= intervall1) {    // wenn x mS vergangen
      altMillis1 = neuMillis1;                        // akt. Zeit altMillis
      for (oESC = 0; oESC <= Drehzahl; oESC += 10) {  // von SPEED_MIN bis Drehzahl
        ESC.write(oESC);                              // Drehzahl an ESC übergeben
        delay(10);                                    // waits 10ms for the ESC to reach speed
      }
      Status_Motor = 2;  // Status Motor wird 2
      Serial.println("Drehzahl übernommen");
      Serial.print("Status Motor=");
      Serial.println(Status_Motor);
    }
  }
  if (Status_Motor == 2) {  // Wenn Status_Motor = 2
    ESC.write(Drehzahl);    //akt. Drehzahl einstellen
    Serial.println("Drehzahl aktualisiert");
    Serial.print("Status Motor=");
    Serial.println(Status_Motor);
  }
}

//Status Eingänge, Formatierung für Anzeige, Aufruf Aktionsanzeige
void Status() {
  neuMillis = millis();                        // akt. Zeit übernehmen
  if ((neuMillis - altMillis) >= intervall) {  // wenn  x ms abgelaugfen
    altMillis = neuMillis;                     // akt. Zeit in altMillis
    Serial.println("Abfrage Status");
    if (digitalRead(Ena_in) == HIGH) {  //Status Eingang Enable
      Farbe_S = TFT_RED;
      Status_Stg = "Aus";
      Steuerung_ein();
    } else {
      Status_Stg = "Ein";
      Farbe_S = TFT_GREEN;
    }
    if (digitalRead(Es_X_in) == LOW) {  //Status Eingang Endschalter X
      Farbe_X = TFT_RED;
      Status_ES_X = "Fehler";
      Achse = "   X-Achse";
      Freifahren();
    } else {
      Status_ES_X = "o.k.";
      Farbe_X = TFT_GREEN;
    }
    if (digitalRead(Es_Y_in) == HIGH) {  //Status Eingang Endschalter Y
      Farbe_Y = TFT_RED;
      Status_ES_Y = "Fehler";
      Achse = "   Y-Achse";
      Freifahren();
    } else {
      Status_ES_Y = "o.k";
      Farbe_Y = TFT_GREEN;
    }
    if (digitalRead(Es_Z_in) == HIGH) {  //Status Eingang Endschalter Z
      Farbe_Z = TFT_RED;
      Status_ES_Z = "Fehler";
      Achse = "   Z-Achse";
      Freifahren();
    } else {
      Status_ES_Z = "o.k.";
      Farbe_Z = TFT_GREEN;
    }
    if (digitalRead(Taster_in) == HIGH) {  //Status Eingang 3D Taster
      Farbe_T = TFT_RED;
      Status_3D = "Fehler";
      Achse = "   Taster";
      Freifahren();
    } else {
      Status_3D = "o.k.";
      Farbe_T = TFT_GREEN;
    }
    if (digitalRead(Wkz_in) == LOW) {  //Status Eingang Werkzeuglänge
      Farbe_W = TFT_RED;
      Status_Wkz_Laenge = "Fehler";
      Achse = "   Werkzeug";
      Freifahren();
    } else {
      Status_Wkz_Laenge = "o.k.";
      Farbe_W = TFT_GREEN;
    }
    if (digitalRead(EStop_in) == HIGH) {  //Status Eingang E-Stop
      Farbe_E = TFT_RED;
      Status_EStop = "Fehler";
      EStop_ein();
    } else {
      Status_EStop = "o.k.";
      Farbe_E = TFT_GREEN;
    }
  }
}

// Status Eingänge anzeigen
void Anzeige() {
  Serial.println("Anzeige");
  tft.setTextColor(TFT_RED);
  tft.drawString("Status", 120, 0, 4);
  tft.setTextColor(TFT_BLUE);
  tft.drawString("Steuerung", 10, 26, 4);
  tft.drawString("Endsch. X", 10, 52, 4);
  tft.drawString("Endsch. Y", 10, 78, 4);
  tft.drawString("Endsch. Z", 10, 104, 4);
  tft.drawString("3D-Taster", 10, 130, 4);
  tft.drawString("WKZ-Laenge", 10, 156, 4);
  tft.drawString("E-Stop", 10, 182, 4);
  tft.drawString("Spindel", 10, 208, 4);
  tft.drawString("U/min", 240, 208, 4);

  tft.setTextColor(Farbe_S, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_Stg, 200, 26, 4);
  tft.setTextColor(Farbe_X, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_ES_X, 200, 52, 4);
  tft.setTextColor(Farbe_Y, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_ES_Y, 200, 78, 4);
  tft.setTextColor(Farbe_Z, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_ES_Z, 200, 104, 4);
  tft.setTextColor(Farbe_T, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_3D, 200, 130, 4);
  tft.setTextColor(Farbe_W, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_Wkz_Laenge, 200, 156, 4);
  tft.setTextColor(Farbe_E, TFT_BLACK, true);
  tft.setTextPadding(tft.textWidth("55555"));
  tft.drawString(Status_EStop, 200, 182, 4);
  if (digitalRead(Spindel_in) == HIGH) {
    tft.setTextColor(TFT_RED);
    tft.setTextPadding(tft.textWidth("55555"));
    tft.drawString("Aus", 150, 208, 4);
  } else {
    tft.setTextColor(TFT_ORANGE, TFT_BLACK, true);
    tft.setTextPadding(tft.textWidth("55555"));
    tft.drawString(DZ, 140, 208, 4);
    tft.setTextPadding(0);
  }
}
//Aktion Achsen freifahren
void Freifahren() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(0, 0);
  tft.setFreeFont(FF19);
  tft.println("");
  tft.setTextColor(TFT_BLUE);
  tft.println("     Aktion");
  tft.println("  erforderlich!");
  tft.println("");
  tft.setTextColor(TFT_RED);
  tft.println(Achse);
  tft.setTextColor(TFT_GREEN);
  tft.println("   freifahren");
  delay(intervall);
  tft.fillScreen(TFT_BLACK);
  altMillis1 = altMillis1 + intervall;
  altMillis = altMillis + intervall;
}

//Aktion bei Steuerung aus
void Steuerung_ein() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(0, 0);
  tft.setFreeFont(FF19);
  tft.println("");
  tft.setTextColor(TFT_BLUE);
  tft.println("    Aktion");
  tft.println("  erforderlich!");
  tft.println("");
  tft.setTextColor(TFT_GREEN);
  tft.println("   Steuerung");
  tft.println("  einschalten!");
  delay(intervall);
  tft.fillScreen(TFT_BLACK);
  altMillis1 = altMillis1 + intervall;
  altMillis = altMillis + intervall;
}
//Aktion bei E-Stop ausgelöst
void EStop_ein() {
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(0, 0);
  tft.setFreeFont(FF19);
  tft.println("");
  tft.setTextColor(TFT_BLUE);
  tft.println("    Aktion");
  tft.println("  erforderlich!");
  tft.println("");
  tft.setTextColor(TFT_GREEN);
  tft.println("  Not Aus");
  tft.println("zuruecksetzen!");
  delay(intervall);
  tft.fillScreen(TFT_BLACK);
  altMillis1 = altMillis1 + intervall;
  altMillis = altMillis + intervall;
}