// Include libraries
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#include <Wire.h>
#include <ESP32Servo.h>

// Define PINs
#define ECHO_PIN 23
#define TRIG_PIN 4
#define RED 18
#define ORANGE 19
#define GREEN 5
#define SERVO_PIN 2

// Define OLED parameters
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

// Create OLED display object
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

float angle = 0;
Servo myservo;

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RED, OUTPUT);
  pinMode(ORANGE, OUTPUT);
  pinMode(GREEN, OUTPUT);

  myservo.attach(SERVO_PIN);

  // Initialize display
    if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();
  delay(2000);
  display.clearDisplay();
  display.display();
}

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  float distance = readDistanceCM();

  if (distance<60.00){
    digitalWrite(RED, HIGH);
    digitalWrite(ORANGE, LOW);
    digitalWrite(GREEN, LOW);
  } else if (distance<240){
    digitalWrite(RED, LOW);
    digitalWrite(ORANGE, HIGH);
    digitalWrite(GREEN, LOW);
  } else{
    digitalWrite(RED, LOW);
    digitalWrite(ORANGE, LOW);
    digitalWrite(GREEN, HIGH);
  }

  angle = distance-60;
  myservo.write(angle);

  print_line(String(distance)+" cm", 45, 30);

  delay(100);
  display.clearDisplay();
  display.display();
}

// Function to print a line of text onto OLED
void print_line(String text, int column, int row) {
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(column, row); // (column, row)
  display.println(text);
  display.display();
}