const rows = 20;
const cols = 30;
const cellSize = 40;
let grid = [];
let cars = [];
const numCars = 4;
let grass_image, road_image, house_image;
let car_images = [];
let carColors = [];

function preload() {
  grass_image = loadImage('/uploads/dash1127/house.jpeg'); // Load grass image for non-road
  road_image = loadImage('/uploads/dash1127/2.jpeg'); // Load road image
  house_image = loadImage('/uploads/nivedithavudayagiri/New-york-buildings-4.png'); // Load house image (destination for cars)

  // Load car images
  for (let i = 1; i <= numCars; i++) {
    car_images.push(loadImage(`/uploads/dash1127/car${i}.png`));
    carColors.push(color(random(255), random(255), random(255))); // Assign random colors for each car trail
  }
}

function setup() {
  createCanvas(cols * cellSize, rows * cellSize);
  
  // Initialize NYC Grid
  grid = createNYCGrid(rows, cols);
  
  // Generate random houses in non-road locations
  let housePositions = generateHouses(numCars); 
  
  // Initialize 4 cars
  cars = createCars(numCars, housePositions);
  
  // Control speed of simulation (2 steps per second)
  frameRate(2); 
}

function draw() {
  background(255);
  drawGrid();
  drawCars();

  // Move cars step-by-step
  moveCars();
}

// Set up the NYC grid layout
function createNYCGrid(rows, cols) {
  let grid = [];
  for (let r = 0; r < rows; r++) {
    let row = [];
    for (let c = 0; c < cols; c++) {
      // Define streets every 3rd row and every 5th column for avenues
      if (r % 3 === 0 || c % 5 === 0) {
        row.push(0); // Road
      } else {
        row.push(1); // Building block
      }
    }
    grid.push(row);
  }
  return grid;
}

// Generate positions for houses within the grid
function generateHouses(n) {
  let houses = [];
  while (houses.length < n) {
    let r = floor(random(1, rows));
    let c = floor(random(1, cols));
    
    if (grid[r][c] === 1) { // Ensure it's a non-road building block
      houses.push({ x: c, y: r });
      grid[r][c] = 2; // Mark cell as house
    }
  }
  return houses;
}

function createCars(n, housePositions) {
  let cars = [];
  for (let i = 0; i < n; i++) {
    let start = randomRoad();
    let end = housePositions[i];
    cars.push({
      image: car_images[i], // Assign car image
      color: carColors[i], // Assign unique color
      position: start,
      initial: start,
      destination: end,
      path: aStar(start, end), // Track the final path of the car
      pathIndex: 0, // Track the current index in the path
      hasReached: false, // Track if the car has reached its destination
      angle: 0 // Initial angle
    });
  }
  return cars;
}

// Get position of a random road location to set as start point for cars
function randomRoad() {
  let r, c;
  do {
    r = floor(random(rows));
    c = floor(random(cols));
  } while (grid[r][c] !== 0); // Ensure it's a road
  return { x: c, y: r };
}

function drawGrid() {
  for (let r = 0; r < rows; r++) {
    for (let c = 0; c < cols; c++) {
      stroke(0);
      noFill();
      rect(c * cellSize, r * cellSize, cellSize, cellSize); // Draw grid

      if (grid[r][c] === 1) {
        image(grass_image, c * cellSize, r * cellSize, cellSize, cellSize); // Draw building block
      } else if (grid[r][c] === 0) {
        image(road_image, c * cellSize, r * cellSize, cellSize, cellSize); // Draw road
      } else if (grid[r][c] === 2) {
        image(house_image, c * cellSize, r * cellSize, cellSize, cellSize); // Draw house
      }
    }
  }
}

function drawCars() {
  cars.forEach(car => {
    push();
    translate(car.position.x * cellSize + cellSize / 2, car.position.y * cellSize + cellSize / 2);
    rotate(car.angle);
    image(car.image, -cellSize / 2 + 5, -cellSize / 2 + 5, cellSize - 10, cellSize - 10);
    pop();

    // Draw car's path
    if (car.path.length > 0) {
      stroke(car.color);
      strokeWeight(2);
      for (let i = 0; i < car.path.length - 1; i++) {
        let startPos = car.path[i];
        let endPos = car.path[i + 1];
        line(startPos.x * cellSize + cellSize / 2, startPos.y * cellSize + cellSize / 2,
             endPos.x * cellSize + cellSize / 2, endPos.y * cellSize + cellSize / 2);
      }
    }

    // Draw destination square
    fill(car.color);
    rect(car.destination.x * cellSize + 10, car.destination.y * cellSize + 10, cellSize - 20, cellSize - 20);
  });
}

