/*
   Forum: https://forum.arduino.cc/t/code-funktioniert-nicht-richtig-warum/1327820
   Wokwi: https://wokwi.com/projects/416192201811842049

  Changes by ec2021
  2024-12-02

*/


#include <Servo.h>
const boolean doOverride = true;
const boolean doTry      = false;
const unsigned long SERIALDELAY = 2000;
const unsigned long SERVODELAY  =  300;
const int potPin = A0; // Potentiometer an Pin A0

Servo servo1; // Servo 1 an Pin 9
Servo servo2; // Servo 2 an Pin 10
enum Commands {IDLE, MOVING, POT_GOSTART, POT_GOTARGET, SER_GOSTART, SER_GOTARGET};
Commands servoCmd = IDLE;
unsigned long targetBySerialStarted;
unsigned long startMoving;

void setup() {
  servo1.attach(9);   // Servo 1 an Pin 9 anschließen
  servo2.attach(10);  // Servo 2 an Pin 10 anschließen
  Serial.begin(9600); // Serielle Kommunikation starten
  // Servos in der Startposition initialisieren
  moveToStartPosition();
  Serial.println("System bereit. Warte auf serielle Befehle.");
}

void loop() {
  checkPoti();
  checkSerial();
  controlServos();
}


void setServos(Commands cmd, boolean Override) {
  if (servoCmd == IDLE || Override) {
    servoCmd = cmd;
    if (cmd == MOVING) {
      startMoving = millis();
    }
    if (cmd == SER_GOSTART) {
      targetBySerialStarted = millis();
    }
  }
}

void checkPoti() {
  static boolean wasAbove990 = false;
  static boolean wasBelow700 = false;
  int potValue = analogRead(potPin); // Potentiometerwert lesen
  boolean below990 = (potValue < 990);
  boolean below700 = (potValue < 700);

  // Hier die "Verriegelungen" lösen
  if (below700 && !wasBelow700) {
    wasBelow700 = true;
    Serial.println("was below 700");
  }
  if (!below990 && !wasAbove990) {
    wasAbove990 = true;
    Serial.println("was above 990");
  }

  // Bei Rückkehr von unterem Bereich nach oben
  if (!below700 && wasBelow700) {
    setServos(POT_GOSTART, doTry);
    wasBelow700      = false;
  }

  // Bei Rückkehr aus dem oberen Bereich nach unten
  if (below990 && wasAbove990) {
    setServos(POT_GOTARGET, doTry);
    wasAbove990 = false;
  }
}

void checkSerial() {
  if (Serial.available() > 0) {
    char receivedChar = Serial.read(); // Lese ein Zeichen von Serial
    if (receivedChar == 'T') {
      setServos(SER_GOTARGET, doTry);
    }
  }
}

void controlServos() {
  switch (servoCmd) {
    case IDLE:
      // do Nothing
      break;
    case SER_GOSTART:
      if (millis() - targetBySerialStarted < SERIALDELAY) {
        break;
      }
    case POT_GOSTART:
      moveToStartPosition();
      break;
    case SER_GOTARGET:
      moveToTargetPosition();
      setServos(SER_GOSTART, doOverride);
      break;
    case POT_GOTARGET:
      moveToTargetPosition();
      break;
    case MOVING:
      if (millis() - startMoving > SERVODELAY) {
        setServos(IDLE, doOverride);
      }
      break;
    default:
      // Do nothing
      break;
  }
}


// Funktion zum Bewegen der Servos in die Zielposition
void moveToTargetPosition() {
  setServos(MOVING, doOverride);
  servo1.write(180); // Servo 1 auf Zielposition: 180 Grad
  servo2.write(0);   // Servo 2 auf Zielposition: 0 Grad
  Serial.println("Go to TARGET");
}

// Funktion zum Bewegen der Servos in die Startposition
void moveToStartPosition() {
  setServos(MOVING, doOverride);
  servo1.write(0);   // Servo 1 zurück auf Startposition: 0 Grad
  servo2.write(180); // Servo 2 zurück auf Startposition: 180 Grad
  Serial.println("Go to START");
}