#include <Wire.h>
#include <MPU6050_tockn.h>

MPU6050 mpu6050(Wire);

// Blinker Intervall (in Millisekunden)
const unsigned long blinkInterval = 500;

// Pin-Definitionen-Blinker
const int leftLED1 = 53; //Blinker Links Vorne
const int leftLED2 = 38; //Blinker Links Hinten
const int rightLED1 = 52; //Blinker Rechts Vorne
const int rightLED2 = 39; //Blinker Rechts Hinten

// Pin-Definitionen-Hupe
const int greenLED = 22; //Blinker Links Vorne
const int yellowLED = 22; //Blinker Links Vorne
const int redLED = 22; //Blinker Links Vorne

// Pin-Definitionen-Ultraschall
const int triggerPin = 22; //Trigger
const int echoPin = 22; //Echo

//Pin-Definitionen-Abblendlicht
const int leftRGBLedRed = 44; //Links Vorne RGB
const int leftRGBLedGreen = 36; //Links Vorne RGB
const int leftRGBLedBlue = 48; //Links Vorne RGB
const int rightRGBLedRed = 42; //Rechts Vorne RGB
const int rightRGBLedGreen = 46; //Rechts Vorne RGB
const int rightRGBLedBlue = 50; //Rechts Vorne RGB
const int reverseLightPin1 = 45; //Rechts Hinten
const int reverseLightPin2 = 49; //Links Hinten

//Pin-Definitionen-Bremslicht
const int brakeLightPin1 = 51;
const int brakeLightPin2 = 43;

// Speicherformate
bool leftState = LOW;
bool rightState = LOW;
unsigned long previousMillis = 0;
bool previousLeftState = LOW;
bool previousRightState = LOW;
bool hazardState = LOW;
bool previousHazardState = LOW;
unsigned long previousDistanceMillis = 0;
unsigned long distanceInterval = 500;
unsigned long duration, distance;

void setup() {
  // Initialisierung der seriellen Kommunikation
  Serial.begin(9600);

  // Initialisierung des MPU-6050 mit Kalibierung
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true, true, true);


  // Initialisierung der Blinker Pins als Ausgänge
  pinMode(leftLED1, OUTPUT);
  pinMode(leftLED2, OUTPUT);
  pinMode(rightLED1, OUTPUT);
  pinMode(rightLED2, OUTPUT);

// Initialisierung der Hupe Pins als Ausgänge
  pinMode(greenLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(redLED, OUTPUT);

   // Initialisierung der Pins Des Ultraschall Modul als Ausgänge
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialisierung der Pins für das Ablendlicht als Ausgänge
  pinMode(leftRGBLedRed, OUTPUT);
  pinMode(leftRGBLedGreen, OUTPUT);
  pinMode(leftRGBLedBlue, OUTPUT);
  pinMode(rightRGBLedRed, OUTPUT);
  pinMode(rightRGBLedGreen, OUTPUT);
  pinMode(rightRGBLedBlue, OUTPUT);
   pinMode(reverseLightPin1, OUTPUT);
  pinMode(reverseLightPin2, OUTPUT);

  // Initialisierung der Pins für das Bremslicht als Ausgänge
  pinMode(brakeLightPin1, OUTPUT);
  pinMode(brakeLightPin2, OUTPUT);
 }
 
void loop() {

  unsigned long currentMillis = millis();

  // Blinkersteuerung über Tastatur
  if (Serial.available() > 0) {
    char key = Serial.read();

    if (key == '1') {
      if (!hazardState && !rightState) {
        toggleLeftBlinker();
      } else if (hazardState) {
        Serial.println("Fehler: Der linke Blinker kann nicht eingeschaltet werden, da der Warnblinker aktiviert ist.");
      } else if (rightState) {
        Serial.println("Fehler: Der linke Blinker kann nicht eingeschaltet werden, da der rechte Blinker bereits aktiviert ist.");
      }
    } else if (key == '2') {
      toggleHazardLights();
    } else if (key == '3') {
      if (!hazardState && !leftState) {
        toggleRightBlinker();
      } else if (hazardState) {
        Serial.println("Fehler: Der rechte Blinker kann nicht eingeschaltet werden, da der Warnblinker aktiviert ist.");
      } else if (leftState) {
        Serial.println("Fehler: Der rechte Blinker kann nicht eingeschaltet werden, da der linke Blinker bereits aktiviert ist.");
      }
    } else if (key == '4') {
      toggleBrakeLight();
    } else if (key == '5') {
      toggleFrontHeadlights();
    } else if (key == '6') {
      readMPU6050Data();
    } else if (key == '7') {
      PrintDistanz();
    }
  }

  // Zeitgesteuerte Aktionen
  if (currentMillis - previousMillis >= blinkInterval) {
    previousMillis = currentMillis;

    if (hazardState) {
      toggleHazardLightsState();
    } else {
      if (leftState) {
        toggleLeftBlinkerState();
      }

      if (rightState) {
        toggleRightBlinkerState();
      }
    }
  }

  if (currentMillis - previousDistanceMillis >= distanceInterval) {
    previousDistanceMillis = currentMillis;
    measureDistance();
    updateLEDBasedOnDistance();
  }
}


void toggleLeftBlinker() 
{
  leftState = !leftState;  // Zustand des linken Blinkers umkehren
  digitalWrite(leftLED1, leftState);
  digitalWrite(leftLED2, leftState);

  if (leftState != previousLeftState) {
    Serial.print("Linker Blinker: ");
    Serial.println(leftState ? "An" : "Aus");
    previousLeftState = leftState;
  }
}

