let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
let lookaheadSteps = 4; // Number of steps to look ahead for conflicts
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(2);

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

  for (let i = 0; i < 4; i++) {
    let car = createCar();
    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(220);

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

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

    if (!car.arrived) {
      car.showDestination(destination);

      if (!car.path || car.path.length === 0) {
        car.findPath(destination, cars);
      } else {
        let conflict = detectConflicts(car);

        if (!conflict) {
          car.executeStep();
          if (car.i === destination.i && car.j === destination.j) {
            car.arrived = true;
            console.log(`Car ${i + 1} has reached its destination.`);
          }
        } else {
          console.log(`Car ${i + 1} is waiting due to conflict.`);
        }
      }
    }

    if (car.arrived) {
      car.show(car.lightColor);
    } else {
      car.show();
    }
  }

  if (cars.every(car => car.arrived)) {
    console.log("All cars have arrived at their destinations.");
    noLoop();
  }
}

function createCar() {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall);
  return new Car(i, j);
}

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) ||
    destinations.some(dest => dest.i === i && dest.j === j)
  );
  return { i, j };
}

function detectConflicts(car) {
  let futurePositions = car.lookaheadPositions(lookaheadSteps);

  for (let otherCar of cars) {
    if (otherCar === car || otherCar.arrived) continue;

    let otherFuturePositions = otherCar.lookaheadPositions(lookaheadSteps);
    for (let step = 0; step < Math.min(futurePositions.length, otherFuturePositions.length); step++) {
      if (
        futurePositions[step].i === otherFuturePositions[step].i &&
        futurePositions[step].j === otherFuturePositions[step].j
      ) {
        if (cars.indexOf(car) < cars.indexOf(otherCar)) {
          return false; // Car can proceed
        } else {
          return true; // Car must wait
        }
      }
    }
  }
  return false; // No conflicts
}

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

    openSet.push(start);
    this.path = [];

    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) {
        let temp = current;
        while (temp.previous) {
          this.path.push([temp.i, temp.j]);
          temp = temp.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) {
          let tempG = current.g + 1;
          let newPath = false;
          if (openSet.includes(neighbor)) {
            if (tempG < neighbor.g) {
              neighbor.g = tempG;
              newPath = true;
            }
          } else {
            neighbor.g = tempG;
            newPath = true;
            openSet.push(neighbor);
          }

          if (newPath) {
            neighbor.h = heuristic(neighbor, end);
            neighbor.f = neighbor.g + neighbor.h;
            neighbor.previous = current;
          }
        }
      }
    }
  };

  this.executeStep = function() {
    if (this.path.length > 0) {
      let nextStep = this.path[0];
      this.i = nextStep[0];
      this.j = nextStep[1];
      this.path.shift();
    }
  };

  this.lookaheadPositions = function(steps) {
    let tempI = this.i;
    let tempJ = this.j;
    let tempPath = [...this.path];
    let futurePositions = [];

    for (let i = 0; i < steps && tempPath.length > 0; i++) {
      let [nextI, nextJ] = tempPath[0];
      futurePositions.push({ i: nextI, j: nextJ });
      tempI = nextI;
      tempJ = nextJ;
      tempPath.shift();
    }
    return futurePositions;
  };
}

function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = true;
  this.f = 0;
  this.g = 0;
  this.h = 0;
  this.previous = undefined;

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

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

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