// Constants
const cw = 900, ch = 600;
const cols = 20, rows = 15;
const numCars = 4;
const wallAmount = 0.2;

// Car colors
const colors = ['red', 'green', 'blue', 'orange'];

// Grid-related variables
let grid = [];
let w, h;

// Car objects
let cars = [];

// Spot object to represent each cell
function Spot(i, j) {
    this.i = i;
    this.j = j;
    this.wall = (Math.random() < wallAmount);

    // Display each spot
    this.show = function(col) {
        fill(this.wall ? 'black' : col || 'white');
        noStroke();
        rect(this.i * w, this.j * h, w, h);
    };

    // Add neighbors for A* pathfinding
    this.addNeighbors = function(grid) {
        let i = this.i, j = this.j;
        this.neighbors = [];
        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]);
    };
}

// Car object with pathfinding
function Car(start, end, color) {
    this.start = start;
    this.end = end;
    this.position = start;
    this.path = aStar(this.start, this.end);
    this.color = color;
    this.completed = false;
}

// Move car one step along path
Car.prototype.move = function() {
    if (this.path.length > 0) {
        this.position = this.path.shift();
    } else {
        this.completed = true;
    }
};

// Setup the grid and initialize the simulation
function setup() {
    frameRate(2);
    createCanvas(cw, ch);
    w = width / cols;
    h = height / rows;

    // Create 2D grid array
    for (let i = 0; i < cols; i++) {
        grid[i] = new Array(rows);
        for (let j = 0; j < rows; j++) {
            grid[i][j] = new Spot(i, j);
        }
    }

    // Add neighbors to each spot
    for (let i = 0; i < cols; i++) {
        for (let j = 0; j < rows; j++) {
            grid[i][j].addNeighbors(grid);
        }
    }

    // Initialize cars with random start/end points on streets
    for (let i = 0; i < numCars; i++) {
        let start = randomStreetPosition();
        let end = randomStreetPosition();
        cars.push(new Car(start, end, colors[i % colors.length]));
    }
}

// A* algorithm to find the shortest path
function aStar(start, goal) {
    let openSet = [{ pos: start, cost: 0, path: [] }];
    let closedSet = new Set();

    while (openSet.length > 0) {
        openSet.sort((a, b) => a.cost - b.cost);
        let { pos, path } = openSet.shift();

        if (pos.x === goal.x && pos.y === goal.y) {
            return path;
        }

        closedSet.add(`${pos.x}-${pos.y}`);
        for (let [dx, dy] of [[1, 0], [-1, 0], [0, 1], [0, -1]]) {
            let neighbor = { x: pos.x + dx, y: pos.y + dy };
            if (isValidPosition(neighbor) && !closedSet.has(`${neighbor.x}-${neighbor.y}`)) {
                openSet.push({
                    pos: neighbor,
                    cost: path.length + heuristic(neighbor, goal),
                    path: [...path, neighbor]
                });
            }
        }
    }
    return [];
}

// Check if position is valid (within bounds, not a wall)
function isValidPosition(pos) {
    let inBounds = pos.x >= 0 && pos.x < cols && pos.y >= 0 && pos.y < rows;
    return inBounds && !grid[pos.x][pos.y].wall;
}

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

// Get a random street position (non-wall)
function randomStreetPosition() {
    let i, j;
    do {
        i = floor(random(cols));
        j = floor(random(rows));
    } while (grid[i][j].wall);
    return { x: i, y: j };
}

// Draw and animate the cars
function draw() {
    background(220);

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

    // Draw cars, start and end points
    cars.forEach(car => {
        // Draw the path (line connecting each point in the path)
        for (let i = 0; i < car.path.length - 1; i++) {
            let p1 = car.path[i];
            let p2 = car.path[i + 1];
            stroke(car.color);
            line(p1.x * w + w / 2, p1.y * h + h / 2, p2.x * w + w / 2, p2.y * h + h / 2);
        }

        // Draw end point as a darker version of the car color
        fill(lerpColor(color(car.color), color('black'), 0.5));
        rect(car.end.x * w, car.end.y * h, w, h);

        // Draw current car position and move
        if (!car.completed) {
            car.move();
            fill(car.color);
            ellipse(car.position.x * w + w / 2, car.position.y * h + h / 2, w * 0.6, h * 0.6);
        }
    });
}