let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
let carColors = ["red", "blue", "green", "yellow"];

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

  // Initialize grid and walls
  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);
    }
  }

  // Initialize cars and destinations with unique positions
  for (let i = 0; i < 4; i++) {
    let carPos = getRandomPosition();
    let destPos = getRandomPosition(carPos);
    cars.push({ pos: carPos, color: carColors[i], path: [] });
    destinations.push({ pos: destPos, color: carColors[i] });
    
    // Compute the path for each car using A* and store it
    cars[i].path = [carPos, ...aStarSearch(carPos, destPos)];
    console.log(`Path for car ${i + 1} (${carColors[i]}):`, cars[i].path);
  }
}

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

  // Draw destinations as squares
  for (let dest of destinations) {
    fill(dest.color);
    noStroke();
    rect(dest.pos.i * w, dest.pos.j * h, w, h);
  }

  // Draw cars as circles and show their paths
  for (let car of cars) {
    fill(car.color);
    noStroke();
    ellipse((car.pos.i + 0.5) * w, (car.pos.j + 0.5) * h, w * 0.6, h * 0.6);
    
    // Draw path lines from car to destination
    stroke(car.color);
    strokeWeight(2);
    noFill();
    beginShape();
    for (let p of car.path) {
      vertex((p.i + 0.5) * w, (p.j + 0.5) * h);
    }
    endShape();
  }
}

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

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

// Get random position on a path cell, ensuring it doesn’t overlap with given position
function getRandomPosition(exclude = null) {
  let pos;
  do {
    let i = int(random(cols));
    let j = int(random(rows));
    pos = { i, j };
  } while (grid[pos.i][pos.j].wall || (exclude && pos.i === exclude.i && pos.j === exclude.j));
  return pos;
}

// A* Search Algorithm
function aStarSearch(start, end) {
  let openSet = [];
  let closedSet = new Set();
  openSet.push({ pos: start, path: [], g: 0, f: heuristic(start, end) });

  while (openSet.length > 0) {
    // Get node with lowest f score
    let current = openSet.reduce((a, b) => (a.f < b.f ? a : b));
    openSet = openSet.filter(node => node !== current);

    // Return path if destination reached
    if (current.pos.i === end.i && current.pos.j === end.j) {
      return current.path;
    }

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

    // Check neighbors
    let neighbors = getNeighbors(current.pos);
    for (let neighbor of neighbors) {
      let key = `${neighbor.i},${neighbor.j}`;
      if (closedSet.has(key) || grid[neighbor.i][neighbor.j].wall) continue;

      let g = current.g + 1;
      let f = g + heuristic(neighbor, end);
      openSet.push({ pos: neighbor, path: [...current.path, neighbor], g, f });
    }
  }
  return []; // No path found
}

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

// Get valid neighbors (no diagonal, only horizontal/vertical)
function getNeighbors(pos) {
  let neighbors = [];
  let { i, j } = pos;

  if (i > 0) neighbors.push({ i: i - 1, j });
  if (i < cols - 1) neighbors.push({ i: i + 1, j });
  if (j > 0) neighbors.push({ i, j: j - 1 });
  if (j < rows - 1) neighbors.push({ i, j: j + 1 });

  return neighbors;
}