// Cloned by Zoro on 10 Nov 2024 from World "A * way 01 (clone by Zoro) (clone by Zoro)" by Zoro 
// Please leave this clone trail here.
 


// Cloned by Zoro on 10 Nov 2024 from World "A * way 01 (clone by Zoro)" by Zoro 
// Please leave this clone trail here.
 


// Cloned by Zoro on 10 Nov 2024 from World "A * way 01" by Zoro 
// Please leave this clone trail here.
 
let cols = 13;
let rows = 10;
let w, h;
let grid = [];
let cars = [];
let destinations = [];
const colors = ["red", "blue", "green", "yellow"]; // Colors for the cars

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

  // Create one car and destination for testing
  let car = createCar(0);
  cars.push(car);
  let destination = createDestination(car);
  destinations.push(destination);
  
  // Calculate the original path using A* without avoiding walls
  let originalPath = findShortestPathAStar(car, destination);
  car.path = originalPath; // Store path in car object
  console.log(`Car 0 original path:`, originalPath.map(p => `(${p.i},${p.j})`));
}

function draw() {
  background(220);
  displayGrid();

  // Display cars, destinations, and paths
  for (let i = 0; i < cars.length; i++) {
    let car = cars[i];
    let destination = destinations[i];

    // Display the car
    car.show();

    // Display the destination with the car's color
    fill(car.color);
    noStroke();
    rect(destination.i * w, destination.j * h, w, h);

    // Draw the planned path from car to destination
    stroke(car.color);
    strokeWeight(2);
    noFill();
    beginShape();
    for (let pos of car.path) {
      vertex((pos.i + 0.5) * w, (pos.j + 0.5) * h);
    }
    endShape();
  }
}

function initializeGrid() {
  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); // Randomly place walls
    }
  }
}

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

function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = false;

  this.show = function() {
    if (this.wall) {
      fill(0); // Black for walls
    } else {
      fill(255); // White for paths
    }
    stroke(0);
    rect(this.i * w, this.j * h, w, h);
  };
}

function createCar(carIndex) {
  let i, j;
  do {
    i = floor(random(cols));
    j = floor(random(rows));
  } while (grid[i][j].wall || isOccupied(i, j)); 

  return new Car(i, j, colors[carIndex]);
}

function isOccupied(i, j) {
  return cars.some(car => car.i === i && car.j === 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) || isDestinationOccupied(i, j));

  return { i, j, color: car.color };
}

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

function Car(startI, startJ, carColor) {
  this.i = startI;
  this.j = startJ;
  this.color = carColor;
  this.path = [];

  this.show = function() {
    fill(this.color);
    noStroke();
    ellipse((this.i + 0.5) * w, (this.j + 0.5) * h, w / 2, h / 2);
  };
}

function findShortestPathAStar(car, destination) {
  let start = grid[car.i][car.j];
  let end = grid[destination.i][destination.j];

  if (!start || !end) {
    return [];
  }

  let openSet = [];
  let closedSet = new Set();
  let cameFrom = {};

  openSet.push({
    node: start,
    g: 0,
    h: manhattanDistance(start.i, start.j, end.i, end.j),
    f: 0
  });
  cameFrom[`${start.i},${start.j}`] = null;

  let directions = [
    [0, 1], [0, -1], [1, 0], [-1, 0]
  ];

  while (openSet.length > 0) {
    openSet.sort((a, b) => a.f - b.f);
    let current = openSet.shift();
    let currentNode = current.node;

    if (currentNode === end) {
      let path = [];
      while (currentNode !== null) {
        path.unshift(currentNode);
        currentNode = cameFrom[`${currentNode.i},${currentNode.j}`];
      }
      return path;
    }

    closedSet.add(`${currentNode.i},${currentNode.j}`);

    for (let dir of directions) {
      let ni = currentNode.i + dir[0];
      let nj = currentNode.j + dir[1];

      if (ni >= 0 && ni < cols && nj >= 0 && nj < rows) {
        let neighbor = grid[ni][nj];
        let g = current.g + 1;
        let h = manhattanDistance(ni, nj, end.i, end.j);
        let f = g + h;

        let existing = openSet.find(n => n.node.i === ni && n.node.j === nj);
        if (!existing || existing.f > f) {
          openSet.push({ node: neighbor, g, h, f });
          cameFrom[`${neighbor.i},${neighbor.j}`] = currentNode;
        }
      }
    }
  }
  return [];
}

function manhattanDistance(x1, y1, x2, y2) {
  return Math.abs(x1 - x2) + Math.abs(y1 - y2);
}