diagonal = false;

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

function randomIntAtoB(a, b){
    return Math.floor(Math.random() * (b - a + 1)) + a;
}

function randomFloatAtoB(a, b){
    return Math.random() * (b - a) + a;
}
var rando = 3;
var cols = 9 * rando;
var rows = 6 * rando;

const wallAmount = randomFloatAtoB(0.1, 0.5);

const backcolor = 'gray';
const wallcolor = 'lightgreen';

var grid = new Array(cols);
var w, h;
const cars = [];
var grid = [];  // Define grid in the global scope
var openSet = [];  // Define openSet in the global scope

class Cell{
    constructor(i, j) {
        this.i = i;
        this.j = j;
        this.f = 0;
        this.g = 0;
        this.h = 0;
        this.neighbors = [];
        this.previous = undefined;
    }
    

    show(col) {
        fill(this.wall ? wallcolor : col || backcolor);
        noStroke();
        rect(this.i * w, this.j * h, w, h);
    }

    addNeighbors(grid)
    {
        let { i, j } = this;
        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 < cols - 1 && j < rows - 1) 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 > 0) this.neighbors.push(grid[i + 1][j - 1]);
            if (i > 0 && j > 0) this.neighbors.push(grid[i - 1][j - 1]);
        
        }
    }
}

//Preset patterns for city layout
const preset1 = [
    [false, false, false, false, false, false, false, false, false],
    [false, true, true, false, true, false, true, true, false],
    [false, true, true, false, true, false, true, true, true],
    [false, false, false, false, false, false, false, false, false],
    [false, true, true, false, true, false, false, true, true],
    [false, true, true, false, true, true, false, true, false],
    [false, false, false, false, false, false, false, false, false],
    [false, true, true, false, true, false, true, true, true],
    [false, true, true, false, true, false, true, false, true]
  ];
  
  const preset2 = [
    [false, false, false, false, false, false, false, false, false],
    [false, true, false, true, true, true, true, false, true],
    [false, true, true, false, false, true, true, false, false],
    [false, false, true, true, false, false, false, false, false],
    [false, false, false, false, false, true, false, true, true],
    [false, true, true, false, true, true, false, true, true],
    [false, true, true, false, false, false, false, false, false],
    [false, false, false, true, true, false, true, false, true],
    [false, true, false, true, true, false, true, true, true]
  ];
  
  const preset3 = [
    [false, false, false, false, false, false, false, false, false],
    [false, true, false, false, true, true, false, true, false],
    [false, true, true, false, true, true, false, true, true],
    [false, false, false, false, false, false, false, false, false],
    [false, true, true, false, false, true, true, false, true],
    [false, true, true, true, false, true, true, false, true],
    [false, false, false, false, false, false, false, false, false],
    [false, true, true, false, true, true, true, true, false],
    [false, true, true, false, true, false, true, false, false]
  ];

  const presets = [preset1, preset2, preset3];

  //Apply preset to grid
  function applyPresetToGrid(grid, startX, startY, preset) {
    for (let i = 0; i < preset.length; i++) {
      for (let j = 0; j < preset[i].length; j++) {
        if (startX + i < grid.length && startY + j < grid[0].length) {
          grid[startX + i][startY + j].wall = preset[i][j];
        }
      }
    }
    return grid;
}
function createCityGrid(grid) {
    const presetSize = 9;
    for (let i = 0; i < grid.length; i += presetSize) {
      for (let j = 0; j < grid[0].length; j += presetSize) {
        const randomPreset = presets[Math.floor(Math.random() * presets.length)];
        grid = applyPresetToGrid(grid, i, j, randomPreset);
      }
    }       
    return grid;
}

// Define the heuristic function before the Car class
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);
}

class Car {
    constructor(start, end, color) {
        this.start = start;  
        this.end = end;
        this.color = color;
        this.current = start;
        this.path = [];
        this.findPath(); // Call findPath once during initialization
    }

