let cols = 19;
let rows = 13;
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 and set 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); // Set wall for cells not on paths
    }
  }

  // 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 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 car = createUniqueCar();
    let destination = createUniqueDestination(car);
    car.color = colors[i];
    car.lightColor = lightColors[i];
    car.destinationColor = colors[i];
    car.generateOriginalPlan(destination); // Generates initial plan
    cars.push(car);
    destinations.push(destination);
  }
}

function draw() {
  background(255);

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

  allCarsArrived = true;

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

  if (!allCarsArrived) {
    let car = cars[currentCarIndex];
    let destination = destinations[currentCarIndex];

    if (!car.arrived) {
      car.senseEnvironment(cars);
      car.planNextMove(destination, cars);
      car.executeMove(destination);
      
      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;
  }

  for (let car of cars) {
    if (car.arrived) {
      car.show(car.lightColor);
    } else {
      car.show();
    }
  }

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

// Functions to identify intersections, create unique cars, and destinations
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 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);
}

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

function isCarPresent(i, j) {
  return cars.some(car => car.i === i && car.j === j);
}

function isDestinationPresent(i, j) {
  return destinations.some(dest => dest.i === i && dest.j === j);
}

// Car class with sensing, planning, and execution
function Car(startI, startJ) {
  this.i = startI;
  this.j = startJ;
  this.originalPlan = [];
  this.path = [];
  this.color = "red";
  this.lightColor = "lightcoral";
  this.destinationColor = "red";
  this.arrived = false;
  this.senseResults = { nextCell: null, blocked: false, atIntersection: 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.generateOriginalPlan = function(destination) {
    this.findPath(destination);
    console.log(`Car ${cars.indexOf(this) + 1} original plan: ${JSON.stringify(this.path)}`);
    this.originalPlan = [...this.path];
  };

  this.findPath = function(dest) {
    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.senseEnvironment = function(otherCars) {
    if (this.path.length > 0) {
      let nextStep = this.path[0];
      let nextSpot = grid[nextStep[0]][nextStep[1]];
      this.senseResults.nextCell = nextSpot;
      this.senseResults.blocked = this.isBlockedByCar(nextSpot, otherCars);
      this.senseResults.atIntersection = isIntersection(this.i, this.j);
    }
  };

  this.planNextMove = function(dest, otherCars) {
    if (this.senseResults.blocked) {
      if (this.senseResults.atIntersection) {
        console.log(`Car ${cars.indexOf(this) + 1} is at an intersection and rerouting due to blockage.`);
        this.findPath(dest);
      } else {
        console.log(`Car ${cars.indexOf(this) + 1} rerouting due to blockage on road.`);
        this.findPath(dest);
      }
    } else if (!this.senseResults.blocked) {
      console.log(`Car ${cars.indexOf(this) + 1} moving as planned.`);
    }
  };

  this.executeMove = function(dest) {
    if (this.path.length > 0 && !this.senseResults.blocked) {
      let [nextI, nextJ] = this.path.shift();
      this.i = nextI;
      this.j = nextJ;
    }
  };

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

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

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

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

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