void toggleRightBlinker() 
{
  rightState = !rightState;  // Zustand des rechten Blinkers umkehren
  digitalWrite(rightLED1, rightState);
  digitalWrite(rightLED2, rightState);

  if (rightState != previousRightState) {
    Serial.print("Rechter Blinker: ");
    Serial.println(rightState ? "An" : "Aus");
    previousRightState = rightState;
  }
}

void toggleHazardLights() {
  hazardState = !hazardState;  // Zustand des Warnblinkers umkehren

  if (hazardState) {
    // Wenn der Warnblinker aktiviert wird, werden die normalen Blinker ausgeschaltet
    digitalWrite(leftLED1, LOW);
    digitalWrite(leftLED2, LOW);
    digitalWrite(rightLED1, LOW);
    digitalWrite(rightLED2, LOW);
  } else {
    // Wenn der Warnblinker deaktiviert wird, werden die normalen Blinker wieder eingeschaltet
    digitalWrite(leftLED1, leftState);
    digitalWrite(leftLED2, leftState);
    digitalWrite(rightLED1, rightState);
    digitalWrite(rightLED2, rightState);
  }

  if (hazardState != previousHazardState) {
    Serial.print("Warnblinker: ");
    Serial.println(hazardState ? "An" : "Aus");
    previousHazardState = hazardState;
  }
}

void toggleLeftBlinkerState() {
  digitalWrite(leftLED1, !digitalRead(leftLED1));  // Zustand des linken Blinkers umkehren
  digitalWrite(leftLED2, !digitalRead(leftLED2));
}

void toggleRightBlinkerState() {
  digitalWrite(rightLED1, !digitalRead(rightLED1));  // Zustand des rechten Blinkers umkehren
  digitalWrite(rightLED2, !digitalRead(rightLED2));
}

void toggleHazardLightsState() {
  digitalWrite(leftLED1, !digitalRead(leftLED1));  // Zustand des linken Blinkers umkehren
  digitalWrite(leftLED2, !digitalRead(leftLED2));
  digitalWrite(rightLED1, !digitalRead(rightLED1));  // Zustand des rechten Blinkers umkehren
  digitalWrite(rightLED2, !digitalRead(rightLED2));
}

void toggleBrakeLight() {
  static bool brakeLightState = LOW;
  brakeLightState = !brakeLightState;
  digitalWrite(brakeLightPin1, brakeLightState);
  digitalWrite(brakeLightPin2, brakeLightState);

  Serial.print("Bremslicht: ");
  Serial.println(brakeLightState ? "An" : "Aus");
}

void measureDistance() {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
}

void updateLEDBasedOnDistance() {
  digitalWrite(greenLED, LOW);
  digitalWrite(yellowLED, LOW);
  digitalWrite(redLED, LOW);

  if (distance <= 50) {
    digitalWrite(redLED, HIGH);  // Rote LED einschalten
  } else if (distance <= 100) {
    // Gelbe LED schnell blinken lassen
    for (int i = 0; i < 10; i++) {
      digitalWrite(yellowLED, HIGH);
      delay(500);  // Gelbe LED für 100 Millisekunden einschalten
      digitalWrite(yellowLED, LOW);
      delay(500);  // Gelbe LED für 100 Millisekunden ausschalten
    }
  } else if (distance <= 200) {
    // Grüne LED langsam blinken lassen
    for (int i = 0; i < 2; i++) {
      digitalWrite(greenLED, HIGH);
      delay(1000);  // Grüne LED für 1 Sekunde einschalten
      digitalWrite(greenLED, LOW);
      delay(1000);  // Grüne LED für 1 Sekunde ausschalten
    }
  }
}

void PrintDistanz() {
    Serial.print("Abstand: ");
  Serial.print(distance);
   Serial.println("cm");
}

void toggleFrontHeadlights() {
  static bool frontHeadlightsState = LOW;

  frontHeadlightsState = !frontHeadlightsState;

  digitalWrite(leftRGBLedRed, frontHeadlightsState);
  digitalWrite(leftRGBLedGreen, frontHeadlightsState);
  digitalWrite(leftRGBLedBlue, frontHeadlightsState);
  digitalWrite(rightRGBLedRed, frontHeadlightsState);
  digitalWrite(rightRGBLedGreen, frontHeadlightsState);
  digitalWrite(rightRGBLedBlue, frontHeadlightsState);

  // Aktiviere oder deaktiviere die Rückscheinwerfer entsprechend dem Zustand der Frontscheinwerfer
  digitalWrite(reverseLightPin1, frontHeadlightsState);
  digitalWrite(reverseLightPin2, frontHeadlightsState);

  Serial.print("Abblendlicht: ");
  Serial.println(frontHeadlightsState ? "An" : "Aus");
}

void readMPU6050Data() {
  mpu6050.update();

  float accelX = mpu6050.getAccX();
  float accelY = mpu6050.getAccY();
  float accelZ = mpu6050.getAccZ();

  float gyroX = mpu6050.getGyroX();
  float gyroY = mpu6050.getGyroY();
  float gyroZ = mpu6050.getGyroZ();

  float temp = mpu6050.getTemp();

  Serial.print("Beschleunigung X: ");
  Serial.print(accelX);
  Serial.print("  Y: ");
  Serial.print(accelY);
  Serial.print("  Z: ");
  Serial.println(accelZ);

  Serial.print("Gyroskop X: ");
  Serial.print(gyroX);
  Serial.print("  Y: ");
  Serial.print(gyroY);
  Serial.print("  Z: ");
  Serial.println(gyroZ);

  Serial.print("Temperatur: ");
  Serial.print(temp);
  Serial.println(" Grad Celsius");
}