// 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


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

#define SONAR_NUM 3      // Number of sensors.
#define MAX_DISTANCE 400 // Maximum distance (in cm) to ping.
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(18, 5, MAX_DISTANCE),
    NewPing(19, 23, MAX_DISTANCE)
};

// 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

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

//-------motor basespeed---------
int LeftSpeed = 90;    // 80   90    100   170
int RightSpeed = 105;   // 93   105   117   198

int fblock=20;
int block=30;

static const uint8_t data[1024] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x03, 0xf0, 0xff, 0xfe, 0x00, 0xfc, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x0f, 0xf8, 0xff, 0xfe, 0x07, 0xfe, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x1f, 0xf8, 0xff, 0xfe, 0x0f, 0xfe, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x3f, 0xf8, 0x7f, 0xfe, 0x1f, 0xfe, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x3c, 0x00, 0x03, 0xc0, 0x3e, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x3c, 0x00, 0x03, 0xc0, 0x7c, 0x00, 0x00, 
    0x00, 0x00, 0x18, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x3c, 0x00, 0x03, 0xc0, 0x78, 0x00, 0x00, 
    0x00, 0x00, 0x3c, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x3e, 0x00, 0x03, 0xc0, 0x78, 0x00, 0x00, 
    0x00, 0x00, 0x18, 0x00, 0x00, 0x03, 0xc3, 0xff, 0xfc, 0x3f, 0xc0, 0x03, 0xc0, 0xf8, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xff, 0xfc, 0x1f, 0xf0, 0x03, 0xc0, 0xf8, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xff, 0xfc, 0x07, 0xf8, 0x03, 0xc0, 0xf8, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc3, 0xff, 0xfc, 0x01, 0xfc, 0x03, 0xc0, 0xf8, 0x00, 0x00, 
    0x00, 0x00, 0x7f, 0x00, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x00, 0x7c, 0x03, 0xc0, 0xf8, 0x00, 0x00, 
    0x00, 0x03, 0xff, 0xc0, 0x00, 0x03, 0xc3, 0xc0, 0x3c, 0x00, 0x3c, 0x03, 0xc0, 0x78, 0x00, 0x00, 
    0x00, 0x0f, 0xff, 0xf0, 0x00, 0x07, 0xc3, 0xc0, 0x3c, 0x00, 0x3c, 0x03, 0xc0, 0x7c, 0x00, 0x00, 
    0x00, 0x1f, 0xff, 0xf8, 0x00, 0x07, 0x83, 0xc0, 0x3c, 0x20, 0x3c, 0x03, 0xc0, 0x3e, 0x00, 0x00, 
    0x00, 0x7f, 0xff, 0xfe, 0x00, 0xff, 0x83, 0xc0, 0x3c, 0x3f, 0xfc, 0x03, 0xc0, 0x3f, 0xfe, 0x00, 
    0x00, 0xff, 0xff, 0xff, 0x00, 0xff, 0x83, 0xc0, 0x3c, 0x3f, 0xf8, 0x03, 0xc0, 0x1f, 0xfe, 0x00, 
    0x01, 0xff, 0xff, 0xff, 0x00, 0xff, 0x03, 0xc0, 0x3c, 0x3f, 0xf0, 0x03, 0xc0, 0x07, 0xfe, 0x00, 
    0x1d, 0xfc, 0x3c, 0x3f, 0xb0, 0xfc, 0x03, 0xc0, 0x3c, 0x0f, 0xc0, 0x03, 0xc0, 0x01, 0xfc, 0x00, 
    0x1b, 0xf0, 0x00, 0x0f, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x1b, 0xf0, 0x00, 0x0f, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x13, 0xe0, 0x00, 0x07, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x13, 0xe0, 0x00, 0x07, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x07, 0xe3, 0x00, 0xc7, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x07, 0xe3, 0x81, 0xc7, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x07, 0xe3, 0x81, 0xc7, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x07, 0xe0, 0x00, 0x07, 0xe0, 0x3e, 0x00, 0x60, 0x00, 0x0c, 0x00, 0x00, 0xf2, 0x00, 0xc0, 0x00, 
    0x03, 0xe0, 0x00, 0x07, 0xc0, 0x33, 0x00, 0x60, 0x00, 0x8c, 0x00, 0x03, 0x12, 0x00, 0xc0, 0x00, 
    0x03, 0xe0, 0x00, 0x07, 0xc0, 0x21, 0x00, 0x60, 0x00, 0x80, 0x00, 0x02, 0x02, 0x00, 0xc0, 0x00, 
    0x03, 0xe0, 0x00, 0x07, 0xc0, 0x21, 0x1c, 0x7c, 0x79, 0xec, 0xf3, 0x86, 0x02, 0x66, 0xf8, 0x00, 
    0x03, 0xe0, 0x00, 0x07, 0xc0, 0x23, 0x32, 0x66, 0xcd, 0x8d, 0x86, 0x06, 0x02, 0x66, 0xcc, 0x00, 
    0x01, 0xe0, 0x00, 0x0f, 0x80, 0x3e, 0x23, 0x62, 0x84, 0x8d, 0x86, 0x06, 0x02, 0x66, 0xc4, 0x00, 
    0x01, 0xf8, 0x00, 0x1f, 0x00, 0x26, 0x63, 0x62, 0x84, 0x8d, 0x03, 0x86, 0x02, 0x66, 0xc4, 0x00, 
    0x00, 0xfe, 0x3f, 0xff, 0x00, 0x22, 0x23, 0x62, 0x84, 0x8d, 0x80, 0xc6, 0x02, 0x66, 0xc4, 0x00, 
    0x00, 0x7e, 0xff, 0xfe, 0x00, 0x23, 0x32, 0x66, 0xcc, 0x8d, 0x94, 0xc3, 0x1a, 0x66, 0xcc, 0x00, 
    0x00, 0x1f, 0xff, 0xf8, 0x00, 0x21, 0x9c, 0x7c, 0x78, 0xec, 0xf3, 0x80, 0xf2, 0x3e, 0xf8, 0x00, 
    0x00, 0x0f, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

