#include <Wire.h>

// Pin assignments for ultrasonic sensors
const int trigPinFront = 2;       // Shared trigger pin for front sensors
const int echoPinLeftFront = 16;
const int echoPinRightFront = 17;
const int trigPinRear = 4;        // Shared trigger pin for rear sensors
const int echoPinLeftRear = 5;
const int echoPinRightRear = 18;

int frontVL53L1X_value = 0;
int rearVL53L0X_value = 0;

// Threshold for detecting a drastic change in distance
const long DRIFT_THRESHOLD = 30; // Adjust this value based on your requirements
const unsigned long DRIFT_TIME_THRESHOLD = 1000; // Time threshold in milliseconds

// Previous distance values and timestamps
long prevLeftFrontDist = 0;
long prevLeftRearDist = 0;
long prevRightFrontDist = 0;
long prevRightRearDist = 0;

unsigned long leftFrontTime = 0;
unsigned long leftRearTime = 0;
unsigned long rightFrontTime = 0;
unsigned long rightRearTime = 0;

bool parkingSpotSearchActive = false;
int parkingSpotSide = -1; // Variable to store the side for parking spot search

void setup() {
    Serial.begin(115200);

    // Initialize pins for ultrasonic sensors
    pinMode(trigPinFront, OUTPUT);
    pinMode(echoPinLeftFront, INPUT);
    pinMode(echoPinRightFront, INPUT);
    pinMode(trigPinRear, OUTPUT);
    pinMode(echoPinLeftRear, INPUT);
    pinMode(echoPinRightRear, INPUT);
    
    Wire.begin();
}

long readUltrasonic(int trigPin, int echoPin) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    
    long duration = pulseIn(echoPin, HIGH);
    long distance = duration * 0.034 / 2;
    
    return distance;
}

long readLeftFrontUltrasonic() {
    return readUltrasonic(trigPinFront, echoPinLeftFront);
}

long readRightFrontUltrasonic() {
    return readUltrasonic(trigPinFront, echoPinRightFront);
}

long readLeftRearUltrasonic() {
    return readUltrasonic(trigPinRear, echoPinLeftRear);
}

long readRightRearUltrasonic() {
    return readUltrasonic(trigPinRear, echoPinRightRear);
}

int motor_pwm = 0; // Variable to store motor PWM value
int servo_angle = 0; // Variable to store servo angle value

void readDistancesFromSerial() {
    if (Serial.available() > 0) {
        String input = Serial.readStringUntil('\n');
        input.trim();
        
        int commaIndex = input.indexOf(',');
        if (commaIndex > 0) {
            String motorPWMStr = input.substring(0, commaIndex);
            String servoAngleStr = input.substring(commaIndex + 1);
            
            motor_pwm = motorPWMStr.toInt();
            servo_angle = servoAngleStr.toInt();
        }
    }
}

// int readFrontVL53L1X() {
//     return frontVL53L1X_value;
// }

// int readRearVL53L0X() {
//     return rearVL53L0X_value;
// }

int readmotorPWM() {
    return motor_pwm;
}

int readServoAngle() {
    return servo_angle;
}

void checkForDrasticChange(long currentDist, long prevDist, unsigned long &prevTime, const char* direction) {
    unsigned long currentTime = millis();
    if (abs(currentDist - prevDist) > DRIFT_THRESHOLD) {
        if ((currentTime - prevTime) > DRIFT_TIME_THRESHOLD) {
            Serial.print("Drastic change detected in ");
            Serial.println(direction);
        } else {
            prevTime = currentTime; // Update the time when a drastic change is detected
        }
    }
}

void readSerialValue() {
    int motor = readmotorPWM();
    Serial.println(motor);
    int servo = readServoAngle();
    Serial.println(servo);
}

void start_parking_spot_search() {
    // Arrays to store previous 5 values of each sensor
    long prevLeftFrontDist[5];
    long prevLeftRearDist[5];
    long prevRightFrontDist[5];
    long prevRightRearDist[5];
    // Index to keep track of the position in the arrays
    int index = 0;

    // Read initial values from the sensors
    for (int i = 0; i < 5; i++) {
        prevLeftFrontDist[i] = readLeftFrontUltrasonic();
        prevLeftRearDist[i] = readLeftRearUltrasonic();
        prevRightFrontDist[i] = readRightFrontUltrasonic();
        prevRightRearDist[i] = readRightRearUltrasonic();
    }

    // Continuously read values from all ultrasonic sensors
    while (true) {
        readDistancesFromSerial();
        readSerialValue();
        long leftFrontDist = readLeftFrontUltrasonic();
        long leftRearDist = readLeftRearUltrasonic();
        long rightFrontDist = readRightFrontUltrasonic();
        long rightRearDist = readRightRearUltrasonic();

        Serial.println("Parking spot search in progress...");

        // Store the current values in the arrays
        prevLeftFrontDist[index] = leftFrontDist;
        prevLeftRearDist[index] = leftRearDist;
        prevRightFrontDist[index] = rightFrontDist;
        prevRightRearDist[index] = rightRearDist;
        // Update the index
        index = (index + 1) % 5;

        // Calculate the average of previous left and right front distances
        long avgPrevLeftFrontDist = 0;
        long avgPrevRightFrontDist = 0;
        for (int i = 0; i < 5; i++) {
            avgPrevLeftFrontDist += prevLeftFrontDist[i];
            avgPrevRightFrontDist += prevRightFrontDist[i];
        }
        avgPrevLeftFrontDist /= 5;
        avgPrevRightFrontDist /= 5;

        delay(1000);

        // Check if any current value is greater than DRIFT_THRESHOLD
        if (abs(leftFrontDist - avgPrevLeftFrontDist) > DRIFT_THRESHOLD || abs(rightFrontDist - avgPrevRightFrontDist) > DRIFT_THRESHOLD) {
            Serial.println("Activating parking spot search...");
            // Determine which side to consider based on which side's values are greater than DRIFT_THRESHOLD
            if (abs(leftFrontDist - avgPrevLeftFrontDist) > DRIFT_THRESHOLD) {
                // Activate parking spot search for the left side
                Serial.println("Parking spot search activated for the left side.");
                parkingSpotSide = 0;
            } else {
                // Activate parking spot search for the right side
                Serial.println("Parking spot search activated for the right side.");
                parkingSpotSide = 1;
            }
            // Set flag to indicate parking spot search is active
            parkingSpotSearchActive = true;
            break; // Break out of the loop
        }
    }
}

