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

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

// define servo parameters
#define SERVO_PIN 19
Servo myservo;

// Define HC-SR04 parameters
#define TRIG_PIN 23
#define ECHO_PIN 2


// Define LEDs parameters
#define RED_LED 18
#define GREEN_LED 4
#define ORANGE_LED 5

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



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); 

  // Initialize led
  pinMode(RED_LED , OUTPUT);
  pinMode(ORANGE_LED , OUTPUT);
  pinMode(GREEN_LED, OUTPUT);

    // Initialize servo
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  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();
} 

//funtion for check distance
float read_distance(){
  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() {
  // put your main code here, to run repeatedly:
  float distance=read_distance();
  //show the distance in display
  print_line(distance, 0, 0);

  //Denote the state By LEDs
  digitalWrite(RED_LED,LOW);
  digitalWrite(ORANGE_LED,LOW);
  digitalWrite(GREEN_LED,LOW);
  
  float angle = distance - 60;
  myservo.write(angle);

  if (distance<=60){
    digitalWrite(RED_LED,HIGH);
    Serial.println(distance);

  }
  else if (60<distance && distance <=240){
    digitalWrite(ORANGE_LED,HIGH);
    Serial.println(distance);

  }
  else{
    digitalWrite(GREEN_LED,HIGH);
    Serial.println(distance);
  } 
  

  display.clearDisplay();

  delay(100);
  


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