#include <Servo.h>

// Pin definitions for Code1
const int trigPin = 9;
const int echoPin = 10;
const int greenLedPin1 = 13; // Grüne LED
const int redLedPin1 = 12; // Rote LED
const int switchPins[] = {2, 3, 4, 5}; // Schalter-Pins
const int servoPin1 = 6; // Servomotor Pin

// Pin definitions for Code2
const int xPin = A0; // Analog pin for X-axis
const int yPin = A1; // Analog pin for Y-axis
const int swPin = 26; // Digital pin for Joystick button (SW pin)
const int greenLedPin2 = 22; // Green LED
const int redLedPin2 = 24; // Red LED
const int servoPin2 = 28; // Servomotor Pin

// Pin definition for Signal reception
const int signalPin = 25; // Pin, an dem das Signal vom ersten Arduino empfangen wird


Servo myservo1;
Servo myservo2;

// Movement tracking variables for Code2
const int correctMovements[4] = {1, 2, 3, 4}; // Correct sequence: Up, Left, Right, Down
int movements[4] = {0, 0, 0, 0}; // {up, left, right, down}
int index = 0;
const int threshold = 512; // Joystick center threshold

void setup() {
  // Setup for Code1
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(greenLedPin1, OUTPUT);
  pinMode(redLedPin1, OUTPUT);
  for (int i = 0; i < 4; i++) {
    pinMode(switchPins[i], INPUT_PULLUP);
  }
  myservo1.attach(servoPin1);
  myservo1.write(0); // Ensure the servo is in the locked position

  // Setup for Code2
  pinMode(greenLedPin2, OUTPUT);
  pinMode(redLedPin2, OUTPUT);
  pinMode(swPin, INPUT_PULLUP);
  myservo2.attach(servoPin2);
  myservo2.write(0); // Ensure the servo is in the locked position

  // Setup for Signal reception
  pinMode(signalPin, INPUT);

  // Begin serial communication
  Serial.begin(9600);
}

void loop() {
  if (digitalRead(signalPin) == HIGH) { // Prüft, ob das Signal an signalPin HIGH ist
    Serial.println("Signal empfangen");
    runCode1();
  } else {
    // Debug output if no signal is detected
    Serial.println("Warten auf Signal");
    delay(1000); // Add delay to reduce spam in serial monitor
  }
}

void runCode1() {
  // Code1 logic
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = duration * 0.34 / 2;

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  if (distance <= 100) {
    int switchStates[4];
    for (int i = 0; i < 4; i++) {
      switchStates[i] = digitalRead(switchPins[i]);
    }

    if (switchStates[0] == HIGH && switchStates[1] == HIGH && switchStates[2] == LOW && switchStates[3] == HIGH) {
      digitalWrite(greenLedPin1, HIGH); // Turn on the green LED
      digitalWrite(redLedPin1, LOW); // Turn off the red LED
      myservo1.write(90); // Unlock the servo
      runCode2(); // Call Code2 when green LED is on
    } else {
      digitalWrite(greenLedPin1, LOW); // Turn off the green LED
      digitalWrite(redLedPin1, HIGH); // Turn on the red LED
      myservo1.write(0); // Lock the servo
    }
  } else {
    digitalWrite(greenLedPin1, LOW); // Ensure the green LED is off if the distance is greater than 100 mm
    digitalWrite(redLedPin1, LOW); // Ensure the red LED is off if the distance is greater than 100 mm
    myservo1.write(0); // Ensure the servo is in the locked position
  }

  delay(100);


}

