#include <Servo.h>
#include <Adafruit_ST7735.h>
#include <arduino-timer.h>
#include <Bounce2.h>
#include <Encoder.h>
#include <VL53L0X.h>
#include <Wire.h>

//Abstandssensor
VL53L0X sensor;

//Bildschirm
#define CS 7
#define DC 6
#define Reset 10

// Verwendete Farben
#define BLACK 0x0000
#define WHITE 0xFFFF
Adafruit_ST7735 tft = Adafruit_ST7735(CS, DC, Reset);

//Motor
Servo myservo; // create servo object to control a servo

// Timer in milli Sekunden
Timer<10, millis> timer; // 10 concurrent tasks, using millis as resolution
int8_t pos = 0; // variable to store the servo position
//Knöpfe
#define buttonPin_R 5
#define buttonPin_start 8
#define buttonPin_L 4
//Encoderknopf
#define buttonPin_E 12
//Auf/ZU-Knopf
#define buttonPin_Auf 14
Bounce2::Button button_R = Bounce2::Button();
Bounce2::Button button_L = Bounce2::Button();
Bounce2::Button button_start = Bounce2::Button();
Bounce2::Button button_E = Bounce2::Button();
Bounce2::Button button_Auf = Bounce2::Button();
// Anfangszustände
long zeit = 0; //eingestellte Zeit
int8_t hunderter = 0; //Zeit Ausgabe x00,00s
int8_t zehner = 0; //Zeit Ausgabe 0x0,00s
int8_t einer = 0; //Zeit Ausgabe 00x,00s
int8_t decimal = 0; //Zeit Ausgabe 000,x0s
int8_t hilfszahl = 0; //Zeit Ausgabe 000,0xs
String h = "0"; //Zeit Ausgabe
String shutter = "R [X] L[X]"; //Shutterzustand Ausgabe
int8_t motor_state = 1; //Anfangszustand (Shutter zu)
int8_t motorspeicher; // Zustand vor dem öffnen
int8_t encoder_state = 1; //state 1=> 50 ms pro Drehung || state 2=> 1 s pro Drehung
int abstand; //Gemessener Abstand
//Abstandsausgabe
float Val = 0;
float abstandaus;
bool ausgabe = 0;
// Motorpositionen für den Shutter verändern
short zu = 62;
short rechts = zu + 29;
short links = zu - 29;
short auf = zu + 60;
// Encoder
#define CLK 2 // Definition der Pins. CLK an D2, DT an D3.
#define DT 3
long altePosition = 0; // Definition der "alten" Position
Encoder meinEncoder(DT, CLK);


void setup() {
  Serial.begin(9600);
  Wire.begin();
  // Abstandssensor initialisieren
  if (!sensor.init())
  {
    String abstand = "Fehler"; //Sollte der Abstandssensor nicht laden wird ein Fehler auf dem Bildschirm sngezeigt
  }
  sensor.startContinuous();
  //display Eigenschaften
  tft.initR(INITR_BLACKTAB);
  tft.setRotation(1);
  tft.fillScreen(ST7735_BLACK);
  //Knöpfe incl. Encoder Knopf
  // Knopf rechts
  button_R.attach( buttonPin_R, INPUT_PULLUP );
  // DEBOUNCE INTERVAL IN MILLISECONDS
  button_R.interval(5);
  // INDICATE THAT THE LOW STATE CORRESPONDS TO PHYSICALLY PRESSING THE BUTTON
  button_R.setPressedState(LOW);
  // Knopf links
  button_L.attach( buttonPin_L, INPUT_PULLUP );
  button_L.interval(5);
  button_L.setPressedState(LOW);
  // Knopf start
  button_start.attach( buttonPin_start, INPUT_PULLUP );
  button_start.interval(5);
  button_start.setPressedState(LOW);
  // Knopf Auf
  button_Auf.attach(buttonPin_Auf, INPUT_PULLUP );
  button_Auf.interval(5);
  button_Auf.setPressedState(LOW);
  // Knopf encoder
  button_E.attach( buttonPin_E, INPUT_PULLUP );
  button_E.interval(5);
  button_E.setPressedState(LOW);
  // Motor an pin 9 und in Startposition
  myservo.attach(9);
  myservo.write(zu);
}

