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 using A* without avoiding walls
    let originalPath = findShortestPathAStar(car, destination);
    car.path = originalPath; // Store path in car object
    console.log(`Car ${i} original path:`, originalPath.map(p => `(${p.i},${p.j})`));
  }
}

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

  // Display cars, destinations, and paths
  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);

    // Draw the planned path from car to destination
    stroke(car.color);
    strokeWeight(2);
    noFill();
    beginShape();
    for (let pos of car.path) {
      vertex((pos.i + 0.5) * w, (pos.j + 0.5) * h);
    }
    endShape();
  }
}

// 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;
  this.path = []; // Placeholder for path

  // 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);
  };
}

// A* to find the shortest path (can pass through black grids for now)
function findShortestPathAStar(car, destination) {
  let start = grid[car.i][car.j];
  let end = grid[destination.i][destination.j];

  // Return empty path if start or end is out of bounds (optional safeguard)
  if (!start || !end) {
    return [];
  }

  let openSet = [];
  let closedSet = new Set();
  let cameFrom = {};

  // Add the start node to the open set
  openSet.push({
    node: start,
    g: 0,
    h: manhattanDistance(start.i, start.j, end.i, end.j),
    f: 0
  });
  cameFrom[`${start.i},${start.j}`] = null;

  // Directions for horizontal and vertical movement only
  let directions = [
    [0, 1],   // down
    [0, -1],  // up
    [1, 0],   // right
    [-1, 0]   // left
  ];

  while (openSet.length > 0) {
    // Sort openSet by lowest `f` value (A* priority queue)
    openSet.sort((a, b) => a.f - b.f);
    let current = openSet.shift();
    let currentNode = current.node;

    // If destination is reached, reconstruct the path
    if (currentNode === end) {
      let path = [];
      while (currentNode !== null) {
        path.unshift(currentNode);
        currentNode = cameFrom[`${currentNode.i},${currentNode.j}`];
      }
      return path;
    }

    closedSet.add(`${currentNode.i},${currentNode.j}`);

    // Explore neighbors
    for (let dir of directions) {
      let ni = currentNode.i + dir[0];
      let nj = currentNode.j + dir[1];

      // Check if the neighbor is within bounds
      if (ni >= 0 && ni < cols && nj >= 0 && nj < rows) {
        let neighbor = grid[ni][nj];
        let g = current.g + 1;  // Cost from start to neighbor
        let h = manhattanDistance(ni, nj, end.i, end.j);  // Heuristic estimate
        let f = g + h;

        // If neighbor is not in closedSet or openSet with lower `f`, add/update it
        let existing = openSet.find(n => n.node.i === ni && n.node.j === nj);
        if (!existing || existing.f > f) {
          openSet.push({ node: neighbor, g, h, f });
          cameFrom[`${neighbor.i},${neighbor.j}`] = currentNode;
        }
      }
    }
  }

  // If no path is found, return an empty path
  return [];
}

// Manhattan distance as the heuristic
function manhattanDistance(x1, y1, x2, y2) {
  return Math.abs(x1 - x2) + Math.abs(y1 - y2);
}