void runCode2() {
  Serial.println("runCode2 gestartet");

  // Variable zur Überprüfung, ob der Joystick in neutraler Position ist
  bool inNeutralPosition = true;

  while (true) {
    int xValue = analogRead(xPin);
    int yValue = analogRead(yPin);
    int swState = digitalRead(swPin);

    // Debugging: Werte des Joysticks anzeigen
    Serial.print("xValue: ");
    Serial.print(xValue);
    Serial.print(" | yValue: ");
    Serial.print(yValue);
    Serial.print(" | swState: ");
    Serial.println(swState);

    // Reset combination if joystick button is pressed
    if (swState == LOW) {
      Serial.println("Joystick-Knopf gedrückt, Sequenz wird zurückgesetzt.");
      index = 0; // Reset index for new input
      memset(movements, 0, sizeof(movements)); // Reset movements array
      digitalWrite(greenLedPin2, LOW); // Turn off the green LED
      digitalWrite(redLedPin2, LOW); // Turn off the red LED
      myservo2.write(0); // Ensure the servo is in the locked position
      inNeutralPosition = true; // Setzt die Neutralposition zurück
      delay(200); // Debounce delay
      break; // Exit Code2 loop and return to Code1
    } else {
      // Prüfen, ob der Joystick in Neutralposition ist (zum Erkennen neuer Bewegungen)
      bool isNeutral = (xValue > threshold - 100 && xValue < threshold + 100 &&
                        yValue > threshold - 100 && yValue < threshold + 100);

      // Nur dann eine neue Bewegung erkennen, wenn der Joystick zuvor in Neutralposition war
      if (inNeutralPosition && !isNeutral) {
        if (yValue > threshold + 300) {
          movements[index] = 1; // Up
          Serial.println("Bewegung erkannt: Oben");
          inNeutralPosition = false;
        } else if (xValue > threshold + 300) {
          movements[index] = 3; // Right
          Serial.println("Bewegung erkannt: Rechts");
          inNeutralPosition = false;
        } else if (yValue < threshold - 300) {
          movements[index] = 4; // Down
          Serial.println("Bewegung erkannt: Unten");
          inNeutralPosition = false;
        } else if (xValue < threshold - 300) {
          movements[index] = 2; // Left
          Serial.println("Bewegung erkannt: Links");
          inNeutralPosition = false;
        }

        // Prüfen, ob die Bewegung der erwarteten Sequenz entspricht
        if (movements[index] != correctMovements[index]) {
          // Falsche Bewegung erkannt
          Serial.println("Falsche Bewegung erkannt, rote LED leuchtet.");
          digitalWrite(greenLedPin2, LOW); // Turn off the green LED
          digitalWrite(redLedPin2, HIGH); // Turn on the red LED
          myservo2.write(0); // Lock the servo
          
          // Verzögerung für die rote LED
          delay(500); // Rote LED bleibt für 500 ms an
          digitalWrite(redLedPin2, LOW); // Turn off the red LED after delay

          // Reset der Sequenz für neue Eingabe
          index = 0; // Reset index for new input
          memset(movements, 0, sizeof(movements)); // Reset movements array
          inNeutralPosition = true; // Neutralposition für neue Eingabe zurücksetzen
        } else {
          // Bewegung war korrekt, gehe zum nächsten Schritt in der Sequenz
          index++;
          delay(200); // Verzögerung zur Entprellung

          // Prüfen, ob die gesamte Sequenz korrekt eingegeben wurde
          if (index == 4) {
            // Korrekte Sequenz eingegeben
            Serial.println("Korrekte Sequenz erkannt! Grüne LED leuchtet und Servo wird entsperrt.");
            digitalWrite(greenLedPin2, HIGH); // Turn on the green LED
            digitalWrite(redLedPin2, LOW); // Ensure the red LED is off
            myservo2.write(90); // Unlock the servo
            delay(500); // Kurze Verzögerung zur Anzeige des Erfolgszustands

            // Zurücksetzen nach erfolgreicher Sequenz
            index = 0; // Reset index for new input
            memset(movements, 0, sizeof(movements)); // Reset movements array
            break; // Exit Code2 loop and return to Code1
          }
        }
      } else if (isNeutral) {
        // Setzt inNeutralPosition auf true, wenn der Joystick in der Mitte ist
        inNeutralPosition = true;
      }
    }

    delay(100);
  }
}