// 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) {
    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");


    
}
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());
}

SemaphoreHandle_t xMutex = NULL;  // Create a mutex object

int counter = 0;  // A shared variable

// // Define task handles
TaskHandle_t task1Handle;
TaskHandle_t task2Handle;


void setup(){
    Serial.begin(115200);
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
    display.setTextColor(WHITE);

    xMutex = xSemaphoreCreateMutex();  // crete a mutex object

    xTaskCreatePinnedToCore (
        task1,     // Function to implement the task
        "task1Handle",   // Name of the task
        2048,      // Stack size in words
        NULL,      // Task input parameter
        2,         // Priority of the task
        // NULL,      // Task handle.
        &task1Handle,      // Task handle.
        1          // Core where the task should 
    );

    xTaskCreatePinnedToCore(
        task2,     // Function to implement the task
        "task2Handle",   // Name of the task
        2048,      // Stack size in words
        NULL,      // Task input parameter
        1,         // Priority of the task
        // NULL,      // Task handle.
        &task2Handle,      // Task handle.
        0          // Core where the task should run
    );
    // Start the scheduler
    vTaskStartScheduler();
}

void task1(void* pvParameters) {
    while (true) {
        if (xSemaphoreTake (xMutex, portMAX_DELAY)) {  // take the mutex

            //------------mesure distance-------
            front = sonar[0].ping_cm();
            left = sonar[1].ping_cm();
            right = sonar[2].ping_cm();

            xSemaphoreGive (xMutex);  // release the mutex
            delay (50);
        }
    }
}

