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

// Initialize OLED display (128x64 pixels) using I2C communication.
Adafruit_SSD1306 display(128, 64, &Wire, -1);

// Function to initialize the OLED display
void displaySetup(){
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
    display.display();                         // showing the Adafruit logo
    //do not forget display.display(); otherwise the picture will not be visible
    delay(100); // waiting 10ms
    display.setTextColor(WHITE);
}

// Function to set up the pins for the ultrasonic sensor
void pinSetup(int trigPin, int echoPin){
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
}

// Function to measure distance using the ultrasonic sensor and display it on the OLED screen
int measureDistance(int trigPin, int echoPin){
    pinSetup(trigPin, echoPin);
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long duration = pulseIn(echoPin, HIGH);
    int distance = duration * 0.034 / 2;
    return distance;
}

void setup(){
    Serial.begin(115250); // Initialize the serial communication for debugging
    displaySetup(); // Initialize the OLED display
}

// Function to display text on the OLED screen at a specific position and size
void displayText(int x, int y, int textSize, const char *displayText){
    display.setCursor(x, y);
    display.setTextSize(textSize);
    display.print(displayText);
}

void loop(){
    int frontDistance = measureDistance(4, 2);   // Measure front distance
    int leftDistance = measureDistance(13, 12);  // Measure left distance
    int rightDistance = measureDistance(23, 18); // Measure right distance

    display.clearDisplay(); // Clear the display buffer

    // Display front, left, and right distances on the OLED screen
    displayText(0, 0, 2, "Front:");
    display.println(frontDistance);
    displayText(0, 20, 2, "Left:");
    display.println(leftDistance);
    displayText(0, 42, 2, "Right:");
    display.println(rightDistance);

    display.display(); // Display the buffer on the OLED screen
    delay(250); // Add a small delay for readability
}