// A* Pathfinding Algorithm
function aStar(start, end) {
  let openSet = [];
  let closedSet = [];
  openSet.push({ ...start, g: 0, f: heuristic(start, end), parent: null });

  while (openSet.length > 0) {
    let current = openSet.sort((a, b) => a.f - b.f)[0]; // Get node with lowest f

    // If the current position is the destination
    if (current.x === end.x && current.y === end.y) {
      console.log(`Path found from ${start.x},${start.y} to ${end.x},${end.y}`);
      return reconstructPath(current);
    }

    openSet = openSet.filter(node => node !== current);
    closedSet.push(current);

    let neighbors = getNeighbors(current);

    neighbors.forEach(neighbor => {
      if (closedSet.find(n => n.x === neighbor.x && n.y === neighbor.y)) {
        return;
      }

      let tentative_g = current.g + 1;
      let inOpenSet = openSet.find(n => n.x === neighbor.x && n.y === neighbor.y);

      if (!inOpenSet || tentative_g < neighbor.g) {
        neighbor.g = tentative_g;
        neighbor.f = neighbor.g + heuristic(neighbor, end);
        neighbor.parent = current;

        if (!inOpenSet) {
          openSet.push(neighbor);
        }
      }
    });
  }

  console.log(`No path found from ${start.x},${start.y} to ${end.x},${end.y}`);
  return []; // No path found
}

// Get neighbors function to get valid roads or houses 
function getNeighbors(node) {
  let neighbors = [];
  let { x, y } = node;

  // Check the cells adjacent to the current node
  if (x > 0 && (grid[y][x - 1] === 0 || grid[y][x - 1] === 2)) neighbors.push({ x: x - 1, y }); // Left
  if (x < cols - 1 && (grid[y][x + 1] === 0 || grid[y][x + 1] === 2)) neighbors.push({ x: x + 1, y }); // Right
  if (y > 0 && (grid[y - 1][x] === 0 || grid[y - 1][x] === 2)) neighbors.push({ x, y: y - 1 }); // Up
  if (y < rows - 1 && (grid[y + 1][x] === 0 || grid[y + 1][x] === 2)) neighbors.push({ x, y: y + 1 }); // Down

  return neighbors;
}

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

function reconstructPath(node) {
  let path = [];
  while (node) {
    path.push({ x: node.x, y: node.y });
    node = node.parent;
  }
  return path.reverse();
}

function moveCars() {
  let occupiedCells = {};

  // Mark current positions of all cars as occupied
  cars.forEach(car => {
    if (!car.hasReached) {
      occupiedCells[`${car.position.x},${car.position.y}`] = true;
    }
  });

  cars.forEach(car => {
    if (car.path.length > 0 && !car.hasReached) {
      // Get the next position in the path
      let nextPos = car.path[car.pathIndex];

      // Check if the next position is occupied
      if (!occupiedCells[`${nextPos.x},${nextPos.y}`]) {
        // Move car to the next position
        car.position = nextPos;
        car.pathIndex++;
      } else {
        // If the next position is occupied, attempt to find an alternative route
        let alternativePath = aStar(car.position, car.destination);
        if (alternativePath.length > 0) {
          car.path = alternativePath;
          car.pathIndex = 0; // Reset index to follow new path
        }
      }

      // Check if car has reached the destination
      if (car.position.x === car.destination.x && car.position.y === car.destination.y) {
        car.hasReached = true;
      }

      // Update car angle based on movement direction
      if (car.pathIndex > 0) {
        let current = car.path[car.pathIndex - 1];
        let next = car.path[car.pathIndex];
        car.angle = atan2(next.y - current.y, next.x - current.x); // Update angle for rotation
      }
    }
  });
}
function draw() {
  background(255);
  drawGrid();
  drawCars();
  
  // Move cars step-by-step
  moveCars();
}