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

#define SCREEN_WIDTH 128  // Define the screen width to be 128 pixels
#define SCREEN_HEIGHT 64  // Define the screen height to be 64 pixels
#define OLED_RESET -1     // Define the OLED reset pin to -1 (or -1 if the reset function is not used)
// Create an Adafruit_SSD1306 object that controls the OLED screen, specifying the screen width, height, I2C bus object, and reset pin
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define Echo 18      // Define the ultrasonic Echo pin
#define Trig 17      // Define the ultrasonic Trig pin
int Udistance = 0;  // Define the ultrasonic distance variable

void setup() {
  Serial.begin(9600);
  pinMode(Echo, INPUT);   // Define the pin operation mode as the input
  pinMode(Trig, OUTPUT);  // Define the pin operation mode as output
  Wire.begin(5,4);        // The initial I2C bus SDA pin is set to 5 and the SCL pin is set to 4

  // Initializing the screen
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 initialization failed"));
    for (;;)
      ;
  }
}

void loop() {
  Udistance = GetDistance();  // Obtaining ultrasonic distance
  display.clearDisplay();     // Clear the screen buffer
  // Display ultrasonic distance
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(16, 8);
  display.println("Distance");
  display.setCursor(32, 40);
  display.print(Udistance);
  display.print(" cm");
  display.display();
  //delay(200);  // Delay the next read for a while to adjust the update rate
  Serial.print("Distance = ");
  Serial.println(Udistance);
}

float GetDistance()  // Get ultrasonic sensor values
{
  float distance;
  digitalWrite(Trig, LOW);  // Sending a low level to the Trig triggers ranging
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  distance = pulseIn(Echo, HIGH) / 58.00;  // Output distance conversion
  // delay(10);
  return distance;
}