// Fix for p5.js AudioContext warning  
function userStartAudio() {  
  getAudioContext().resume();  
}  

// Constants and variables  
const numVehicles = 4;  
const gridMatrix = [];  
const vehicleColors = ['red', 'green', 'blue', 'yellow'];  
const vehicleArray = [];  
let numCols, numRows;  
let cellWidth, cellHeight;  
const backColor = 'black';  
const wallColor = 'gray';  

// Cell class to represent each cell in the grid  
class Cell {  
  constructor(i, j) {  
    this.i = i;  
    this.j = j;  
    this.isBarrier = random(1) < 0.3; // Randomly create barriers (walls)  
    this.gScore = 0;  
    this.hScore = 0;  
    this.fScore = 0;  
    this.neighbors = [];  
    this.previous = undefined;  
  }  

  // Display the cell as a rectangle or wall  
  show() {  
    if (this.isBarrier) {  
      fill(wallColor);  
    } else {  
      fill(backColor);  
    }  
    noStroke();  
    rect(this.i * cellWidth, this.j * cellHeight, cellWidth, cellHeight);  
  }  

  // Add adjacent (neighboring) cells to the current cell  
  addAdjacent(gridMatrix) {  
    const { i, j } = this;  
    if (i < numCols - 1) this.neighbors.push(gridMatrix[i + 1][j]);  
    if (i > 0) this.neighbors.push(gridMatrix[i - 1][j]);  
    if (j < numRows - 1) this.neighbors.push(gridMatrix[i][j + 1]);  
    if (j > 0) this.neighbors.push(gridMatrix[i][j - 1]);  
  }  
}  

// Vehicle class to represent a moving vehicle  
class Vehicle {  
  constructor(startCell, endCell, color) {  
    this.startCell = startCell;  
    this.endCell = endCell;  
    this.color = color;  
    this.path = [];  
    this.currentCell = startCell;  
    this.calculatePath();  
  }  

  // A* pathfinding algorithm to calculate the path  
  calculatePath() {  
    let openSet = [this.startCell];  
    let closedSet = [];  
    this.path = [];  

    while (openSet.length > 0) {  
      let lowestIndex = openSet.reduce((best, cell, idx) => cell.fScore < openSet[best].fScore ? idx : best, 0);  
      let currentCell = openSet[lowestIndex];  

      if (currentCell === this.endCell) {  
        let temp = currentCell;  
        this.path = [];  
        while (temp) {  
          this.path.push(temp);  
          temp = temp.previous;  
        }  
        this.path.reverse(); // Get the path from start to end  
        return;  
      }  

      openSet.splice(lowestIndex, 1);  
      closedSet.push(currentCell);  

      currentCell.neighbors.forEach(neighbor => {  
        if (!closedSet.includes(neighbor) && !neighbor.isBarrier) {  
          let tempG = currentCell.gScore + 1;  

          if (!openSet.includes(neighbor)) {  
            openSet.push(neighbor);  
          } else if (tempG >= neighbor.gScore) {  
            return; // Not a better path  
          }  

          // Update the scores  
          neighbor.gScore = tempG;  
          neighbor.hScore = this.heuristic(neighbor, this.endCell);  
          neighbor.fScore = neighbor.gScore + neighbor.hScore;  
          neighbor.previous = currentCell;  
        }  
      });  
    }  
  }  

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

  // Move the vehicle to the next position in its path  
  move() {  
    if (this.path.length > 0) {  
      this.currentCell = this.path[0];  
      this.path.shift(); // Remove the first cell as it's now the current  
    }  
  }  

  // Draw the path of the vehicle  
  drawPath() {  
    noFill();  
    stroke(this.color);  
    strokeWeight(3);  
    beginShape();  
    this.path.forEach(cell => vertex(cell.i * cellWidth + cellWidth / 2, cell.j * cellHeight + cellHeight / 2));  
    endShape();  
  }  

  // Display the vehicle's current position and destination  
  show() {  
    // Display destination cell as a colored triangle  
    fill(this.color);  
    noStroke();  
    triangle(  
      this.endCell.i * cellWidth + cellWidth / 2, this.endCell.j * cellHeight + cellHeight / 2,  
      this.endCell.i * cellWidth, this.endCell.j * cellHeight,  
      this.endCell.i * cellWidth + cellWidth, this.endCell.j * cellHeight + cellHeight  
    );  

    // Display vehicle's current position as a colored rectangle  
    fill(this.color);  
    noStroke();  
    rect(this.currentCell.i * cellWidth, this.currentCell.j * cellHeight, cellWidth, cellHeight);  
  }  
}  

function setup() {  
  createCanvas(800, 500);  
  userStartAudio(); // Ensure audio context resumes after user interaction  

  numCols = floor(width / 30); // Grid columns  
  numRows = floor(height / 30); // Grid rows  
  cellWidth = width / numCols;  // Width of each cell  
  cellHeight = height / numRows; // Height of each cell  

  // Create the grid  
  for (let i = 0; i < numCols; i++) {  
    gridMatrix[i] = new Array(numRows);  
    for (let j = 0; j < numRows; j++) {  
      gridMatrix[i][j] = new Cell(i, j);  
    }  
  }  

  // Add adjacent cells to each cell in the grid  
  for (let i = 0; i < numCols; i++) {  
    for (let j = 0; j < numRows; j++) {  
      gridMatrix[i][j].addAdjacent(gridMatrix);  
    }  
  }  

  // Create vehicles with random start and end positions  
  for (let i = 0; i < numVehicles; i++) {  
    let startCell, endCell;  
    do {  
      startCell = gridMatrix[floor(random(numCols))][floor(random(numRows))];  
    } while (startCell.isBarrier);  
    
    do {  
      endCell = gridMatrix[floor(random(numCols))][floor(random(numRows))];  
    } while (endCell.isBarrier || endCell === startCell);  

    let color = vehicleColors[i % vehicleColors.length];  
    vehicleArray.push(new Vehicle(startCell, endCell, color));  
  }  
}  

function draw() {  
  background(backColor);  

  // Display the grid  
  gridMatrix.flat().forEach(cell => cell.show());  

  // Update and display each vehicle  
  vehicleArray.forEach(vehicle => {  
    vehicle.drawPath();  
    vehicle.move();  
    vehicle.show();  
  });  
}