// Cloned by Vanisri on 2 Nov 2024 from World "A star" by "Coding Train" project 
// Please leave this clone trail here.
 
let cols, rows;
let grid = [];
let cars = [];
let cellSize = 50;
let obstacles = [];

// canvas size 
const cw = 700;
const ch = 700; 
 

function setup() 
{
    //slower frame rate to see how it is working 
    frameRate (2);
    
    createCanvas(cw, ch);
    cols = width / cellSize;    //14 Columns
    rows = height / cellSize;   //14 Rows
    background(220);
    
    //Draw Road structure
    drawGrid();
    
    // Initialize the grid
    for (let i = 0; i < cols; i++) {
        grid[i] = [];
        for (let j = 0; j < rows; j++) {
            grid[i][j] = 0; // empty cell
        }
    }
    
    // Vanisri: Add 15 random obstacles to the grid
    for (let i = 0; i < 15; i++) {
        let x = floor(random(cols));
        let y = floor(random(rows));
        grid[x][y] = 1;
        obstacles.push([x, y]);
    }

    // Vanisri: Create multiple cars with random start and goal positions
    for (let i = 0; i < 4; i++) {
        let start = randomPosition();
        let goal;
        do {
            goal = randomPosition();
        } while (arraysEqual(start, goal));

        cars.push(new Car(i, start, goal, color(random(255), random(255), random(255))));
    }
    
    //Draw walls/obstructions
    //drawWalls();
}

// Utility function to generate a random position
function randomPosition() {
    return [floor(random(cols)), floor(random(rows))];
}

function draw()
// the search goes on over many timesteps 
// each timestep, check one more square and draw current partial solution 
{
    
    let occupiedPositions = cars.map(car => `${car.position}`);//Fetches the current position of all cars. Eg. positions=["2,3", "5,4", "1,1"]
    for (let car of cars) {
        if (!car.path.length || arraysEqual(car.position, car.goal)) {
            car.recalculatePath();
        } else {
            if (!occupiedPositions.includes(`${car.path[1]}`)) {
                //If car's next move doesn't have another car there, then move forward with calculated path (car.path[1])
                car.move();
            } else {
                //If car's next move has another car there, then Recalculate path
                car.recalculatePath();//If car's next move has another car there, then Recalculate path
            }
        }
        car.draw();
    }
}

//Sanidhya: Draws Black grids and borders to create a road like view.
function drawGrid() { 
    
    //Black Grids and Border
    stroke("black");
    strokeWeight(3);
    for (let i = 0; i < cols; i++) {
        for (let j = 0; j < rows; j++) { 
            noFill();
            rect(i * cellSize, j * cellSize, cellSize, cellSize); 
        }
    }
    
    //White Dotted Lines
    stroke("white");
    strokeWeight(3);
    
    for (let i = 0; i <= cellSize*cols; i+=cellSize) { 
        var interval=25;
        var interval2 = 50;
        for (let j = 0; j <= cellSize*rows; j+=cellSize) { 
            noFill();
            line(i, interval, j,interval);
            line(i, interval2, j,interval2);
            setLineDash([5, 5]);
            interval+=50;
            interval2+=50;
        }
    }
}

// Utility function to draw dotted lines
function setLineDash(list) {
  drawingContext.setLineDash(list);
}

//Sanidhya: Draws walls. Used Emoticons to denote Obstructions.
function drawWalls() { 
    obstructions=["🌲","🧱","🌳","🏢","🪵","🏨","🏭","🚶","🤰","🪨"];
    for (let i = 0; i < cols; i++) {
        for (let j = 0; j < rows; j++) { 
            if (grid[i][j] === 1) {
                fill('black');
                stroke('black');
                rect(i * cellSize, j * cellSize, cellSize, cellSize); //cellsize=50
                textSize(34);
                text(random(obstructions), i * cellSize, j * cellSize, cellSize, cellSize);
            } else {
                noFill();
            }
        }
    }
}