void parking_spot_search(int parkingSpotSide, long &prevLeftFrontDist, long &prevLeftRearDist, long &prevRightFrontDist, long &prevRightRearDist) {
    unsigned long startTime = 0;
    unsigned long endTime = 0;
    bool timerStarted = false;
    bool searching = true;

    while (searching) {
        readDistancesFromSerial();
        readSerialValue();
        if (parkingSpotSide == 0) {
            // Use only left ultrasonic sensors
            long leftFrontDist = readLeftFrontUltrasonic();
            long leftRearDist = readLeftRearUltrasonic();

            Serial.print("Parking spot search in progress on side: Left\n");
            Serial.print("Left Front Ultrasonic Distance: ");
            Serial.println(leftFrontDist);
            Serial.print("Left Rear Ultrasonic Distance: ");
            Serial.println(leftRearDist);

            // Check for change in front sensor
            if (!timerStarted && abs(leftFrontDist - prevLeftFrontDist) > DRIFT_THRESHOLD) {
                startTime = millis();
                timerStarted = true;
                Serial.println("Change detected in Left Front Ultrasonic Sensor. Timer started.");
            }

            // Check if timer is started
            if (timerStarted) {
                // Check if both front and rear sensors are within the range (360, 400)
                if (leftFrontDist >= 360 && leftFrontDist <= 400 && leftRearDist >= 360 && leftRearDist <= 400) {
                    endTime = millis();
                    Serial.println("Parking spot identified!");
                    endTime = endTime - startTime;
                    Serial.println(endTime);
                    searching = false; // End the search
                }
            }

            // Update the previous distance for the rear sensor
            prevLeftRearDist = leftRearDist;

        } else if (parkingSpotSide == 1) {
            // Use only right ultrasonic sensors
            long rightFrontDist = readRightFrontUltrasonic();
            long rightRearDist = readRightRearUltrasonic();

            Serial.print("Parking spot search in progress on side: Right\n");
            Serial.print("Right Front Ultrasonic Distance: ");
            Serial.println(rightFrontDist);
            Serial.print("Right Rear Ultrasonic Distance: ");
            Serial.println(rightRearDist);

            // Check for change in front sensor
            if (!timerStarted && abs(rightFrontDist - prevRightFrontDist) > DRIFT_THRESHOLD) {
                startTime = millis();
                timerStarted = true;
                Serial.println("Change detected in Right Front Ultrasonic Sensor. Timer started.");
            }

            // Check if timer is started
            if (timerStarted) {
                // Check if both front and rear sensors are within the range (360, 400)
                if (rightFrontDist >= 360 && rightFrontDist <= 400 && rightRearDist >= 360 && rightRearDist <= 400) {
                    Serial.println("Parking spot identified!");
                    searching = false; // End the search
                }
            }

            // Update the previous distance for the rear sensor
            prevRightRearDist = rightRearDist;
        } else {
            Serial.println("Nothing");
        }

        delay(1000); // Small delay to prevent overwhelming the serial output
    }

    // Call maneuver function after parking spot is identified
    maneuver(parkingSpotSide);
}

void maneuver(int parkingSpotSide) {
    Serial.print("Maneuvering to park on side: ");
    Serial.println(parkingSpotSide == 0 ? "Left" : "Right");
    
    // Add your L298 motor driver and servo control codes here
    // Example:
    // if (parkingSpotSide == 0) {
    //     // Code to control motors and servos for left-side parking
    // } else {
    //     // Code to control motors and servos for right-side parking
    // }
}

void loop() {
    // Define previous distance values
    static long prevLeftFrontDist = readLeftFrontUltrasonic();
    static long prevLeftRearDist = readLeftRearUltrasonic();
    static long prevRightFrontDist = readRightFrontUltrasonic();
    static long prevRightRearDist = readRightRearUltrasonic();
    
    // If parking spot search is not active, start the search
    if (!parkingSpotSearchActive) {
        start_parking_spot_search();
    }
    
    // If parking spot search is active, perform necessary actions
    if (parkingSpotSearchActive) {
        parking_spot_search(parkingSpotSide, prevLeftFrontDist, prevLeftRearDist, prevRightFrontDist, prevRightRearDist);
        parkingSpotSearchActive = false;  // Reset the flag after the search is completed
    }

    // Optionally, you can include the rest of your loop logic here

    delay(2000); // Delay for readability or adjust as needed
    while(1);
}