let cols = 13;
let rows = 10;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
let openSet = [];
let closedSet = [];

function setup() {
  createCanvas(900, 600);
  w = width / cols;
  h = height / rows;

  // Initialize the grid and set walls, intersections, and paths
  for (let i = 0; i < cols; i++) {
    grid[i] = [];
    for (let j = 0; j < rows; j++) {
      grid[i][j] = new Spot(i, j);
      grid[i][j].wall = !(i % 3 === 0 || j % 3 === 0); // Wall setup

      if (!grid[i][j].wall) {
        whiteGrids.push({ i, j });
      }
    }
  }

  // Initialize cars and their destinations
  for (let i = 0; i < 4; i++) {
    let start = getRandomWhiteGrid();
    let end = getRandomWhiteGrid();
    cars.push(new Car(start.i, start.j, end.i, end.j, color(random(255), random(255), random(255))));
    destinations.push(end); // To store the target for each car
  }
}

// Utility function to get random start and destination points
function getRandomWhiteGrid() {
  return whiteGrids[int(random(whiteGrids.length))];
}
class Car {
  constructor(x, y, destX, destY, col) {
    this.x = x;
    this.y = y;
    this.destX = destX;
    this.destY = destY;
    this.color = col;
    this.path = [];
    this.calculatePath(); // Initial path calculation
  }

  calculatePath() {
    // Perform A* search to find the path from current position to destination
    this.path = aStar(grid[this.x][this.y], grid[this.destX][this.destY]);
  }

  update() {
    if (this.path.length > 0) {
      // Move one step along the path
      let next = this.path[0];
      this.x = next.i;
      this.y = next.j;
      this.path.shift(); // Remove the step taken

      // Check for blockages
      if (isBlocked(this.x, this.y)) {
        console.log(`Car blocked at (${this.x},${this.y}). Recalculating path.`);
        this.calculatePath();
      }
    } else {
      console.log("Destination reached!");
    }
  }

  display() {
    fill(this.color);
    ellipse(this.x * w + w / 2, this.y * h + h / 2, w * 0.8, h * 0.8); // Car shape
    fill(this.color);
    rect(this.destX * w, this.destY * h, w * 0.8, h * 0.8); // Destination shape
  }
}
function aStar(start, end) {
  openSet = [start];
  closedSet = [];
  start.g = 0;
  start.h = heuristic(start, end);
  start.f = start.g + start.h;

  while (openSet.length > 0) {
    let current = openSet.reduce((a, b) => (a.f < b.f ? a : b)); // Lowest f value
    if (current === end) return reconstructPath(current);

    removeFromArray(openSet, current);
    closedSet.push(current);

    for (let neighbor of current.neighbors) {
      if (closedSet.includes(neighbor) || neighbor.wall) continue;

      let tentativeG = current.g + gfn(current, neighbor);

      if (!openSet.includes(neighbor) || tentativeG < neighbor.g) {
        neighbor.g = tentativeG;
        neighbor.h = heuristic(neighbor, end);
        neighbor.f = neighbor.g + neighbor.h;
        neighbor.previous = current;

        if (!openSet.includes(neighbor)) openSet.push(neighbor);
      }
    }
  }
  return [];
}

// Reconstruct the path from end to start
function reconstructPath(current) {
  let path = [];
  while (current) {
    path.push(current);
    current = current.previous;
  }
  return path.reverse();
}
function draw() {
  background(255);

  // Draw the grid
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      grid[i][j].show();
    }
  }

  // Update and display each car
  for (let car of cars) {
    car.update();
    car.display();
  }
}