    findPath() {
        let openSet = [this.start];
        let closedSet = [];
        this.path = [];

        while (openSet.length > 0) {
            let winner = openSet.reduce((best, cell, idx) => cell.f < openSet[best].f ? idx : best, 0);
            let current = openSet[winner];

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

            }

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

            current.neighbors.forEach((neighbor) => { 
                if (!closedSet.includes(neighbor) && !neighbor.wall) {
                    let tempG = current.g + 1;
                    if (!openSet.includes(neighbor)) {
                        openSet.push(neighbor);
                    }else if (tempG >= neighbor.g) 
                    {
                        return;
                    }
                    neighbor.g = tempG;
                    neighbor.h = heuristic(neighbor, this.end);
                    neighbor.f = neighbor.g + neighbor.h;
                    neighbor.previous = current;
                }
            });
        }
    }

    moveOneStep() {
        if (this.path.length > 1){
            this.current = this.path[1];
            this.path.shift();
        } 
    }

    drawPath(){
        noFill();
        stroke(this.color);
        strokeWeight(4);
        beginShape();
        for (let cell of this.path) {
            vertex(cell.i * w + w / 2, cell.j * h + h / 2);
        }
        endShape();
    }

    showPath() {
        fill(this.color); 
        noStroke();
        rect(this.current.i * w, this.current.j * h, w, h);
    }
}

// Setup function
function setup() {
    frameRate(2);

    // Create canvas and set width and height
    createCanvas(cw, ch);
    w = width / cols;
    h = height / rows;

    console.log("cols:", cols, "rows:", rows); // Add this line to debug

    // Initialize grid
    if (!Number.isInteger(cols) || cols <= 0) {
        console.error("Invalid array length for cols:", cols);
        return;
    }
    if (!Number.isInteger(rows) || rows <= 0) {
        console.error("Invalid array length for rows:", rows);
        return;
    }

    for (var i = 0; i < cols; i++) {
        grid[i] = new Array(rows);
    }

    for (var i = 0; i < cols; i++) {
        for (var j = 0; j < rows; j++) {
            grid[i][j] = new Cell(i, j);
        }
    }
    
    grid = createCityGrid(grid); // Fix this line to call the function
    grid[2][2].wall = true;

    for (var i = 0; i < cols; i++) {
        for (var j = 0; j < rows; j++) {
            grid[i][j].addNeighbors(grid);
        }
    }
    
    start = grid[0][0];
    end = grid[cols - 1][rows - 1];
    start.wall = false;
    end.wall = false;

    openSet.push(start);
    console.log('start search');

    // Initialize cars
    for (let i = 0; i < numCars; i++) {
        let start, end;
        let retryCount = 0;

        const maxRetries = 200;
        do{ 
            start = grid[floor(random(cols))][floor(random(rows))];
            retryCount++;

            if (retryCount > maxRetries) {
                console.log('Max retries reached');
                break;
            }

        }while (start.wall);

        retryCount = 0;

        do {
            end = grid[floor(random(cols))][floor(random(rows))];
            retryCount++;

            if (retryCount > maxRetries) {
                console.log('Max retries reached');
                break;
            }

        } while (end.wall || end === start);

        if (!start || !end) {
            console.error('Failed to assign start or end point for a car');
            continue; // Skip this iteration if either start or end is undefined
        }

        start.wall = false;
        end.wall = false;

        let color = carColors[i % carColors.length];
        cars.push(new Car(start, end, color, i));
    }
}

// Draw function
function draw() {
    background(backcolor);

    grid.flat().forEach(cell => cell.show());

    cars.forEach((car, index) => {
        car.drawPath();

        let blockingCar = cars.find((otherCar, otherIndex) => otherIndex !== index && otherCar.current === car.path[1]);

        if (blockingCar) {
            car.findPath(); // Recalculate path if blocked
        } else {
            car.moveOneStep(); // Move the car one step
        }

        car.showPath();
    });
}