let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let cars = [];

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

  // Initialize the 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);
    }
  }

  // Initialize cars with random positions and destinations
  initializeCars();
}

function draw() {
  background(255);

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

  // Draw cars and their paths
  for (let car of cars) {
    car.show();
    if (car.destination) {
      let path = aStar(car.i, car.j, car.destination.i, car.destination.j);
      console.log(`Car at (${car.i}, ${car.j}) to Destination (${car.destination.i}, ${car.destination.j}):`, path);
      drawPath(path, car.color);
    }
  }
}

// Define the Spot object to represent each cell in the grid
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = false;

  this.show = function() {
    fill(this.wall ? 0 : 255); // Wall - Black, Path - White
    stroke(200);
    rect(this.i * w, this.j * h, w, h);
  };
}

// Car class to represent each car
class Car {
  constructor(i, j, color) {
    this.i = i; // Current grid position
    this.j = j;
    this.color = color; // Car color
    this.destination = null; // Destination grid position
  }

  setDestination(destI, destJ) {
    this.destination = { i: destI, j: destJ };
  }

  show() {
    fill(this.color);
    ellipse(this.i * w + w / 2, this.j * h + h / 2, w * 0.6); // Draw car
    if (this.destination) {
      fill(this.color);
      rect(this.destination.i * w + w / 4, this.destination.j * h + h / 4, w / 2, h / 2); // Draw destination
    }
  }
}

// Initialize cars with unique colors and random positions
function initializeCars() {
  let uniqueColors = ['red', 'blue', 'green', 'orange'];
  for (let color of uniqueColors) {
    let startPos, destPos;

    // Get valid random start position
    do {
      startPos = random(whiteGrids());
    } while (cars.some(car => car.i === startPos.i && car.j === startPos.j));

    // Get valid random destination position
    do {
      destPos = random(whiteGrids());
    } while ((destPos.i === startPos.i && destPos.j === startPos.j) || 
              cars.some(car => car.destination && car.destination.i === destPos.i && car.destination.j === destPos.j));

    let car = new Car(startPos.i, startPos.j, color);
    car.setDestination(destPos.i, destPos.j);
    cars.push(car);
  }
}

// A* Pathfinding Algorithm
function aStar(startX, startY, targetX, targetY) {
  let openSet = [];
  let closedSet = [];
  let start = grid[startX][startY];
  let target = grid[targetX][targetY];

  openSet.push(start);
  start.gScore = 0;
  start.fScore = heuristic(start, target);

  while (openSet.length > 0) {
    // Find the node with the lowest fScore
    let lowestIndex = 0;
    for (let i = 0; i < openSet.length; i++) {
      if (openSet[i].fScore < openSet[lowestIndex].fScore) {
        lowestIndex = i;
      }
    }

    let current = openSet[lowestIndex];

    // Check if we reached the target
    if (current === target) {
      return reconstructPath(current);
    }

    // Move current from openSet to closedSet
    openSet.splice(lowestIndex, 1);
    closedSet.push(current);

    // Get neighbors
    let neighbors = getNeighbors(current);
    for (let neighbor of neighbors) {
      if (closedSet.includes(neighbor)) {
        continue; // Ignore the neighbor which is already evaluated
      }

      // The distance from start to the neighbor
      let tentativeGScore = current.gScore + 1;

      if (!openSet.includes(neighbor)) {
        openSet.push(neighbor); // Discover a new node
      } else if (tentativeGScore >= neighbor.gScore) {
        continue; // This is not a better path
      }

      // This path is the best until now. Record it!
      neighbor.cameFrom = current;
      neighbor.gScore = tentativeGScore;
      neighbor.fScore = neighbor.gScore + heuristic(neighbor, target);
    }
  }

  return []; // Return an empty path if there is no path
}

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

// Get neighbors of the current node
function getNeighbors(node) {
  let neighbors = [];
  let directions = [
    { i: 0, j: 1 },  // Down
    { i: 1, j: 0 },  // Right
    { i: 0, j: -1 }, // Up
    { i: -1, j: 0 }  // Left
  ];

  for (let dir of directions) {
    let newI = node.i + dir.i;
    let newJ = node.j + dir.j;
    if (newI >= 0 && newI < cols && newJ >= 0 && newJ < rows && !grid[newI][newJ].wall) {
      neighbors.push(grid[newI][newJ]);
    }
  }
  return neighbors;
}

// Reconstruct the path from the target to the start
function reconstructPath(current) {
  let totalPath = [];
  while (current.cameFrom) {
    totalPath.push({ i: current.i, j: current.j });
    current = current.cameFrom;
  }
  totalPath.reverse(); // Return the path in the correct order
  return totalPath; // Return the path
}

// Function to draw the path
function drawPath(path, color) {
  stroke(color);
  strokeWeight(3);
  for (let i = 0; i < path.length - 1; i++) {
    let startX = path[i].i * w + w / 2;
    let startY = path[i].j * h + h / 2;
    let endX = path[i + 1].i * w + w / 2;
    let endY = path[i + 1].j * h + h / 2;
    line(startX, startY, endX, endY); // Draw line between path points
  }
}

// Function to get white grid positions
function whiteGrids() {
  let validGrids = [];
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      if (!grid[i][j].wall) {
        validGrids.push({ i, j });
      }
    }
  }
  return validGrids;
}