#include <ESP32Servo.h>
#include <queue>
#include <vector>

#define TRIG_PIN1 22  // Sensor 1 Trig pin
#define ECHO_PIN1 23  // Sensor 1 Echo pin
#define TRIG_PIN2 19  // Sensor 2 Trig pin
#define ECHO_PIN2 18  // Sensor 2 Echo pin
#define TRIG_PIN3 17  // Sensor 3 Trig pin
#define ECHO_PIN3 16  // Sensor 3 Echo pin

#define SERVO_PIN1 12  // Servo 1 pin
#define SERVO_PIN2 13  // Servo 2 pin

long duration1, duration2, duration3;
float distance1, distance2, distance3;

// Grid size and the robot's current position
const int GRID_SIZE = 5;
int robotX = 0, robotY = 0;
std::vector<std::vector<int>> grid(GRID_SIZE, std::vector<int>(GRID_SIZE, 0));  // Grid initialized with 0 (empty)

struct Node {
  int x, y;
  Node(int _x, int _y) : x(_x), y(_y) {}
};

// Servo objects
Servo servo1;  // Servo for motor 1
Servo servo2;  // Servo for motor 2

// Function to move forward
void moveForward() {
  servo1.write(180);  // Rotate servo 1 forward
  servo2.write(180);    // Rotate servo 2 forward
}

// Function to turn left
void turnLeft() {
  servo1.write(0);    // Rotate servo 1 backward
  servo2.write(180);    // Rotate servo 2 forward
}

// Function to turn right
void turnRight() {
  servo1.write(180);  // Rotate servo 1 forward
  servo2.write(0);  // Rotate servo 2 backward
}

// Function for U-turn
void uturn() {
  servo1.write(0);    // Rotate servo 1 backward
  servo2.write(180);  // Rotate servo 2 forward
}

// Function to stop the motors
void stop() {
  servo1.write(90);  // Stop servo 1
  servo2.write(90);  // Stop servo 2
}

// Function to measure distance for any sensor
float measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

// BFS function to find the shortest path
std::vector<Node> bfs(int startX, int startY, int goalX, int goalY) {
  std::vector<std::vector<bool>> visited(GRID_SIZE, std::vector<bool>(GRID_SIZE, false));
  std::queue<std::vector<Node>> q;
  q.push({Node(startX, startY)});
  visited[startX][startY] = true;

  // Possible moves (up, down, left, right)
  int dx[] = {-1, 1, 0, 0};
  int dy[] = {0, 0, -1, 1};

  while (!q.empty()) {
    std::vector<Node> path = q.front();
    q.pop();
    Node current = path.back();

    // If goal is reached, return the path
    if (current.x == goalX && current.y == goalY) {
      return path;
    }

    // Explore neighbors
    for (int i = 0; i < 4; i++) {
      int newX = current.x + dx[i];
      int newY = current.y + dy[i];

      if (newX >= 0 && newX < GRID_SIZE && newY >= 0 && newY < GRID_SIZE && !visited[newX][newY] && grid[newX][newY] == 0) {
        visited[newX][newY] = true;
        std::vector<Node> newPath = path;
        newPath.push_back(Node(newX, newY));
        q.push(newPath);
      }
    }
  }

  return {};  // Return empty path if no path is found
}

// Function to dynamically update grid based on sensor data
void updateGrid() {
  // Mark nearby obstacles in the grid based on sensor distances
  if (robotY > 0 && distance1 < 10.0) grid[robotX][robotY - 1] = 1;  // Left obstacle
  if (robotX > 0 && distance2 < 10.0) grid[robotX - 1][robotY] = 1;  // Front obstacle
  if (robotY < GRID_SIZE - 1 && distance3 < 10.0) grid[robotX][robotY + 1] = 1;  // Right obstacle
}

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

  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);

  servo1.attach(SERVO_PIN1);  // Attach servo 1 to the defined pin
  servo2.attach(SERVO_PIN2);  // Attach servo 2 to the defined pin

  stop();  // Ensure motors are stopped initially
}

void loop() {
    // Measure distances from the three sensors
    distance1 = measureDistance(TRIG_PIN1, ECHO_PIN1);  // Left sensor
    distance2 = measureDistance(TRIG_PIN2, ECHO_PIN2);  // Front sensor
    distance3 = measureDistance(TRIG_PIN3, ECHO_PIN3);  // Right sensor

    Serial.print("Distance 1 (Left): ");
    Serial.println(distance1);
    Serial.print("Distance 2 (Front): ");
    Serial.println(distance2);
    Serial.print("Distance 3 (Right): ");
    Serial.println(distance3);

    // Define a threshold to detect obstacles
    float obstacleThreshold = 5.0;  // Threshold in cm for detecting obstacles

    // Check if all three sensors detect an obstacle (Dead End)
    if (distance1 < obstacleThreshold && distance2 < obstacleThreshold && distance3 < obstacleThreshold) {
        // Dead end detected
        Serial.println("Dead end detected! Performing U-turn...");
        uturn();  // Perform U-turn
        delay(1000);  // Delay to allow U-turn to complete
        stop();  // Stop motors after U-turn
    } 
    // Otherwise, check for clear paths
    else {
        // Move according to sensor readings
        if (robotY > 0 && distance1 < obstacleThreshold && distance3 < obstacleThreshold) {
            moveForward();  // Path is clear ahead, move forward
            Serial.println("Move foward");
            robotY--;  // Update the robot's position
        } else if (robotY < GRID_SIZE - 1 && distance1 < obstacleThreshold) {
            turnRight();  // Turn right if right side is clear
            Serial.println("Turn right");
            delay(1000);  // Wait for turn to complete
        } else if (robotX > 0 && distance3 < obstacleThreshold) {
            turnLeft();  // Turn left if left side is clear
            Serial.println("Turn left");
            delay(1000);  // Wait for turn to complete
        } else {
            stop();  // Stop if no movement is possible
            Serial.println("stop");
        }
    }

    delay(500);  // Wait before the next movement
}