// MH edit 
const rows = 15; // 20;
const cols = 15; // 30;


const cellSize = 40;
let grid = [];
let cars = [];
const numCars = 4;
let grass_image,road_image,house_image;
let car_images = []; // Array to hold car images
let carColors = []; 

function preload() {
  grass_image = loadImage('/uploads/nivedithavudayagiri/grass.png'); //Load grass image for non-road
  road_image = loadImage('/uploads/nivedithavudayagiri/Road-2-way.png'); //Load road image
  road_intersection_image = loadImage('/uploads/nivedithavudayagiri/Road-intersection.jpg'); //Load road image
  house_image = loadImage('/uploads/nivedithavudayagiri/New-york-buildings-4.png'); // Load house image (destination spot 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))); // Assigning random colors for each car trail
  }
}

function setup() {
  createCanvas(cols * cellSize, rows * cellSize);
  
  //Initialise NYC Grid
  grid = createNYCGrid(rows, cols);
  
  //Generate random houses in non-road locations
  housePositions = generateHouses(numCars); 
  
  //Initialise 4 cars
  cars = createCars(numCars);
  
  // 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) {
  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] === 1); 
  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 grass
      } else if (grid[r][c] === 0) {
        if (r % 3 === 0 && c % 5 === 0) {
          image(road_intersection_image, c * cellSize, r * cellSize, cellSize, cellSize);
        } else {
          // Check if this cell should be an avenue (e.g., every third column is an avenue)
          if (c % 5 === 0) { 
            push(); // Save current transformation state
            translate(c * cellSize + cellSize / 2, r * cellSize + cellSize / 2); // Move origin to cell center
            rotate(HALF_PI); // Rotate 90 degrees (HALF_PI radians)
            image(road_image, -cellSize / 2, -cellSize / 2, cellSize, cellSize); // Draw rotated road image
            pop(); // Restore previous transformation state
          } else {
            image(road_image, c * cellSize, r * cellSize, cellSize, cellSize); // Draw normal 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();

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

    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.h = heuristic(neighbor, end);
        neighbor.f = neighbor.g + neighbor.h;
        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) {
  
  // Create a set to keep track of occupied positions by other cars
  let occupiedPositions = new Set(cars.map(car => `${car.position.x},${car.position.y}`)); 
  let neighbors = [];
  let { x, y } = node;
  
    // Define potential movement directions (left, right, up, down)
  const directions = [
    { dx: -1, dy: 0 }, // Left
    { dx: 1, dy: 0 },  // Right
    { dx: 0, dy: -1 }, // Up
    { dx: 0, dy: 1 }   // Down
  ];

  directions.forEach(({ dx, dy }) => {
    const newX = x + dx;
    const newY = y + dy;
    
    // Check if the adjacent cell is within the grid boundaries
    if (newX >= 0 && newX < cols && newY >= 0 && newY < rows) {
      const cellType = grid[newY][newX];
      const positionKey = `${newX},${newY}`;

      // Allow only road cells (0) as passable
      if (cellType === 0) {
        neighbors.push({ x: newX, y: newY });
      } 
      
      // Check if the cell is a destination house and only add if it's the final destination
      else if (cellType === 2 && housePositions.some(house => house.x === newX && house.y === newY)) {
        neighbors.push({ x: newX, y: newY });
      }
    }
  });
  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();
}

// Move cars with priority handling at intersections
function moveCars() {
  cars.forEach((car, index) => {
    if (car.path.length > 0 && car.pathIndex < car.path.length) {
      let nextPosition = car.path[car.pathIndex];

      let collision = cars.some((otherCar) =>
        otherCar !== car && otherCar.position.x === nextPosition.x && otherCar.position.y === nextPosition.y
      );

      if (collision) {
        console.log(`Collision detected! Checking priority for Car ${index + 1}.`);
        
        let otherCar = cars.find((otherCar) =>
          otherCar !== car && otherCar.position.x === nextPosition.x && otherCar.position.y === nextPosition.y
        );

        let carPriority = heuristic(car.position, car.destination);
        let otherCarPriority = heuristic(otherCar.position, otherCar.destination);
        
        if (carPriority < otherCarPriority || (carPriority == otherCarPriority && index < cars.indexOf(otherCar))) {
          console.log(`Car ${index + 1} has priority. Path recomputing...`);
          grid[nextPosition.y][nextPosition.x]=1;
          car.path = aStar(car.position, car.destination);
          car.pathIndex++;
          car.pathIndex = 0; // Reset path index to start again
          grid[nextPosition.y][nextPosition.x]=0;
        } else {
          console.log(`Car ${cars.indexOf(otherCar) + 1} has priority. Car ${index + 1} will reroute.`);
          return;
        }
      } else {
        let dx = nextPosition.x - car.position.x;
        let dy = nextPosition.y - car.position.y;
        car.angle = atan2(dy, dx);
        car.position = nextPosition;
        car.pathIndex++;

        if (car.position.x === car.destination.x && car.position.y === car.destination.y) {
          car.hasReached = true;
          console.log(`Car ${index + 1} has reached destination from (${car.initial.x},${car.initial.y}) to (${car.destination.x},${car.destination.y})`);
        }
      }
    }
  });
}