void task2(void* pvParameters) {
    while (true) {
        if (xSemaphoreTake (xMutex, (200 * portTICK_PERIOD_MS))) {  // try to acquire the mutex
            //display information
            display.clearDisplay();
            if (right >= 50  && front >= 50 && left >= 50) {
                display.drawBitmap(0, 0, data, 128, 64, 1);
            } else {
                DisplayStatus(front, left, right);
                // displayText(0, 45, 2, "A:", motors.getSpeedA());
                // displayText(65, 45, 2, "B:", motors.getSpeedA()); 
            }
            display.display(); // Display the buffer on the OLED screen
                    
            // movment control
            if (right >= 50 && front >= 50 && left >= 50) {
                motors.stop();
            } else if (right >= block) {
                F = false, L = false, R = true;
                go("right");
            } else if (front >= fblock) {
                F = true, L = false, R = false;   
                goForward();
            } else if (left >= block) {
                F = false, L = true, R = false;
                go("left");    
            } else if (right <= block && front <=fblock && left <=block) {
                F = false, L = false, R = false;
                uTurn();
            } else {
                motors.stop();
            }

            xSemaphoreGive (xMutex);  // release the mutex
            delay (50);
        }
    }
}


void loop(){}

// -------------------- motor control -----------------
void goForward() {  // Function to make the robot go forward
    motors.stop();
    // Set initial speed for both motors
    motors.setSpeedA(LeftSpeed);
    motors.setSpeedB(RightSpeed);
    motors.forward();       // Move both motors forward
    if (right <=5) {
        motors.setSpeedA(LeftSpeed-5);
        motors.setSpeedB(RightSpeed+11);
        motors.forward();

        DisplayStatus(front, left, right); 
        printSomeInfo();
        // motors.stop();
    }
    else if (right >=6 && right<=8){   
        motors.setSpeedA(LeftSpeed);
        motors.setSpeedB(RightSpeed);
        motors.forward();

        DisplayStatus(front, left, right); 
        printSomeInfo();
        // motors.stop();
    } 
    else if (right >=9){   
        motors.setSpeedA(LeftSpeed+10);
        motors.setSpeedB(RightSpeed-5);
        motors.forward();

        DisplayStatus(front, left, right); 
        printSomeInfo();
    }
    motors.stop();
}

void go(const char *dir){
    motors.stop();
        // turn right
        if (dir == "right"){
            //if needed to go a little forward before turn 
            // Left = 80; Right = 93 ... Estimate 3-6 cm
            // Left = 90; Right = 105 ... Estimate 4-7 cm
            // Left = 100; Right = 117 ... Estimate 7-10 cm
            // Left = 170; Right = 198 ... Estimate 14-16 cm
            motors.setSpeedA(80); //80
            motors.setSpeedB(93); //93
            motors.forward();
            printSomeInfo();
            delay(300);
            motors.stop();

            // turn right --- estimate 90 degre
            motors.setSpeedA(170);
            motors.setSpeedB(198);
            motors.forwardA();
            motors.backwardB(); 
            printSomeInfo();
            delay(280);
            motors.stop();
        }

        // turn Left --- estimate 90 degre
        if (dir == "left"){
            // turn Left --- estimate 90 degre
            motors.setSpeedA(170);
            motors.setSpeedB(198);
            motors.backwardA();
            motors.forwardB();
            printSomeInfo(); 
            delay(280);
            motors.stop();
        }
    
    // will go a little forward
    // Left = 170; Right = 198 ... for 400ms Estimate 17-20 cm
    motors.setSpeedA(170); //80
    motors.setSpeedB(198); //93
    motors.forward();
    printSomeInfo();
    delay(400);
    motors.stop();
}

void uTurn() {  // Function to make the robot perform a U-turn
    motors.stop();
    motors.setSpeedA(170);
    motors.setSpeedB(198);
    motors.backwardA();
    motors.forwardB();
    printSomeInfo();
    delay(500);
    motors.stop();
}