% A* Pathfinding Implementation
% Finds a path, but it's inefficient (goes through unnecessary points. F-values f----- up?)

View.Set ("graphics")
randomize
type Node :
    record
        x : int
        y : int
        f : int
        parentX, parentY : int
    end record

function onClosedList (node : Node, closedList : array 1 .. * of Node, counter : int) : boolean
    for i : 1 .. counter
        if node.x = closedList (i).x and node.y = closedList (i).y then
            result true
        end if
    end for
    result false
end onClosedList

proc findPath (map : array 1 .. 20, 1 .. 20 of int, startNode, goalNode : Node)
    var currentNode, temp : Node
    var openList : array 1 .. 20 of Node
    var closedList : array 1 .. 20 of Node
    var path : array 1 .. 400 of Node
    var numOnOpenList, numOnClosedList, lowestFNum : int := 1
    var lowestFVal : int := 100
    var pathCount, tempNum, count : int := 0
    var foundNode : boolean := false

    for i : 1 .. 20
        openList (i).x := 0
        openList (i).y := 0
        openList (i).f := 0
        openList (i).parentX := 0
        openList (i).parentY := 0
        closedList (i).x := 0
        closedList (i).y := 0
        closedList (i).f := 0
        path (i).x := 0
        path (i).y := 0
    end for

    openList (1) := startNode
    openList (2).parentX := startNode.x
    openList (2).parentY := startNode.y
    currentNode := startNode
    numOnOpenList := 1
    loop
        % Is the current node the goal node?
        if currentNode.x = goalNode.x and currentNode.y = goalNode.y then
            put "Target reached"
            exit
        end if

        % Find the possible successors to currentNode
        % LEFT
        if currentNode.x >= 2 and map (currentNode.x - 1, currentNode.y) not= 5 then
            currentNode.parentX := openList (numOnOpenList).x
            currentNode.parentY := openList (numOnOpenList).y
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).x -= 1
            openList (numOnOpenList).f := round (sqrt (intreal ((abs (goalNode.x - openList (numOnOpenList).x) ** 2 + abs (goalNode.y - openList (numOnOpenList).y) ** 2))))
        end if
        % DOWN
        if currentNode.y >= 2 and map (currentNode.x, currentNode.y - 1) not= 5 then
            currentNode.parentX := openList (numOnOpenList).x
            currentNode.parentY := openList (numOnOpenList).y
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).y -= 1
            openList (numOnOpenList).f := round (sqrt (intreal ((abs (goalNode.x - openList (numOnOpenList).x) ** 2 + abs (goalNode.y - openList (numOnOpenList).y) ** 2))))
        end if
        % RIGHT
        if currentNode.x <= 18 and map (currentNode.x + 1, currentNode.y) not= 5 then
            currentNode.parentX := openList (numOnOpenList).x
            currentNode.parentY := openList (numOnOpenList).y
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).x += 1
            openList (numOnOpenList).f := round (sqrt (intreal ((abs (goalNode.x - openList (numOnOpenList).x) ** 2 + abs (goalNode.y - openList (numOnOpenList).y) ** 2))))
        end if
        % UP
        if currentNode.y <= 18 and map (currentNode.x, currentNode.y + 1) not= 5 then
            currentNode.parentX := openList (numOnOpenList).x
            currentNode.parentY := openList (numOnOpenList).y
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).y += 1
            openList (numOnOpenList).f := round (sqrt (intreal ((abs (goalNode.x - openList (numOnOpenList).x) ** 2 + abs (goalNode.y - openList (numOnOpenList).y) ** 2))))
        end if

        % Sort the openList using F-costs
        for i : 1 .. numOnOpenList
            for j : i .. numOnOpenList
                if openList (i).f > openList (j).f then
                    temp := openList (i)
                    openList (i) := openList (j)
                    openList (j) := temp
                end if
            end for
        end for

        % Assign a node to currentNode that is not on the Closed List
        if onClosedList (openList (1), closedList, numOnClosedList) = false then
            currentNode := openList (1)
        else
            for i : 2 .. numOnOpenList
                %if onClosedList (openList (i), closedList, numOnClosedList) = false and foundNode = false then
                if foundNode = false then
                    foundNode := true
                    currentNode := openList (i)
                    tempNum := i
                    pathCount += 1
                    path (pathCount) := currentNode
                end if
            end for
        end if

        % Remove the first node from the openList
        for i : 1 .. numOnOpenList - 1
            openList (i) := openList (i + 1)
        end for

        % Add current node to the closed list
        numOnClosedList += 1
        closedList (numOnClosedList) := currentNode
        if numOnClosedList > 15 then
            numOnClosedList := 0
        end if
        if pathCount >= 400 then
            put "No path found"
            exit
        end if

        drawfillbox (currentNode.x * 8, currentNode.y * 8, currentNode.x * 8 + 7, currentNode.y * 8 + 7, blue)
        numOnOpenList := 1
        lowestFNum := 1
        lowestFVal := 100
        foundNode := false
    end loop

    % Trace the path
    loop
        count := pathCount - 1
        path (pathCount - count) := currentNode
        count -= 1
        exit when count <= 1
    end loop
end findPath

var map : array 1 .. 20, 1 .. 20 of int
for i : 1 .. 20
    for j : 1 .. 20
        map (i, j) := 2
        drawbox (i * 8, j * 8, 8 + i * 8, 8 + j * 8, black)
    end for
end for

var start, goal : Node
var mx, my, mb : int
buttonchoose ("multibutton")
start.x := 14
start.y := 2
start.f := 2
randint (goal.x, 1, 20)
randint (goal.y, 1, 20)
drawfillbox (goal.x * 8, goal.y * 8, goal.x * 8 + 8, goal.y * 8 + 8, green)
loop
    mousewhere (mx, my, mb)
    if mb = 1 then
        map (mx div 8, my div 8) := 5
        drawfillbox ((mx div 8) * 8, (my div 8) * 8, (mx div 8) * 8 + 8, (my div 8) * 8 + 8, red)
    end if
    exit when mb = 100
end loop
findPath (map, start, goal)