#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"





#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 32 


#define OLED_RESET     4 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


const int trigPin = 9;
const int echoPin = 10;
const int greenLedPin = 13;
const int buzzerPin = 12;
const int yellowLedPin = 7;
const int redLedPin = 2;


const int trigDistance = 150; 
const int trigDistance2 = 100;

long duration;
int distance;
uint32_t delayMS;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT); 
  pinMode(buzzerPin, OUTPUT); 
  pinMode(greenLedPin, OUTPUT); 
  pinMode(yellowLedPin, OUTPUT);
  pinMode(redLedPin, OUTPUT);
  Serial.begin(9600); 
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
  
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.clearDisplay(); 
  delayMS = 1000;
}

void ledOnOff(int distance){
  if(distance < trigDistance){
    digitalWrite(greenLedPin, LOW);
    digitalWrite(yellowLedPin, HIGH);   
   
  }
  else{
    digitalWrite(greenLedPin, HIGH);   
    digitalWrite(buzzerPin, LOW);   
  }
  if(distance < trigDistance2){
    digitalWrite(yellowLedPin, LOW);
    digitalWrite(redLedPin, HIGH);
    digitalWrite(buzzerPin, HIGH);   
    delay(100);
    digitalWrite(buzzerPin, LOW);   
    delay(100);
    digitalWrite(buzzerPin, HIGH);
  }
  else{
   digitalWrite(redLedPin, LOW);
   digitalWrite(buzzerPin, LOW);
  }
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance= duration*0.034/2;
  Serial.print("Distance: ");
  Serial.println(distance);

  if (distance < trigDistance){
      ledOnOff(distance);
      display.setTextSize(1);
      display.setCursor(1,1);
      display.print("Abstand Halten!"); 
      display.println();
      display.setTextSize(2);
      display.setCursor(15,15);  
      display.print(distance);     
      display.display();
      delay(delayMS);
      display.clearDisplay();
  }
  else{
      ledOnOff(distance);
      display.setTextSize(1);
      display.setCursor(1,1);
      display.print("Distance:"); 
      display.println();
      display.setTextSize(2);
      display.setCursor(15,15);  
      display.print(distance);     
      display.display();
      delay(delayMS);
      display.clearDisplay();
  }
}