let diagonal = false;

const cw = 900;
const ch = 600;
const numCars = 4;
const carColors = ['red', 'blue', 'green', 'yellow'];

let cols, rows;
const backcolor = 'gray';
const wallcolor = 'lightgreen';
let grid = [];
let openSets = Array.from({ length: numCars }, () => []);
let closedSets = Array.from({ length: numCars }, () => []);
let cars = [];
let carOrder = []; // Order of cars for movement
let w, h;
let selectedStart = null;
let selectedEnd = null;

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

function gfn(a, b) {
    return diagonal ? dist(a.i, a.j, b.i, b.j) : abs(a.i - b.i) + abs(a.j - b.j);
}

function removeFromArray(arr, elt) {
    for (let i = arr.length - 1; i >= 0; i--) {
        if (arr[i] === elt) {
            arr.splice(i, 1);
        }
    }
}

function Spot(i, j) {
    this.i = i;
    this.j = j;
    this.f = 0;
    this.g = 0;
    this.h = 0;
    this.neighbors = [];
    this.previous = undefined;
    this.wall = false;
    this.start = false;
    this.carColor = null;
    this.end = false;

    this.show = function(col) {
        if (this.carColor) {
            fill(this.carColor);
        } else if (this.end) {
            fill('black');
        } else if (this.start) {
            fill('this.color');
        } else if (this.wall) {
            fill(wallcolor);
        } else {
            fill(backcolor);
        }
        noStroke();
        rect(this.i * w, this.j * h, w, h);
        if (col) {
            fill(col);
            rect(this.i * w, this.j * h, w, h);
        }
    };

    this.addNeighbors = function(grid) {
        if (i < cols - 1) this.neighbors.push(grid[i + 1][j]);
        if (i > 0) this.neighbors.push(grid[i - 1][j]);
        if (j < rows - 1) this.neighbors.push(grid[i][j + 1]);
        if (j > 0) this.neighbors.push(grid[i][j - 1]);
        if (diagonal) {
            if (i > 0 && j > 0) this.neighbors.push(grid[i - 1][j - 1]);
            if (i < cols - 1 && j > 0) this.neighbors.push(grid[i + 1][j - 1]);
            if (i > 0 && j < rows - 1) this.neighbors.push(grid[i - 1][j + 1]);
            if (i < cols - 1 && j < rows - 1) this.neighbors.push(grid[i + 1][j + 1]);
        }
    };
}

class Car {
    constructor(start, end, color, index) {
        this.start = start;
        this.startColor = color;
        this.end = end;
        this.color = color;
        this.index = index;
        this.current = start;
        this.path = [];
        this.failed = false;
        openSets[index].push(this.start);
        this.start.start = true;
        this.start.carColor = color;
        this.end.end = true;
        this.moving = false;
    }

    findPath() {
        if (this.failed) return true;

        if (openSets[this.index].length > 0) {
            let winner = openSets[this.index].reduce((acc, elt, idx) => elt.f < openSets[this.index][acc].f ? idx : acc, 0);
            this.current = openSets[this.index][winner];

            if (this.current === this.end) {
                console.log("success - car " + this.index + " reached its destination");
                return true;
            }

            removeFromArray(openSets[this.index], this.current);
            closedSets[this.index].push(this.current);

            this.current.neighbors.forEach(neighbor => {
                let collision = cars.some(car => car !== this && (car.current === neighbor || car.start === neighbor));
                if (!closedSets[this.index].includes(neighbor) && !neighbor.wall && !collision) {
                    let g = this.current.g + gfn(neighbor, this.current);
                    let newPath = !openSets[this.index].includes(neighbor) || g < neighbor.g;
                    if (newPath) {
                        neighbor.g = g;
                        neighbor.h = heuristic(neighbor, this.end);
                        neighbor.f = neighbor.g + neighbor.h;
                        neighbor.previous = this.current;
                        if (!openSets[this.index].includes(neighbor)) openSets[this.index].push(neighbor);
                    }
                }
            });
            return false;
        } else {
            console.log('fail - no path exists for car ' + this.index);
            this.failed = true;
            return true;
        }
    }

    updatePath() {
        if (this.failed) return;
        this.path = [];
        let temp = this.current;
        while (temp.previous) {
            this.path.push(temp);
            temp = temp.previous;
        }
    }

    showPath() {
        stroke(this.color);
        for (let cell of this.path) {
            cell.show(color(this.color + 'AA'));
        }
    }

    moveOneStep() {
        if (this.path.length > 0 && !this.failed) {
            this.current.carColor = null;
            let nextCell = this.path.pop();
            this.current = nextCell;
            this.current.carColor = this.color;
            this.moving = false;
        }
    }
}

function setup() {
    createCanvas(cw, ch);
    cols = 15;
    rows = 10;
    w = width / cols;
    h = height / rows;

    for (let i = 0; i < cols; i++) grid[i] = new Array(rows);
    for (let i = 0; i < cols; i++)
        for (let j = 0; j < rows; j++)
            grid[i][j] = new Spot(i, j);

    for (let i = 0; i < cols; i++)
        for (let j = 0; j < rows; j++)
            grid[i][j].addNeighbors(grid);

    carOrder = Array.from({ length: numCars }, (_, i) => i).sort(() => Math.random() - 0.5);
}

function draw() {
    background(255);

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

    let allArrived = true;
    for (let i = 0; i < carOrder.length; i++) {
        let car = cars[carOrder[i]];
        if (car && !car.findPath()) {
            allArrived = false;
        }
        if (car) {
            car.updatePath();
            car.showPath();
        }
    }

    if (frameCount % 120 === 0) {
        for (let i = 0; i < carOrder.length; i++) {
            let car = cars[carOrder[i]];
            if (car && !car.moving) {
                car.moving = true;
                car.moveOneStep();
                break;
            }
        }
    }

    if (allArrived && cars.length > 0) {
        noLoop();
        console.log("All cars reached their destinations or cannot proceed");
    }
}

function mousePressed() {
    let i = floor(mouseX / w);
    let j = floor(mouseY / h);
    if (i >= 0 && i < cols && j >= 0 && j < rows) {
        let spot = grid[i][j];
        if (!selectedStart) {
            selectedStart = spot;
            spot.start = true;
            spot.show(); // Update grid to show start point
        } else if (!selectedEnd && spot !== selectedStart) {
            selectedEnd = spot;
            spot.end = true;
            spot.show(); // Update grid to show end point
            cars.push(new Car(selectedStart, selectedEnd, carColors[cars.length % carColors.length], cars.length));
            selectedStart = null;
            selectedEnd = null;
        } else if (spot !== selectedStart && spot !== selectedEnd) {
            spot.wall = !spot.wall;
            spot.show(); // Update grid to show wall state
        }
    }
}