let cols = 13;
let rows = 10;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
const colors = ["red", "blue", "green", "yellow"]; // Colors for the cars

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

  // Create cars and destinations
  for (let i = 0; i < 4; i++) {
    let car = createCar(i); // Create each car
    cars.push(car);

    // Create unique destination for each car
    let destination = createDestination(car);
    destinations.push(destination);

    // Calculate the original path (BFS for valid path)
    let originalPath = findValidPathBFS(car, destination);
    console.log(`Car ${i} original path:`, originalPath); // Log the path
  }
}

function draw() {
  background(220);
  displayGrid();

  // Display cars and destinations
  for (let i = 0; i < cars.length; i++) {
    let car = cars[i];
    let destination = destinations[i];

    // Display the car
    car.show();

    // Display the destination with the car's color
    fill(car.color);
    noStroke();
    rect(destination.i * w, destination.j * h, w, h);
  }
}

// Create and initialize the grid
function initializeGrid() {
  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); // Randomly place walls
    }
  }
  // Place walls at corners
  grid[0][0].wall = true;
  grid[0][rows - 1].wall = true;
  grid[cols - 1][0].wall = true;
  grid[cols - 1][rows - 1].wall = true;
}

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

// Spot object to define each cell on the grid
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = false;

  // Function to display the cell
  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);
  };
}

// Create a car and place it on a random open space
function createCar(carIndex) {
  let i, j;
  // Make sure car is placed on an open spot
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || isOccupied(i, j)); // Ensure the car isn't placed on a wall or already occupied

  let car = new Car(i, j, colors[carIndex]);
  return car;
}

// Check if the spot is already occupied by another car
function isOccupied(i, j) {
  return cars.some(car => car.i === i && car.j === j);
}

// Create a unique destination for each car
function createDestination(car) {
  let i, j;
  // Make sure destination is not on a wall, not on the car's initial position, and not already occupied by another destination
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || (i === car.i && j === car.j) || isDestinationOccupied(i, j));

  return { i, j, color: car.color }; // Set the destination color to match the car
}

// Check if a destination is already occupied by another destination
function isDestinationOccupied(i, j) {
  return destinations.some(dest => dest.i === i && dest.j === j);
}

// Car object to represent each car
function Car(startI, startJ, carColor) {
  this.i = startI;
  this.j = startJ;
  this.color = carColor;

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

// BFS to find the shortest valid path (avoiding walls)
function findValidPathBFS(car, destination) {
  let start = grid[car.i][car.j];
  let end = grid[destination.i][destination.j];
  
  // If start or end is a wall, return an empty path
  if (start.wall || end.wall) {
    return [];
  }
  
  let queue = [];
  let cameFrom = {}; // To reconstruct the path
  let path = [];

  // Enqueue the start cell
  queue.push(start);
  cameFrom[`${start.i},${start.j}`] = null;

  let directions = [
    [0, 1], // Move down
    [0, -1], // Move up
    [1, 0], // Move right
    [-1, 0] // Move left
  ];

  // BFS to find the shortest path
  while (queue.length > 0) {
    let current = queue.shift();

    // If we've reached the destination, reconstruct the path
    if (current === end) {
      let temp = current;
      while (temp !== null) {
        path.unshift(temp);
        temp = cameFrom[`${temp.i},${temp.j}`];
      }
      break;
    }

    // Explore the neighboring cells
    for (let dir of directions) {
      let ni = current.i + dir[0];
      let nj = current.j + dir[1];

      // Check if the new cell is within bounds and not a wall
      if (ni >= 0 && ni < cols && nj >= 0 && nj < rows && !grid[ni][nj].wall) {
        let neighbor = grid[ni][nj];

        // If the neighbor hasn't been visited yet
        if (!( `${neighbor.i},${neighbor.j}` in cameFrom )) {
          queue.push(neighbor);
          cameFrom[`${neighbor.i},${neighbor.j}`] = current;
        }
      }
    }
  }

  // Return the path from start to destination
  return path;
}