type Coords :
    record
        x, y : int
    end record

class PathFinding
    import Coords
    export hasMoreMoves, findPath, nextMove, foundPath

    var hasMoreMoves : boolean := false
    var foundPath : boolean := false
    var pathPosition : int := 0
    var path : flexible array 1 .. 0 of Coords

    fcn BFS (finish : Coords, cols, rows : int, next : array 1 .. * of Coords, var trav : array 1 .. *, 1 .. * of boolean, map : array 1 .. *, 1 .. * of boolean) : Coords
        var ret, temp : Coords
        var newNext : flexible array 1 .. 0 of Coords

        for i : 1 .. upper (next)
            if next (i).x = finish.x & next (i).y = finish.y then
                result next (i)
            else
                for x : -1 .. 1
                    for y : -1 .. 1
                        if next (i).x + x >= 1 & next (i).x + x <= cols & next (i).y + y >= 1 & next (i).y + y <= rows
                                & abs (x) ~= abs (y) & ~trav (next (i).x + x, next (i).y + y) & ~map (next (i).x + x, next (i).y + y) then
                            trav (next (i).x + x, next (i).y + y) := true
                            temp.x := next (i).x + x
                            temp.y := next (i).y + y
                            new newNext, upper (newNext) + 1
                            newNext (upper (newNext)) := temp
                        end if
                    end for
                end for
            end if
        end for

        if upper (newNext) > 0 then
            ret := BFS (finish, cols, rows, newNext, trav, map)
            new path, upper (path) + 1
            path (upper (path)) := ret
            for i : 1 .. upper (next)
                if abs (next (i).x - ret.x) + abs (next (i).y - ret.y) = 1 then
                    result next (i)
                end if
            end for
            result ret
        end if

        ret.x := 0
        ret.y := 0
        result ret
    end BFS

    proc findPath (map : array 1 .. *, 1 .. * of boolean, cols, rows, sx, sy, fx, fy : int)
        new path, 0

        var start : array 1 .. 1 of Coords
        start (1).x := sx
        start (1).y := sy

        var trav : array 1 .. cols, 1 .. rows of boolean
        for i : 1 .. cols
            for j : 1 .. rows
                trav (i, j) := false
            end for
        end for

        var finish : Coords
        finish.x := fx
        finish.y := fy

        start (1) := BFS (finish, cols, rows, start, trav, map)
        new path, upper (path) + 1
        path (upper (path)) := start (1)

        if start (1).x = start (1).y and start (1).x = 0 then
            hasMoreMoves := false
            foundPath := false
            pathPosition := 0
        else
            hasMoreMoves := true
            foundPath := true
            pathPosition := upper (path)
        end if
    end findPath

    fcn nextMove : Coords
        pathPosition -= 1
        if pathPosition = 0 then
            hasMoreMoves := false
        end if
        result path (pathPosition + 1)
    end nextMove
end PathFinding

setscreen ("graphics:max;max,offscreenonly,nobuttonbar")

const GRID_SIZE := 10
const COLS := maxx div GRID_SIZE - 1
const ROWS := maxy div GRID_SIZE - 3

var path : ^PathFinding
new PathFinding, path

var move, start, finish : Coords
start.x := 1
start.y := 1
var mx, my, md, t : int

var map : array 1 .. COLS, 1 .. ROWS of boolean
for x : 1 .. COLS
    for y : 1 .. ROWS
        if Rand.Real < 0.25 then
            map (x, y) := true
        else
            map (x, y) := false
        end if
    end for
end for

proc drawMap
    for x : 1 .. COLS
        for y : 1 .. ROWS
            if map (x, y) then
                Draw.FillBox (x * GRID_SIZE, y * GRID_SIZE, x * GRID_SIZE + GRID_SIZE, y * GRID_SIZE + GRID_SIZE, 7)
            else
                Draw.FillBox (x * GRID_SIZE, y * GRID_SIZE, x * GRID_SIZE + GRID_SIZE, y * GRID_SIZE + GRID_SIZE, grey)
            end if
        end for
    end for
    Draw.FillBox (start.x * GRID_SIZE + 1, start.y * GRID_SIZE + 1, start.x * GRID_SIZE + GRID_SIZE - 1, start.y * GRID_SIZE + GRID_SIZE - 1, green)
    View.Update
end drawMap

proc drawPath
    Draw.FillBox (finish.x * GRID_SIZE + 1, finish.y * GRID_SIZE + 1, finish.x * GRID_SIZE + GRID_SIZE - 1, finish.y * GRID_SIZE + GRID_SIZE - 1, brightred)
    View.Update
    loop
        exit when ~path -> hasMoreMoves
        move := path -> nextMove
        Draw.FillBox (move.x * GRID_SIZE + 1, move.y * GRID_SIZE + 1, move.x * GRID_SIZE + GRID_SIZE - 1, move.y * GRID_SIZE + GRID_SIZE - 1, 43)
        View.Update
        delay (10)
    end loop
end drawPath


loop
    drawMap
    loop
        mousewhere (mx, my, md)
        exit when md = 1 & mx >= GRID_SIZE & mx <= GRID_SIZE * COLS + GRID_SIZE & my >= GRID_SIZE & my <= GRID_SIZE * ROWS + GRID_SIZE
    end loop

    finish.x := mx div GRID_SIZE
    finish.y := my div GRID_SIZE

    t := Time.Elapsed
    path -> findPath (map, COLS, ROWS, start.x, start.y, finish.x, finish.y)
    locate (1, 1)
    put "Time to find path: ", (Time.Elapsed - t) / 1000, " seconds"

    if path -> foundPath then
        drawPath
        start.x := finish.x
        start.y := finish.y
    end if

end loop