let cols = 13;
let rows = 10;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
let allCarsArrived = false;
let currentCarIndex = 0; // Track the current car to move

// Distinct colors for each car and destination
const colors = ["red", "blue", "green", "yellow"];

function setup() {
  createCanvas(900, 600);
  w = width / cols;
  h = height / rows;
  frameRate(2); // Slow down to visualize moves

  // Initialize the 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); // Make walls and paths
    }
  }

  // Place walls at corners
  grid[0][0].wall = true;
  grid[0][rows - 1].wall = true;
  grid[cols - 1][0].wall = true;
  grid[cols - 1][rows - 1].wall = true;

  // Create cars and their destinations
  for (let i = 0; i < 4; i++) {
    let car = createCar();
    let destination = createDestination(car);
    car.color = colors[i];
    car.destinationColor = colors[i]; // Color matching for destination
    cars.push(car);
    destinations.push(destination);
  }
}

function draw() {
  background(220);

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

  allCarsArrived = true;

  // Show destinations and update arrival status
  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 only the current car in sequence
  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} is waiting for path to clear.`);
        }
      }

      if (car.i === destination.i && car.j === destination.j) {
        car.arrived = true;
      }
    }

    // Move to the next car's turn
    currentCarIndex = (currentCarIndex + 1) % cars.length;
  }

  // Draw all cars that haven't arrived
  for (let car of cars) {
    if (!car.arrived) {
      car.show();
    }
  }

  // Stop when all cars arrive at their destinations
  if (allCarsArrived) {
    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 createDestination(car) {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || (i === car.i && j === car.j));
  return { i, j };
}

function Car(startI, startJ) {
  this.i = startI;
  this.j = startJ;
  this.path = [];
  this.color = "red"; // default color, will be set in setup()
  this.destinationColor = "red"; // matching color for destination
  this.arrived = false;

  this.show = function () {
    fill(this.color);
    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 && !this.isBlockedByCar(neighbor, otherCars)) {
          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.moveAlongPath = function(otherCars) {
    if (this.path.length > 0) {
      let nextStep = this.path[0];
      let nextSpot = grid[nextStep[0]][nextStep[1]];

      if (!this.isBlockedByCar(nextSpot, otherCars) || this.isDestination(nextSpot)) {
        this.i = nextStep[0];
        this.j = nextStep[1];
        this.path.shift();
        return true; // Moved successfully
      } else {
        console.log(`Car at (${this.i}, ${this.j}) is blocked at (${nextStep[0]}, ${nextStep[1]})`);
        return false; // Path is blocked
      }
    }
    return false; // No path to move
  };

  this.isBlockedByCar = function(spot, otherCars) {
    for (let car of otherCars) {
      if (car !== this && !car.arrived && car.i === spot.i && car.j === spot.j) {
        return true;
      }
    }
    return false;
  };

  this.isDestination = function(spot) {
    for (let i = 0; i < destinations.length; i++) {
      if (spot.i === destinations[i].i && spot.j === destinations[i].j && cars[i] === this) {
        return true;
      }
    }
    return false;
  };
}

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