let cols = 30;
let rows = 20;
let grid = new Array(cols);
let w, h;
let start = [];
let end = [];
let openSet = [];
let closedSet = [];
let paths = [];
let carCount = 4; // Number of agents

let roadImage; // Placeholder for road image
let intersectionImage; // Placeholder for intersection image

// Colors for visualization
let backcolor = [255, 255, 255]; // Background color
let closedcolor = [255, 0, 0];   // Closed set color (red)
let opencolor = [0, 255, 0];     // Open set color (green)
let pathcolor = [0, 0, 255];     // Path color (blue)

function preload() {
  // Load images for road and intersection (custom)
  // Replace these with actual image files
  roadImage = loadImage('/uploads/nivedithavudayagiri/Road-2-way.png');
  buildingImage = loadImage('/uploads/nivedithavudayagiri/New-york-buildings-4.png');
}
let pathFound = []; // Track if the path is found for each car

function setup() {
  createCanvas(900, 600);

  frameRate(2);

  w = width / cols;
  h = height / rows;

  for (let i = 0; i < cols; i++) {
    grid[i] = new Array(rows);
  }

  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      grid[i][j] = new Spot(i, j);
    }
  }

  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      grid[i][j].addNeighbors(grid);
    }
  }

  for (let carIndex = 0; carIndex < carCount; carIndex++) {
    start[carIndex] = grid[Math.floor(random(0, cols))][Math.floor(random(0, rows))];
    end[carIndex] = grid[Math.floor(random(0, cols))][Math.floor(random(0, rows))];
    start[carIndex].wall = false;
    end[carIndex].wall = false;

    openSet[carIndex] = [start[carIndex]];
    closedSet[carIndex] = [];
    paths[carIndex] = [];
    pathFound[carIndex] = false; // Initialize pathFound to false
  }
}

function draw() {
  background(backcolor);
  drawRoadsAndIntersections();

  for (let carIndex = 0; carIndex < carCount; carIndex++) {
    if (pathFound[carIndex]) continue; // Skip processing if path is already found

    if (openSet[carIndex].length > 0) {
      let winner = 0;

      for (let i = 0; i < openSet[carIndex].length; i++) {
        if (openSet[carIndex][i].f < openSet[carIndex][winner].f) {
          winner = i;
        }
      }

      let current = openSet[carIndex][winner];

      if (current === end[carIndex]) {
        console.log("Path found for Agent " + (carIndex + 1));

        paths[carIndex] = reconstructPath(current);

        // Draw the path as a line from start to end
        stroke(random(255), random(255), random(255)); // Random color for each car
        strokeWeight(2);
        for (let i = 0; i < paths[carIndex].length - 1; i++) {
          let startX = paths[carIndex][i].i * w + w / 2;
          let startY = paths[carIndex][i].j * h + h / 2;
          let endX = paths[carIndex][i + 1].i * w + w / 2;
          let endY = paths[carIndex][i + 1].j * h + h / 2;
          line(startX, startY, endX, endY);
        }

        pathFound[carIndex] = true; // Mark path as found for this car
        continue;
      }

      removeFromArray(openSet[carIndex], current);
      closedSet[carIndex].push(current);

      let neighbors = current.neighbors;
      for (let i = 0; i < neighbors.length; i++) {
        let neighbor = neighbors[i];

        if (!closedSet[carIndex].includes(neighbor) && !neighbor.wall) {
          let tempG = current.g + 1;

          let newPath = false;
          if (openSet[carIndex].includes(neighbor)) {
            if (tempG < neighbor.g) {
              neighbor.g = tempG;
              newPath = true;
            }
          } else {
            neighbor.g = tempG;
            newPath = true;
            openSet[carIndex].push(neighbor);
          }

          if (newPath) {
            neighbor.h = heuristic(neighbor, end[carIndex]);
            neighbor.f = neighbor.g + neighbor.h;
            neighbor.previous = current;
          }
        }
      }
    } else {
      console.log("Replanning for Agent " + (carIndex + 1));
      openSet[carIndex] = [start[carIndex]];
      closedSet[carIndex] = [];
    }

    // Draw start and end points
    fill(0, 255, 255);
    ellipse(start[carIndex].i * w + w / 2, start[carIndex].j * h + h / 2, w / 2, h / 2);
    fill(255, 0, 255);
    ellipse(end[carIndex].i * w + w / 2, end[carIndex].j * h + h / 2, w / 2, h / 2);
  }
}
// Reconstructs the path from the end node
function reconstructPath(current) {
    try{
  let path = [];
  let temp = current;
  while (temp) {
    path.push(temp);
    temp = temp.previous;
  }
  return path.reverse(); // Reverse the path for correct order
    }catch{
        console.log('Problem with reconstructing path')
    }
}

// Utility function to remove an element from the array
function removeFromArray(arr, elt) {
  for (let i = arr.length - 1; i >= 0; i--) {
    if (arr[i] === elt) {
      arr.splice(i, 1);
    }
  }
}

// Spot object for each cell in the grid
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.f = 0;
  this.g = 0;
  this.h = 0;
  this.neighbors = [];
  this.previous = undefined;
  this.wall = random(1) < 0.3; // Randomly assign some spots as walls

  this.show = function(col) {
    fill(col);
    noStroke();
    rect(this.i * w, this.j * h, w - 1, h - 1);
  };

  this.addNeighbors = function(grid) {
    let i = this.i;
    let j = this.j;
    if (i < cols - 1) this.neighbors.push(grid[i + 1][j]);
    if (i > 0) this.neighbors.push(grid[i - 1][j]);
    if (j < rows - 1) this.neighbors.push(grid[i][j + 1]);
    if (j > 0) this.neighbors.push(grid[i][j - 1]);
  };
}

// Heuristic function (Manhattan distance)
function heuristic(a, b) {
  return abs(a.i - b.i) + abs(a.j - b.j);
}

// Visualize roads and intersections (placeholders for images)
function drawRoadsAndIntersections() {
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      if (!grid[i][j].wall) {
        image(roadImage, i * w, j * h, w, h);
      } else {
        image(buildingImage, i * w, j * h, w, h);
      }
    }
  }
}