let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let intersections = [];
let cars = [];
let destinations = [];
let carColors = ["red", "blue", "green", "yellow"];
let lightColors = ["pink", "lightblue", "lightgreen", "lightyellow"];
let currentCarIndex = 0;

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

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

  // Adding corner walls
  grid[0][0].wall = true;
  grid[0][rows - 1].wall = true;
  grid[cols - 1][0].wall = true;
  grid[cols - 1][rows - 1].wall = true;

  // Identify intersections
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      if (!grid[i][j].wall && isIntersection(i, j)) {
        intersections.push({ i, j });
      }
    }
  }

  // Initialize cars and destinations
  for (let i = 0; i < 4; i++) {
    let carPos, destPos;
    do {
      carPos = getRandomPosition();
      destPos = getRandomPosition(carPos);
    } while (
      cars.some(car => car.pos.i === carPos.i && car.pos.j === carPos.j) ||
      destinations.some(dest => dest.pos.i === destPos.i && dest.pos.j === destPos.j)
    );

    cars.push({ pos: carPos, color: carColors[i], primaryPath: [], reachedDestination: false });
    destinations.push({ pos: destPos, color: carColors[i], lightColor: lightColors[i], reached: false });

    // Find primary paths for each car
    cars[i].primaryPath = [carPos, ...aStarSearch(carPos, destPos)];
  }
}

function draw() {
  background(255);

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

  // Display destinations
  for (let dest of destinations) {
    fill(dest.reached ? dest.lightColor : dest.color);
    noStroke();
    rect(dest.pos.i * w, dest.pos.j * h, w, h);
  }

  // Display cars
  for (let car of cars) {
    if (!car.reachedDestination) {
      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
      stroke(car.color);
      strokeWeight(2);
      noFill();
      beginShape();
      for (let p of car.primaryPath) {
        vertex((p.i + 0.5) * w, (p.j + 0.5) * h);
      }
      endShape();
    }
  }

  // Sequential movement - each car moves one step per turn
  moveCarsSequentially();
}

function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = false;
  this.show = function() {
    if (this.wall) fill(0);
    else fill(255);
    stroke(200);
    rect(this.i * w, this.j * h, w, h);
  };
}

function isIntersection(i, j) {
  let whiteNeighbors = 0;
  if (j > 0 && !grid[i][j - 1].wall) whiteNeighbors++;
  if (j < rows - 1 && !grid[i][j + 1].wall) whiteNeighbors++;
  if (i > 0 && !grid[i - 1][j].wall) whiteNeighbors++;
  if (i < cols - 1 && !grid[i + 1][j].wall) whiteNeighbors++;
  return whiteNeighbors > 2;
}

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

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

  let blockedSet = new Set(blocked.map(b => `${b.i},${b.j}`));

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

    if (current.pos.i === end.i && current.pos.j === end.j) return current.path;

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

    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 || blockedSet.has(key)) continue;

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

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

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

function performSensing(car) {
  let allowedGrids = [];
  let blockedGrids = [];
  let neighbors = getNeighbors(car.pos);

  for (let neighbor of neighbors) {
    let gridCell = grid[neighbor.i][neighbor.j];
    let occupied = cars.some(c => c.pos.i === neighbor.i && c.pos.j === neighbor.j && c.color !== car.color);

    if (gridCell.wall || occupied) {
      blockedGrids.push(neighbor);
    } else {
      allowedGrids.push(neighbor);
    }
  }
  console.log(`${car.color} car sensing:`);
  console.log("Allowed Grids: ", allowedGrids);
  console.log("Blocked Grids: ", blockedGrids);
  return { allowedGrids, blockedGrids };
}

function makeMove(car, destPos) {
  let sensing = performSensing(car);

  if (car.pos.i === destPos.i && car.pos.j === destPos.j) {
    console.log(`${car.color} car has reached its destination`);
    car.reachedDestination = true;

    // Remove car from the game and update destination
    let dest = destinations.find(d => d.pos.i === destPos.i && d.pos.j === destPos.j);
    dest.reached = true;

    // Change the destination grid color to light color and clear the grid
    grid[destPos.i][destPos.j].wall = false;
    dest.lightColor = lightColors[carColors.indexOf(car.color)];

    return;
  }

  let nextGrid = car.primaryPath[1];
  if (sensing.allowedGrids.some(g => g.i === nextGrid.i && g.j === nextGrid.j)) {
    console.log(`${car.color} car decision: Proceed as per the original plan`);
    car.pos = nextGrid;
    car.primaryPath.shift();
    console.log(`Moving to (${nextGrid.i}, ${nextGrid.j}), f: ${heuristic(nextGrid, destPos)}`);
  } else if (sensing.blockedGrids.some(g => g.i === nextGrid.i && g.j === nextGrid.j)) {
    let intersection = intersections.some(inter => inter.i === nextGrid.i && inter.j === nextGrid.j);
    if (intersection) {
      console.log(`${car.color} car decision: Wait and pass the turn to the next car`);
    } else {
      console.log(`${car.color} car decision: Re-route`);
      car.primaryPath = [car.pos, ...aStarSearch(car.pos, destPos, [car.primaryPath[1]])];

      // Move immediately after re-routing to follow the revised path
      car.pos = car.primaryPath[1];
      car.primaryPath.shift();
      console.log(`Moving to (${car.pos.i}, ${car.pos.j}), f: ${heuristic(car.pos, destPos)}`);
    }
  } else {
    console.log("Error: Grid not found in allowed or blocked lists");
  }
}

function moveCarsSequentially() {
  if (cars[currentCarIndex] && !cars[currentCarIndex].reachedDestination) {
    let car = cars[currentCarIndex];
    let dest = destinations.find(d => d.color === car.color);
    makeMove(car, dest.pos);
  }
  currentCarIndex = (currentCarIndex + 1) % cars.length;
}