//Vanisri: Car class to store details of each of the 4 cars
class Car {
    constructor(id, start, goal, col) {
        this.id = id;
        this.position = start;
        this.goal = goal;
        this.color = col;
        this.path = [];
        this.drawSourceGrid();
        this.drawDestinationGrid();
        this.recalculatePath();
    }

    //Calls aStar function to recalculate the path.
    recalculatePath() {
        this.path = aStar(this.position, this.goal);
    }

    move() {
        if (this.path.length > 1) {
            this.position = this.path[1]; // Move to the next step
            this.path.shift();
        }
    }

    //Displays the path of car moved
    draw() {
        noFill();
        stroke(this.color);
        strokeWeight(3);
        rect(this.position[0] * cellSize, this.position[1] * cellSize, cellSize, cellSize); 
        textSize(34);
        text("🚗", this.position[0] * cellSize,this.position[1] * cellSize, cellSize, cellSize);
    }
    
    //To highlight the source of each car
    drawSourceGrid(){
        fill(this.color);
        rect(this.position[0] * cellSize, this.position[1] * cellSize, cellSize, cellSize); 
    }
    
    //To highlight the destination of each car
    drawDestinationGrid() { 
        stroke(this.color);
        strokeWeight(3);
        rect(this.goal[0] * cellSize, this.goal[1] * cellSize, cellSize, cellSize); 
    }
    
}

// https://github.com/qiao/PathFinding.js/blob/master/src/finders/AStarFinder.js
// https://youtu.be/aKYlikFAV4k
// https://briangrinstead.com/blog/astar-search-algorithm-in-javascript/
// Referenced all 3 sources above for the aStar function to find the shortest path
function aStar(start, goal) {
    let openList = [];
    let closedList = new Set();
    let gScore = {};
    let fScore = {};
    let cameFrom = {};

    gScore[`${start}`] = 0;
    fScore[`${start}`] = heuristic(start, goal);
    openList.push(start);

    while (openList.length > 0) {
        let current = openList.reduce((a, b) => (fScore[a] < fScore[b] ? a : b));

        if (arraysEqual(current, goal)) {
            return reconstructPath(cameFrom, current);
        }

        openList = openList.filter(node => !arraysEqual(node, current));
        closedList.add(`${current}`);

        for (let neighbor of getNeighbors(current)) {
            if (closedList.has(`${neighbor}`) || grid[neighbor[0]][neighbor[1]] === 1 || cars.some(car => arraysEqual(car.position, neighbor))) {
                continue;
            }

            let tentativeGScore = gScore[`${current}`] + 1;
            if (tentativeGScore < (gScore[`${neighbor}`] || Infinity)) {
                cameFrom[`${neighbor}`] = current;
                gScore[`${neighbor}`] = tentativeGScore;
                fScore[`${neighbor}`] = tentativeGScore + heuristic(neighbor, goal);

                if (!openList.some(n => arraysEqual(n, neighbor))) {
                    openList.push(neighbor);
                }
            }
        }
    }

    return []; // Return empty if no path found

    function heuristic([x1, y1], [x2, y2]) {
        return abs(x1 - x2) + abs(y1 - y2);
    }

    function reconstructPath(cameFrom, current) {
        let path = [current];
        while (cameFrom[`${current}`]) {
            current = cameFrom[`${current}`];
            path.unshift(current);
        }
        return path;
    }

    function getNeighbors([x, y]) {
        let neighbors = [
            [x + 1, y], [x - 1, y],
            [x, y + 1], [x, y - 1]
        ];
        return neighbors.filter(([nx, ny]) => nx >= 0 && nx < cols && ny >= 0 && ny < rows);
    }
}

// Utility function to check if 2 positions(arrays) are same
function arraysEqual(a, b) {
    return a[0] === b[0] && a[1] === b[1];
}