let cols = 19;
let rows = 13;
let w, h;
let grid = [];
let intersections = [];
let cars = [];
let destinations = [];
let carColors = ["red", "blue", "green", "yellow"];
let sensingResults = [];

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

  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);
    }
  }

  // Adding corner walls
  grid[0][0].wall = true;
  grid[0][rows - 1].wall = true;
  grid[cols - 1][0].wall = true;
  grid[cols - 1][rows - 1].wall = true;

  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 });
      }
    }
  }

  for (let i = 0; i < 4; i++) {
    let carPos = getRandomPosition();
    let destPos = getRandomPosition(carPos);
    cars.push({ pos: carPos, color: carColors[i], primaryPath: [], secondaryPath: [] });
    destinations.push({ pos: destPos, color: carColors[i] });

    // Find paths
    cars[i].primaryPath = [carPos, ...aStarSearch(carPos, destPos)];
    let firstGrid = cars[i].primaryPath[1]; // First grid after the start point
    cars[i].secondaryPath = [carPos, ...aStarSearch(carPos, destPos, [firstGrid])];

    console.log(`Primary Path for car ${i + 1} (${carColors[i]}):`, cars[i].primaryPath);
    console.log(`Secondary Path for car ${i + 1} (${carColors[i]}):`, cars[i].secondaryPath);
  }

  performSensing();
}

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

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

  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);

    stroke(car.color);
    strokeWeight(2);
    noFill();
    beginShape();
    for (let p of car.primaryPath) {
      vertex((p.i + 0.5) * w, (p.j + 0.5) * h);
    }
    endShape();

    stroke(car.color);
    strokeWeight(1);
    beginShape();
    for (let p of car.secondaryPath) {
      vertex((p.i + 0.5) * w, (p.j + 0.5) * h);
    }
    endShape();
  }
}

function Spot(i, j) {
  this.i = i;
  this.j = j;
  this.wall = false;
  this.show = function() {
    if (this.wall) fill(0);
    else fill(255);
    stroke(200);
    rect(this.i * w, this.j * h, w, h);
  };
}

function isIntersection(i, j) {
  let whiteNeighbors = 0;
  if (j > 0 && !grid[i][j - 1].wall) whiteNeighbors++;
  if (j < rows - 1 && !grid[i][j + 1].wall) whiteNeighbors++;
  if (i > 0 && !grid[i - 1][j].wall) whiteNeighbors++;
  if (i < cols - 1 && !grid[i + 1][j].wall) whiteNeighbors++;
  return whiteNeighbors > 2;
}

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;
}

function aStarSearch(start, end, blocked = []) {
  let openSet = [];
  let closedSet = new Set();
  openSet.push({ pos: start, path: [], g: 0, f: heuristic(start, end) });

  let blockedSet = new Set(blocked.map(b => `${b.i},${b.j}`));

  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 || blockedSet.has(key)) continue;

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

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

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;
}

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:`, result);
  }
}

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 };
}