let cols = 10;
let rows = 7;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
let intersections = [];
let allCarsArrived = false;
let currentCarIndex = 0;

const colors = ["red", "blue", "green", "yellow"];
const lightColors = ["lightcoral", "lightblue", "lightgreen", "lightyellow"];

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

  // Initialize grid with walls and paths
  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); // Set wall for cells not on paths
    }
  }

  // Set specific corner cells as walls for layout consistency
  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 and save all 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 with unique positions
  for (let i = 0; i < 4; i++) {
    let car = createUniqueCar();
    let destination = createUniqueDestination(car);
    car.color = colors[i];
    car.lightColor = lightColors[i];
    car.destinationColor = colors[i];
    cars.push(car);
    destinations.push(destination);
  }
}

function draw() {
  background(255);

  // Draw grid, walls, and intersections
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      grid[i][j].show();
    }
  }

  allCarsArrived = true;

  // Draw each car’s destination
  for (let i = 0; i < cars.length; i++) {
    let car = cars[i];
    let destination = destinations[i];
    
    if (!car.arrived) {
      allCarsArrived = false;
      car.showDestination(destination);
    }
  }

  // Move cars in sequence until all cars reach destinations
  if (!allCarsArrived) {
    let car = cars[currentCarIndex];
    let destination = destinations[currentCarIndex];

    if (!car.arrived) {
      if (!car.path || car.path.length === 0) {
        car.findPath(destination, cars);
      } else {
        let moved = car.moveAlongPath(cars);
        if (!moved) {
          console.log(`Car ${currentCarIndex + 1} cannot move due to blockage.`);
        }
      }

      if (car.i === destination.i && car.j === destination.j) {
        car.arrived = true;
        console.log(`Car ${currentCarIndex + 1} has reached its destination.`);
      }
    }

    currentCarIndex = (currentCarIndex + 1) % cars.length;
  }

  // Draw cars
  for (let car of cars) {
    car.show(car.arrived ? car.lightColor : car.color);
  }

  if (allCarsArrived) {
    console.log("All cars have arrived at their destinations.");
    noLoop();
  }
}

// Check if a cell is an intersection
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;
}

// Create unique car with non-overlapping initial positions
function createUniqueCar() {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || isCarPresent(i, j));
  return new Car(i, j);
}

// Create unique destination for each car
function createUniqueDestination(car) {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || (i === car.i && j === car.j) || isDestinationPresent(i, j));
  return { i, j };
}

// Helper to check if car is at given coordinates
function isCarPresent(i, j) {
  return cars.some(car => car.i === i && car.j === j);
}

// Helper to check if a destination is at given coordinates
function isDestinationPresent(i, j) {
  return destinations.some(dest => dest.i === i && dest.j === j);
}

// Car class
function Car(startI, startJ) {
  this.i = startI;
  this.j = startJ;
  this.path = [];
  this.color = "red";
  this.lightColor = "lightcoral";
  this.destinationColor = "red";
  this.arrived = false;

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

  this.showDestination = function (dest) {
    fill(this.destinationColor);
    noStroke();
    rect(dest.i * w, dest.j * h, w, h);
  };

  this.findPath = function(dest, otherCars) {
    let openSet = [];
    let closedSet = [];
    let start = grid[this.i][this.j];
    let end = grid[dest.i][dest.j];

    start.g = 0;
    start.h = heuristic(start, end);
    start.f = start.g + start.h;
    openSet.push(start);

    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) {
        this.path = [];
        while (current.previous) {
          this.path.push([current.i, current.j]);
          current = current.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)) continue;

        let tempG = current.g + 1;

        if (!openSet.includes(neighbor) || tempG < neighbor.g) {
          neighbor.g = tempG;
          neighbor.h = heuristic(neighbor, end);
          neighbor.f = neighbor.g + neighbor.h;
          neighbor.previous = current;

          if (!openSet.includes(neighbor)) openSet.push(neighbor);
        }
      }
    }
    this.path = [];
  };

  this.moveAlongPath = function(otherCars) {
    if (this.path.length > 0) {
      let [nextI, nextJ] = this.path[0];
      let nextSpot = grid[nextI][nextJ];

      if (this.isBlockedByCar(nextSpot, otherCars) || nextSpot.wall) {
        this.reRoute(destinations[currentCarIndex], otherCars);
        return false;
      } 

      this.i = nextI;
      this.j = nextJ;
      this.path.shift();
      return true;

    } else {
      this.reRoute(destinations[currentCarIndex], otherCars);
      return false;
    }
  };

  this.isBlockedByCar = function(spot, otherCars) {
    return otherCars.some(car => car !== this && !car.arrived && car.i === spot.i && car.j === spot.j);
  };

  this.isIntersection = function() {
    return intersections.some(intersection => intersection.i === this.i && intersection.j === this.j);
  };

  this.isDestination = function(spot) {
    return spot.i === destinations[currentCarIndex].i && spot.j === destinations[currentCarIndex].j;
  };

  this.reRoute = function(dest, otherCars) {
    this.findPath(dest, otherCars);
  };
}

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

// Spot (grid cell) class
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.f = 0;
  this.g = 0;
  this.h = 0;
  this.wall = false;
  this.previous = undefined;

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

  this.show = function() {
    fill(this.wall ? 0 : 255);
    stroke(0);
    rect(this.i * w, this.j * h, w, h);
  };
}