let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let intersections = []; // List of intersection cells
let cars = [];
let destinations = [];
let carColors = ["red", "blue", "green", "yellow"];
let sensingResults = []; // Store sensing results here

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

  // Initialize grid and walls
  for (let i = 0; i < cols; i++) {
    grid[i] = [];
    for (let j = 0; j < rows; j++) {
      grid[i][j] = new Spot(i, j);
      // Mark walls at intervals
      grid[i][j].wall = !(i % 3 === 0 || j % 3 === 0);
    }
  }

  // Add black grids (bricks) at the corners
  grid[0][0].wall = true;       // Top-left corner
  grid[0][rows - 1].wall = true; // Top-right corner
  grid[cols - 1][0].wall = true; // Bottom-left corner
  grid[cols - 1][rows - 1].wall = true; // Bottom-right corner

  // Identify and save all intersections
  for (let i = 0; i < cols; i++) {
    for (let j = 0; j < rows; j++) {
      if (!grid[i][j].wall && isIntersection(i, j)) {
        intersections.push({ i, j }); // Save intersection coordinates
      }
    }
  }

  // Initialize cars and destinations with unique positions
  for (let i = 0; i < 4; i++) {
    let carPos = getRandomPosition();
    let destPos = getRandomPosition(carPos);
    cars.push({ pos: carPos, color: carColors[i], path: [] });
    destinations.push({ pos: destPos, color: carColors[i] });
    
    // Compute the path for each car using A* and store it
    cars[i].path = [carPos, ...aStarSearch(carPos, destPos)];
    console.log(`Path for car ${i + 1} (${carColors[i]}):`, cars[i].path);
  }

  // Perform consistent sensing for all cars before any moves are made
  performSensing();
}


function draw() {
  background(255);

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

  // Draw destinations as squares
  for (let dest of destinations) {
    fill(dest.color);
    noStroke();
    rect(dest.pos.i * w, dest.pos.j * h, w, h);
  }

  // Draw cars as circles and show their paths
  for (let car of cars) {
    fill(car.color);
    noStroke();
    ellipse((car.pos.i + 0.5) * w, (car.pos.j + 0.5) * h, w * 0.6, h * 0.6);

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

  // Perform planning for all cars (this will log to the console)
  performPlanning();
}

// Spot object represents each cell in the grid
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 and intersections
    }
    stroke(200);
    rect(this.i * w, this.j * h, w, h);
  };
}

// Check if a cell is an intersection by counting adjacent white cells
function isIntersection(i, j) {
  let whiteNeighbors = 0;

  // Check above
  if (j > 0 && !grid[i][j - 1].wall) whiteNeighbors++;
  // Check below
  if (j < rows - 1 && !grid[i][j + 1].wall) whiteNeighbors++;
  // Check left
  if (i > 0 && !grid[i - 1][j].wall) whiteNeighbors++;
  // Check right
  if (i < cols - 1 && !grid[i + 1][j].wall) whiteNeighbors++;

  // If more than two white neighbors, it's an intersection
  return whiteNeighbors > 2;
}

// Get random position on a path cell, ensuring it doesn’t overlap with given position
function getRandomPosition(exclude = null) {
  let pos;
  do {
    let i = int(random(cols));
    let j = int(random(rows));
    pos = { i, j };
  } while (grid[pos.i][pos.j].wall || (exclude && pos.i === exclude.i && pos.j === exclude.j));
  return pos;
}

// A* Search Algorithm
function aStarSearch(start, end, blockedGrids = []) {
  let openSet = [];
  let closedSet = new Set();
  openSet.push({ pos: start, path: [], g: 0, f: heuristic(start, end) });

  // Mark the blocked grids as walls
  blockedGrids.forEach(grid => {
    grid.wall = true; // Mark the blocked grids as obstacles
  });

  while (openSet.length > 0) {
    let current = openSet.reduce((a, b) => (a.f < b.f ? a : b));
    openSet = openSet.filter(node => node !== current);

    if (current.pos.i === end.i && current.pos.j === end.j) {
      return current.path;
    }

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

    let neighbors = getNeighbors(current.pos);
    for (let neighbor of neighbors) {
      let key = `${neighbor.i},${neighbor.j}`;
      if (closedSet.has(key) || grid[neighbor.i][neighbor.j].wall) continue;

      let g = current.g + 1;
      let f = g + heuristic(neighbor, end);
      openSet.push({ pos: neighbor, path: [...current.path, neighbor], g, f });
    }
  }
  return [];
}

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

