let cols = 13;  // Number of columns
let rows = 10;  // Number of rows
let w, h;       // Width and height of each cell
let grid = [];  // Grid array to hold each cell
let cars = [];  // Array to hold multiple cars
let destinations = []; // Array to hold destinations for each car

function setup() {
  createCanvas(900, 600);
  w = width / cols;
  h = height / rows;
  
  frameRate(2); // Slow down motion for visibility

  // Create the grid and apply the wall/path pattern
  for (let i = 0; i < cols; i++) {
    grid[i] = [];
    for (let j = 0; j < rows; j++) {
      grid[i][j] = new Spot(i, j);

      // Define walls and paths
      if (i % 3 === 0 || j % 3 === 0) {
        grid[i][j].wall = false; // Path (white)
      } else {
        grid[i][j].wall = true; // Wall (black)
      }
    }
  }

  // Add black grid blocks at each corner
  grid[0][0].wall = true;                    
  grid[0][rows - 1].wall = true;             
  grid[cols - 1][0].wall = true;             
  grid[cols - 1][rows - 1].wall = true;       

  // Initialize 4 cars with random positions and destinations
  for (let i = 0; i < 4; i++) {
    let car = createCar();
    let destination = createDestination(car);
    car.color = color(random(255), random(255), random(255)); // Assign a unique color to each car
    cars.push(car);
    destinations.push(destination);
  }
}

function draw() {
  background(220);

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

  // Move each car one step at a time and handle path recalculation if blocked
  for (let i = 0; i < cars.length; i++) {
    let car = cars[i];
    let destination = destinations[i];

    if (!car.path || car.path.length === 0) {
      car.findPath(destination, cars); // Calculate path if empty or blocked
    } else {
      car.moveAlongPath(cars); // Move along the path
    }

    car.show();
    car.showDestination(destination);
  }
}

// Function to create a car at a random path position
function createCar() {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall); // Ensure the car is placed on a path
  return new Car(i, j);
}

// Function to create a destination at a random path position
function createDestination(car) {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || (i === car.i && j === car.j)); // Ensure destination is on a path and not the car's position
  return { i, j };
}

// Car class with movement logic
function Car(startI, startJ) {
  this.i = startI;
  this.j = startJ;
  this.path = []; // Path property for storing the calculated path
  this.color = 'red';

  // Display the car
  this.show = function () {
    fill(this.color); 
    noStroke();
    ellipse((this.i + 0.5) * w, (this.j + 0.5) * h, w / 2, h / 2);
  };

  // Display the destination
  this.showDestination = function (dest) {
    fill('blue');
    noStroke();
    rect(dest.i * w, dest.j * h, w, h);
  };

  // Find the path to the destination using A* algorithm
  this.findPath = function(dest, otherCars) {
    // Initialize open and closed sets for A* search
    let openSet = [];
    let closedSet = [];
    let start = grid[this.i][this.j];
    let end = grid[dest.i][dest.j];

    openSet.push(start);
    this.path = []; // Reset path each time

    while (openSet.length > 0) {
      let lowestIndex = 0;
      for (let i = 1; i < openSet.length; i++) {
        if (openSet[i].f < openSet[lowestIndex].f) {
          lowestIndex = i;
        }
      }
      
      let current = openSet[lowestIndex];

      if (current === end) {
        let temp = current;
        while (temp.previous) {
          this.path.push([temp.i, temp.j]);
          temp = temp.previous;
        }
        this.path.reverse();
        return;
      }

      openSet.splice(lowestIndex, 1);
      closedSet.push(current);

      let neighbors = current.getNeighbors();
      for (let neighbor of neighbors) {
        if (!closedSet.includes(neighbor) && !neighbor.wall && !this.isBlockedByCar(neighbor, otherCars)) {
          let tempG = current.g + 1;
          let newPath = false;
          if (openSet.includes(neighbor)) {
            if (tempG < neighbor.g) {
              neighbor.g = tempG;
              newPath = true;
            }
          } else {
            neighbor.g = tempG;
            newPath = true;
            openSet.push(neighbor);
          }

          if (newPath) {
            neighbor.h = heuristic(neighbor, end);
            neighbor.f = neighbor.g + neighbor.h;
            neighbor.previous = current;
          }
        }
      }
    }
  };

  // Move along the calculated path, if not blocked by another car
  this.moveAlongPath = function(otherCars) {
    if (this.path.length > 0) {
      let nextStep = this.path[0];
      
      // Check if the next step is blocked by another car
      if (!this.isBlockedByCar(grid[nextStep[0]][nextStep[1]], otherCars)) {
        this.i = nextStep[0];
        this.j = nextStep[1];
        this.path.shift(); // Remove the first step
      } else {
        this.path = []; // Recalculate path if blocked
      }
    }
  };

  // Check if a spot is blocked by another car
  this.isBlockedByCar = function(spot, otherCars) {
    for (let car of otherCars) {
      if (car !== this && car.i === spot.i && car.j === spot.j) {
        return true;
      }
    }
    return false;
  };
}

// Spot class representing each cell in the grid
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = true; 
  this.f = 0;
  this.g = 0;
  this.h = 0;
  this.previous = undefined;

  this.show = function() {
    if (this.wall) {
      fill(0); // Black for walls
    } else {
      fill(255); // White for paths
    }
    stroke(0);
    rect(this.i * w, this.j * h, w, h);
  };

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

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