//Open Console to see the status of each car!!

// Configurations
const diagonal = false; // No diagonal movement for simplicity
const cw = 600; // Canvas width
const ch = 600; // Canvas height
const cols = 20; // Number of columns
const rows = 20; // Number of rows
const wallAmount = 0.2; // Percentage of grid cells that are walls
const numCars = 4; // Number of cars in the simulation
const waitLimit = 20; // Number of frames a car will wait before returning to start

// Colors
const backcolor = 'white';
const wallcolor = 'black';
const pathcolor = 'blue';
const carColors = ['red', 'green', 'orange', 'purple']; // Colors for each car

// Grid and Cars
let grid = [];
let cars = [];

// Cell dimensions
let w, h;

// A* Algorithm variables
let openSet = [];
let closedSet = [];

// Spot Class for each cell in the grid
function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.f = 0;
  this.g = 0;
  this.h = 0;
  this.neighbors = [];
  this.previous = undefined;
  this.wall = random(1) < wallAmount;

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

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

// Car Class
function Car(start, end, color) {
  this.start = start;
  this.end = end;
  this.current = start;
  this.path = [];
  this.color = color;
  this.trail = [];
  this.state = 'SEARCHING';
  this.waitCounter = 0;
  this.blockedPaths = new Set(); // Stores previously blocked spots as Set for unique values
}

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

// A* pathfinding function that considers blocked paths
function aStar(start, end, blockedPaths) {
  openSet = [start];
  closedSet = [];
  start.g = 0;
  start.f = heuristic(start, end);
  start.previous = undefined;

  while (openSet.length > 0) {
    let winner = 0;
    for (let i = 0; i < openSet.length; i++) {
      if (openSet[i].f < openSet[winner].f) winner = i;
    }

    let current = openSet[winner];
    if (current === end) {
      let path = [];
      let temp = current;
      path.push(temp);
      while (temp.previous) {
        path.push(temp.previous);
        temp = temp.previous;
      }
      return path.reverse();
    }

    openSet.splice(winner, 1);
    closedSet.push(current);

    let neighbors = current.neighbors;
    for (let neighbor of neighbors) {
      if (!closedSet.includes(neighbor) && !neighbor.wall && !blockedPaths.has(neighbor)) {
        let tempG = current.g + 1;
        if (!openSet.includes(neighbor)) {
          openSet.push(neighbor);
        } else if (tempG >= neighbor.g) {
          continue;
        }

        neighbor.g = tempG;
        neighbor.h = heuristic(neighbor, end);
        neighbor.f = neighbor.g + neighbor.h;
        neighbor.previous = current;
      }
    }
  }
  return [];
}

// Check if car is in proximity of the destination
function isNearDestination(car) {
  const { i, j } = car.current;
  const { i: ei, j: ej } = car.end;
  return (
    (i === ei && abs(j - ej) === 1) || // same column, adjacent row
    (j === ej && abs(i - ei) === 1)    // same row, adjacent column
  );
}

// Check if another car is blocking the next position
function isBlocked(car, next) {
  for (let otherCar of cars) {
    if (otherCar !== car && otherCar.current === next) {
      return true;
    }
  }
  return false;
}

// Setup the grid and cars
function setup() {
  createCanvas(cw, ch);
  w = width / cols;
  h = height / rows;

  // Slower frame rate for easier tracking
  frameRate(5);

  // Initialize the grid
  for (let i = 0; i < cols; i++) {
    grid[i] = new Array(rows);
    for (let j = 0; j < rows; j++) {
      grid[i][j] = new Spot(i, j);
    }
  }

  // Add neighbors for each cell
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      grid[i][j].addNeighbors(grid);
    }
  }

  // Initialize cars with random start and end positions
  for (let i = 0; i < numCars; i++) {
    let start = grid[floor(random(cols))][floor(random(rows))];
    let end = grid[floor(random(cols))][floor(random(rows))];
    while (end.wall || end === start) {
      end = grid[floor(random(cols))][floor(random(rows))];
    }
    start.wall = false;
    end.wall = false;
    cars.push(new Car(start, end, carColors[i % carColors.length]));
  }
}

// Draw function
function draw() {
  background(backcolor);
  
  // Draw grid border
  stroke(0);
  strokeWeight(4);
  noFill();
  rect(0, 0, cols * w, rows * h);

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

  // Draw each car, its path, and trail
  for (let [index, car] of cars.entries()) {
    // Calculate path if not already done and if the car is searching
    if (car.path.length === 0 && car.state === 'SEARCHING') {
      car.path = aStar(car.current, car.end, car.blockedPaths);
      if (car.path.length > 0) {
        console.log(`Car ${index} (${car.color}) - Path found, starting movement.`);
      } else {
        car.state = 'PATH NOT FOUND';
        console.log(`Car ${index} (${car.color}) - No path to destination.`);
      }
    }

    // Check if the car is at its destination
    if (isNearDestination(car)) {
      if (car.state !== 'REACHING') {
        car.state = 'REACHING';
        console.log(`Car ${index} (${car.color}) - Reached destination.`);
      }
    } else if (car.path.length > 1) {
      let next = car.path[0];
      if (!isBlocked(car, next)) {
        car.trail.push({ i: car.current.i, j: car.current.j });
        car.current = car.path.shift();
        car.waitCounter = 0; // Reset wait counter on successful move
        console.log(`Car ${index} (${car.color}) - Moving to (${car.current.i},${car.current.j}).`);
      } else {
        console.log(`Car ${index} (${car.color}) - Waiting due to block at (${next.i},${next.j}).`);
        car.waitCounter++; // Increment wait counter

        // Check if car has waited too long
        if (car.waitCounter >= waitLimit) {
          console.log(`Car ${index} (${car.color}) - Returning to start due to wait limit.`);
          car.blockedPaths.add(car.path[0]); // Block this specific cell temporarily
          car.current = car.start; // Reset to starting position
          car.path = []; // Clear path to recalculate next time
          car.waitCounter = 0; // Reset wait counter
          car.state = 'SEARCHING'; // Reset state to try a new route
        }
      }
    }

    // Draw the destination as a circle in the car's color
    fill(car.color);
    noStroke();
    ellipse((car.end.i + 0.5) * w, (car.end.j + 0.5) * h, w / 2, h / 2);

    // Draw the trail of the car
    for (let pos of car.trail) {
      fill(car.color);
      noStroke();
      ellipse((pos.i + 0.5) * w, (pos.j + 0.5) * h, w / 4, h / 4);
    }

    // Draw the car
    fill(car.color);
    rect(car.current.i * w, car.current.j * h, w, h);
  }
}