void loop() {
  // Abstandsmessung
  abstand = sensor.readRangeContinuousMillimeters();
  Val = 0.8 * Val + (1 - 0.8) * abstand; // Gleitender Mittelwert der Messung
  abstandaus = 296.5 - 125 - Val; // Gesamtlänge-Schlittenlänge-Messwert= Ausgegebener Abstand
  //Bildschirmausgaben
  //Zeiteinstellung
  tft.setCursor(5, 5);
  tft.setTextColor(WHITE);
  tft.setTextSize(1);
  tft.println("Beleuchtungszeit");
  if (encoder_state == 1) {
    tft.setCursor(105, 5);
    tft.setTextColor(WHITE, BLACK);
    tft.setTextSize(1);
    tft.println("[0,05s] ");
  }
  else if (encoder_state == 2) {
    tft.setCursor(105, 5);
    tft.setTextColor(WHITE, BLACK);
    tft.setTextSize(1);
    tft.println("[1s] ");
  }
  h = String(hunderter) + String(zehner) + String( einer) + "," +
      String(decimal) + String(hilfszahl) + "s ";
  tft.setCursor(50, 25);
  tft.setTextColor(WHITE, BLACK);
  tft.setTextSize(2);
  tft.print(h);
  //Shutterausgabe
  tft.setCursor(5, 45);
  tft.setTextColor(WHITE);
  tft.setTextSize(1);
  tft.println("Shutter bedeckt:");
  //Abstandausgabe
  tft.setCursor(5, 85);
  tft.setTextColor(WHITE);
  tft.setTextSize(1);
  tft.println("Abstand:");
  tft.setCursor(5, 105);
  tft.setTextColor(WHITE, BLACK);
  tft.setTextSize(2);
  tft.println(String(int(abstandaus)) + " mm ");
  //Shutterausgabe 2
  if (ausgabe == 1) {
    shutter = "An ";
    tft.setCursor(20, 60);
    tft.setTextColor(WHITE, BLACK);
    tft.setTextSize(2);
    tft.println(shutter);
  }
  else if (ausgabe == 0) {
    if (motor_state == 1) {
      shutter = "L[X] R[X]";
      tft.setCursor(20, 60);
      tft.setTextColor(WHITE, BLACK);
      tft.setTextSize(2);
      tft.println(shutter);
    }
    else if (motor_state == 2) {
      shutter = "L[ ] R[X]";
      tft.setCursor(20, 60);
      tft.setTextColor(WHITE, BLACK);
      tft.setTextSize(2);
      tft.println(shutter);
    }
    else if (motor_state == 3) {
      shutter = "L[X] R[ ]";
      tft.setCursor(20, 60);
      tft.setTextColor(WHITE, BLACK);
      tft.setTextSize(2);
      tft.println(shutter);
    }
    else if (motor_state == 4) {
      shutter = "L[ ] R[ ]";
      tft.setCursor(20, 60);
      tft.setTextColor(WHITE, BLACK);
      tft.setTextSize(2);
      tft.println(shutter);
    }
    else if (motor_state == 5) {
      shutter = "offen ";
      tft.setCursor(20, 60);
      tft.setTextColor(WHITE, BLACK);
      tft.setTextSize(2);
      tft.println(shutter);
    }
  }
  button_R.update();
  button_L.update();
  button_start.update();
  button_E.update();
  button_Auf.update();
  // Encoder/ Zeit ist Bestrahlungsdauer; rest ist Ausgabe
  long neuePosition = meinEncoder.read(); // Die "neue" Position des Encoders wird definiert. Dabei wird die aktuelle Position des Encoders über die Variable.Befehl() ausgelesen.
  if (neuePosition != altePosition && ausgabe == 0) // Sollte die neue Position ungleich der alten (0) sein (und nur dann!!)...
  {
    delay(50);
    if (encoder_state == 1) {
      if (neuePosition < altePosition) {
        zeit = zeit - 50;
        hilfszahl = hilfszahl - 5;
        if (zeit < 0) {
          zeit = 0;
        }
        if (hilfszahl < 0 && decimal == 0 && einer == 0 && zehner == 0 &&
            hunderter == 0) {
          hilfszahl = 0;
        }
        if (hilfszahl < 0) {
          hilfszahl = 5;
          decimal--;
          if (decimal < 0 ) {
            decimal = 9;
            einer--;
            if (einer < 0) {
              einer = 9;
              zehner--;
              if (zehner < 0) {
                zehner = 9;
                hunderter--;
              }
            }
          }
        }
      }
      else if (neuePosition > altePosition) {
        zeit = zeit + 50;
        hilfszahl = hilfszahl + 5;
        if (hilfszahl > 5) {
          hilfszahl = 0;
          decimal = decimal + 1;
          if (decimal > 9) {
            decimal = 0;
            einer++;
            if (einer > 9) {
              einer = 0;
              zehner++;
            }
            if (zehner > 9) {
              zehner = 0;
              hunderter++;
            }
          }
        }
      }
      altePosition = neuePosition;
    }
    // Sekundenausgabe
    if (encoder_state == 2) {
      if (neuePosition < altePosition) {
        zeit = zeit - 1000;
        einer = einer - 1;
        if (zeit < 0) {
          zeit = 0;
        }
        if (zeit == 0) {
          hilfszahl = 0;
          decimal = 0;
        }
        if (einer < 0 && decimal == 0 && hilfszahl == 0 && zehner == 0 &&
            hunderter == 0) {
          einer = 0;
        }
        if (einer < 0) {
          einer = 9;
          zehner--;
        }
        if (zehner < 0) {
          zehner = 9;
          hunderter--;
        }
      }
      else if (neuePosition > altePosition) {
        zeit = zeit + 1000;
        einer = einer + 1;
        if (einer > 9) {
          einer = 0;
          zehner++;
          if (zehner > 9) {
            zehner = 0;
            hunderter++;
          }
        }
      }
      altePosition = neuePosition;
    }
  }
  // Wechsel Zwischen 0,05 s und 1 s Pro encoderdrehung
  switch (encoder_state) {
    case 1:
      if (button_E.pressed()) {
        encoder_state = 2;
      }
      break;
    case 2:
      if (button_E.pressed()) {
        encoder_state = 1;
      }
      break;
  }
  // Starten des Timers
  timer.tick();
  if (ausgabe == 0) { // Falls die Bestrahlung läuft ist keine Einstellung möglich
    // Auswahl der verschiedenen Motor/Shutter Positionen
    switch (motor_state) {
      case 1:
        if (button_R.pressed()) {
          motor_state = 3;
        } else if (button_L.pressed()) {
          motor_state = 2;
        } else if (button_Auf.pressed()) {
          motorspeicher = motor_state;
          motor_state = 5;
        }
        break;
      case 2:
        if (button_R.pressed()) {
          motor_state = 4;
        } else if (button_L.pressed()) {
          motor_state = 1;
        } else if (button_start.pressed() && zeit > 0) {
          motor_start(motor_state, zeit);
        } else if (button_Auf.pressed()) {
          motorspeicher = motor_state;
          motor_state = 5;
        }
        break;
      case 3:
        if (button_R.pressed()) {
          motor_state = 1;
        } else if (button_L.pressed()) {
          motor_state = 4;
        } else if (button_start.pressed() && zeit > 0) {
          motor_start(motor_state, zeit);
        } else if (button_Auf.pressed()) {
          motorspeicher = motor_state;
          motor_state = 5;
        }
        break;
      case 4:
        if (button_R.pressed()) {
          motor_state = 2;
        } else if (button_L.pressed()) {
          motor_state = 3;
        } else if (button_start.pressed() && zeit > 0) {
          motor_start(motor_state, zeit);
        } else if (button_Auf.pressed()) {
          motorspeicher = motor_state;
          motor_state = 5;
        }
        break;
      case 5:
        myservo.write(auf);
        if (button_Auf.pressed()) {
          myservo.write(zu);
          motor_state = motorspeicher;
        }
        break;
    }
  }
}

// Funktion zum Schließen des Shutters
bool function_to_call(void *argument ) {
  myservo.write(zu);
  ausgabe = 0; //erlaubt neue Eingaben
  motor_state = 1;
}

// Funktion öffnet den Shutter. Ruft die Funktion zum Schließen nach vergehen der Zeit auf
void motor_start(int motor_state, float zeit) {
  ausgabe = 1;
  if (motor_state == 1) {
    pos = zu; // tell servo to go to position in variable 'pos'
  } else if (motor_state == 2) {
    pos = rechts;
  }
  else if (motor_state == 3) {
    pos = links;
  }
  else if (motor_state == 4) {
    pos = auf;
  }
  myservo.write(pos);
  timer.in(zeit, function_to_call);
}