#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

const int VornetrigPin = 5;
const int VorneechoPin = 18;
const int UntentrigPin = 2;
const int UntenechoPin = 4;

#define SOUND_SPEED 0.034

long durationVorne;
long durationUnten;
int distanceVorne;
int distanceUnten;

const int vorneLedPin = 13; // Red LED
const int untenLedPin = 12; // Blue LED

void setup() {
  Serial.begin(115200);
  pinMode(VornetrigPin, OUTPUT);
  pinMode(VorneechoPin, INPUT);
  pinMode(UntentrigPin, OUTPUT);
  pinMode(UntenechoPin, INPUT);

  pinMode(vorneLedPin, OUTPUT);
  pinMode(untenLedPin, OUTPUT);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;) ;
  }
  delay(500);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
}

void loop() {
  digitalWrite(VornetrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(VornetrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(VornetrigPin, LOW);
  durationVorne = pulseIn(VorneechoPin, HIGH);

  digitalWrite(UntentrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(UntentrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(UntentrigPin, LOW);
  durationUnten = pulseIn(UntenechoPin, HIGH);

  distanceVorne = durationVorne * SOUND_SPEED / 2;
  distanceUnten = durationUnten * SOUND_SPEED / 2;

  Serial.print("Distance Vorne (cm): ");
  Serial.println(distanceVorne);
  Serial.print("Distance Unten (cm): ");
  Serial.println(distanceUnten);

  display.clearDisplay();
  display.setCursor(0, 0);


  int alarmStatusVorne = getVorneAlarmStatus(distanceVorne);
  int alarmStatusUnten = getUntenAlarmStatus(distanceUnten);

  setLedBehavior(vorneLedPin, alarmStatusVorne);
  setLedBehavior(untenLedPin, alarmStatusUnten);

  display.print("Alarm Status: ");
  display.println(alarmStatusVorne);
  display.setCursor(0, 10);
  display.print(distanceVorne);
  display.print("cm Vorne");
  display.setCursor(0, 20);
  display.print(distanceUnten);
  display.print("cm Unten");
  display.display();
  
}

void setLedBehavior(int ledPin, int alarmStatus) {
  switch (alarmStatus) {
    case 0: // Normal (N) status
      digitalWrite(ledPin, LOW);
      break;
    case 1: // Gelb (1) status
      blinkLED(ledPin, 500);
      break;
    case 2: // Rot (2) status
      blinkLED(ledPin, 200);
      break;
    default:
      break;
  }
}

void blinkLED(int ledPin, int interval) {
  static unsigned long previousMillis = 0;
  static int ledState = LOW;

  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    ledState = !ledState;
    digitalWrite(ledPin, ledState);
  }
}

int getVorneAlarmStatus(int distance) {
  if (distance >= 100) {
    return 0; // Normal (N) status
  } else if (distance >= 50) {
    return 1; // Gelb (1) status
  } else {
    return 2; // Rot (2) status
  }
}

int getUntenAlarmStatus(int distance) {
  if (distance >= 100) {
    return 0; // Normal (N) status
  } else if (distance >= 50) {
    return 1; // Gelb (1) status
  } else {
    return 2; // Rot (2) status
  }
}