// Include necessary libraries
#include <SPI.h>              // For Serial Peripheral Interface communication
#include <Wire.h>             // For I2C communication
#include <Adafruit_GFX.h>     // Adafruit's graphics library for displays
#include <Adafruit_SSD1306.h> // Adafruit's SSD1306 OLED display library
#include <NewPing.h>          // Library for using ultrasonic distance sensor
#include <L298NX2.h>          // Library for controlling motors using L298N module
  
#define SONAR_NUM 3      // Number of sensors.
#define MAX_DISTANCE 400 // Maximum distance (in cm) to ping.

int block = 10;
// int motorDelay = 5000;
int front, left, right;                 // Declare variables to store distances from sensors
bool F = true, L = false, R = false;    // Define boolean variables to represent sensor directions


NewPing sonar[SONAR_NUM] = {      // Sensor object array.
    NewPing(15, 4, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
    NewPing(19, 23, MAX_DISTANCE),
    NewPing(5, 18, MAX_DISTANCE)};

// 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
    display.setTextColor(WHITE);
}
// Function to display text on the OLED display with the specified position, size, text, and value
void displayText(int x, int y, int textSize, const char *displayText, int value) {
    display.setCursor(x, y);         // Set the cursor position on the display
    display.setTextSize(textSize);   // Set the text size
    display.print(displayText);      // Print the provided text
    display.println(value);          // Print the provifront, left, rightded value
}

// Function to display the status information on the OLED display
void DisplayStatus(int f, int l, int r) {
    display.clearDisplay(); // Clear the display buffer
    displayText(0, 0, 2, "F:", f);    // Display front sensor's distance
    displayText(0, 22, 2, "L:", l);    // Display left sensor's distance
    displayText(65, 0, 2, "R:", r);   // Display right sensor's distance
    
    // Display movement direction on the display
    display.setCursor(0, 45);   // Set cursor position for movement direction text
    if (F)display.print("Go Forward");
    else if(R)display.print("Go Right");
    else if (L)display.print("Go Left");
    else display.print("U Turn");

    // Print status information to the Serial Monitor
    Serial.print("Front:");
    Serial.println(f);
    Serial.print(" Left:");
    Serial.println(l);
    Serial.print(" Right:");
    Serial.println(r);
    Serial.println(" ");
    display.display();
}

// Left Motor connections
int enL = 13, in1 = 14, in2 = 12;
// Right Motor connections
int enR = 25, in3 = 26, in4 = 27;

L298NX2 motors(enL, in1, in2, enR, in3, in4);   // Create an instance of the L298NX2 motor controller with the specified pins

// Function to print information about motor movement and speed to the Serial Monitor
void printSomeInfo() {
    Serial.print("Motor A is moving = ");
    Serial.print(motors.isMovingA() ? "YES" : "NO");
    Serial.print(" at speed = ");
    Serial.println(motors.getSpeedA());
    Serial.print("Motor B is moving = ");
    Serial.print(motors.isMovingB() ? "YES" : "NO");
    Serial.print(" at speed = ");
    Serial.println(motors.getSpeedB());
}
void goForward() {  // Function to make the robot go forward
    // Set initial speed for both motors
    motors.setSpeedA(100);
    motors.setSpeedB(100);
    motors.forward();       // Move both motors forward
    readsonar();
    if (right <=4) {
        motors.setSpeedA(70);
        motors.setSpeedB(100);
        motors.forward(); 
        DisplayStatus(front, left, right); 
        printSomeInfo();
        readsonar();
    }
    else if (right >=5 && right<=7){   
        F = true, L = false, R = false;
        motors.setSpeedA(100);
        motors.setSpeedB(100);
        motors.forward(); 
        DisplayStatus(front, left, right); 
        printSomeInfo();
        readsonar();
    } 
    else if (right >= 10){   
        F = true, L = false, R = false;
        motors.setSpeedA(100);
        motors.setSpeedB(70);
        motors.forward(); 
        DisplayStatus(front, left, right); 
        printSomeInfo();
        readsonar();
    } 
    printSomeInfo();        // Print motor information
}
void goLeft() {     // Function to make the robot turn left
    // Set initial speed for both motors
    motors.setSpeedA(10);
    motors.setSpeedB(100);
    motors.forward();
    readsonar();
    DisplayStatus(front, left, right); 
    printSomeInfo();
    while (front <15){
        readsonar();
        DisplayStatus(front, left, right); 
        printSomeInfo();
    }
    readsonar();
    motors.setSpeedA(100);
    motors.forward(); 
    DisplayStatus(front, left, right); 
    printSomeInfo();
}
void goRight() {    // Function to make the robot turn right
    // Set initial speed for both motors
    motors.setSpeedA(100);
    motors.setSpeedB(10);
    motors.forward();
    readsonar();
    DisplayStatus(front, left, right); 
    printSomeInfo();
    while (front <15){
        readsonar();
        DisplayStatus(front, left, right); 
        printSomeInfo();
    }
    readsonar();
    motors.setSpeedB(100);
    motors.forward();
    DisplayStatus(front, left, right); 
    printSomeInfo();
    }
void uTurn() {  // Function to make the robot perform a U-turn
    readsonar();
    // Set initial speed for both motors
    motors.setSpeedA(100);
    motors.setSpeedB(100);
    // Stop motors, then move one motor backward and the other forward
    motors.stop();
    motors.backwardA();
    motors.forwardB();
    readsonar();
    DisplayStatus(front, left, right); 
    printSomeInfo();
    while (front <15){
        readsonar();
        DisplayStatus(front, left, right); 
        printSomeInfo();
    }
    readsonar();
    motors.setSpeedA(100);
    motors.setSpeedB(100);
    motors.forwardA();
    motors.forwardB();

    DisplayStatus(front, left, right); 
    printSomeInfo();
    printSomeInfo(); 
    }
void setup(){
    Serial.begin(115200); // Initialize the serial communication for debugging
    displaySetup();       // Initialize the OLED display

    // Turn off motors - Initial state
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
}
void readsonar(){
    front = sonar[0].ping_cm(); // Measure front distance
    left = sonar[1].ping_cm();  // Measure left distance
    right = sonar[2].ping_cm(); // Measure right distance
} 
void loop(){
    readsonar();
    display.clearDisplay(); // Clear the display buffer
    DisplayStatus(front, left, right);    // Display the current status on the OLED display
    printSomeInfo();

    
    // Check distance readings and decide robot's movement
    if (right > 20) {
        F = false, L = false, R = true;
        goRight();
    }
    
    else if (front > 15){   
        F = true, L = false, R = false;
        goForward();
    }
    else if (left > 20){
        F = false, L = true, R = false;
        goLeft();
    }
    else{
        F = false, L = false, R = false;
        uTurn();
    }

    display.display(); // Display the buffer on the OLED screen
    
}