// Get valid neighbors (no diagonal, only horizontal/vertical)
function getNeighbors(pos) {
  let neighbors = [];
  let { i, j } = pos;

  if (i > 0) neighbors.push({ i: i - 1, j });
  if (i < cols - 1) neighbors.push({ i: i + 1, j });
  if (j > 0) neighbors.push({ i, j: j - 1 });
  if (j < rows - 1) neighbors.push({ i, j: j + 1 });

  return neighbors;
}

// Consistent sensing function for each car agent
function performSensing() {
  let carPositions = cars.map(car => car.pos); 

  for (let i = 0; i < cars.length; i++) {
    let car = cars[i];
    let result = sensing(car, carPositions);
    sensingResults.push(result);
    console.log(`Car ${i + 1} (${car.color}) Sensing Results:`);
    console.log("Allowed Grids:", result.allowedGrids);
    console.log("Blocked Grids:", result.blockedGrids);
    console.log("Detected Cars:", result.detectedCars);
    console.log("Nearby Intersections:", result.intersections);
  }
}

// Sensing function with intersections
function sensing(car, carPositions) {
  let allowedGrids = [];
  let blockedGrids = [];
  let detectedCars = [];
  let intersectionsNearby = [];
  let neighbors = getNeighbors(car.pos);

  for (let neighbor of neighbors) {
    let gridCell = grid[neighbor.i][neighbor.j];
    let occupied = carPositions.some(pos => pos.i === neighbor.i && pos.j === neighbor.j);

    if (gridCell.wall || occupied) {
      blockedGrids.push(neighbor);
      if (occupied) {
        let detectedCarIndex = carPositions.findIndex(pos => pos.i === neighbor.i && pos.j === neighbor.j);
        detectedCars.push({ pos: neighbor, carColor: cars[detectedCarIndex].color });
      }
    } else {
      allowedGrids.push(neighbor);
      if (isIntersection(neighbor.i, neighbor.j)) {
        intersectionsNearby.push(neighbor);
      }
    }
  }

  return { allowedGrids, blockedGrids, detectedCars, intersections: intersectionsNearby };
}

// Plan Movement based on sensing results
function planMovement(car, sensingResult) {
  let currentPos = car.pos;
  let nextPos = car.path[1]; // Assuming path contains at least 2 elements

  // Check if the next position is in allowed grids
  let allowedGridMatch = sensingResult.allowedGrids.some(grid => grid.i === nextPos.i && grid.j === nextPos.j);
  
  if (allowedGridMatch) {
    console.log("Proceed as per the original plan");
    return "Proceed as per the original plan";
  }

  // Check if next position is in blocked grids
  let blockedGridMatch = sensingResult.blockedGrids.some(grid => grid.i === nextPos.i && grid.j === nextPos.j);

  if (blockedGridMatch) {
    // Check if the next position is an intersection
    let isIntersection = sensingResult.intersections.some(grid => grid.i === nextPos.i && grid.j === nextPos.j);
    
    if (isIntersection) {
      console.log("Wait (do not move) and pass the turn to the next car");
      return "Wait (do not move) and pass the turn to the next car";
    } else {
      console.log("Re-route");

      // Create the updated list of blocked grids
      let updatedBlockedGrids = sensingResult.blockedGrids;

      // Generate the revised path using A* with the new blocked grids
      let newPath = aStarSearch(currentPos, destinations.find(d => d.color === car.color).pos, updatedBlockedGrids);

      // Check if the new path is clear (matches the allowed grids)
      if (isPathValid(newPath, sensingResult.allowedGrids)) {
        car.path = [currentPos, ...newPath];
        console.log("Revised Path:", car.path);
        return "Re-route";
      } else {
        console.log("Re-route failed: No valid path found.");
        return "Re-route failed";
      }
    }
  }

  // If the grid is neither allowed nor blocked, raise an error
  console.error("Error: Next position is neither in allowed nor blocked grids.");
  return "Error: Invalid grid state.";
}

// Function to check if the path is valid (all grids in the path must be in the allowed grids)
function isPathValid(path, allowedGrids) {
  return path.every(p => allowedGrids.some(grid => grid.i === p.i && grid.j === p.j));
}


// Perform planning for all cars
function performPlanning() {
  for (let car of cars) {
    let sensingResult = sensingResults[cars.indexOf(car)];
    let decision = planMovement(car, sensingResult);
    console.log(`Car (${car.color}) Decision:`, decision);
  }
}