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

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 .. 5, 1 .. 5 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 .. 50 of Node
    var numOnOpenList, numOnClosedList, lowestFNum : int := 1
    var lowestFVal : int := 100
    var tieCount, pathCount, tempNum : int := 0
    var foundNode : boolean := false
    var tieNums : array 1 .. 20 of int

    for i : 1 .. 20
        openList (i).x := 0
        openList (i).y := 0
        openList (i).f := 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
    currentNode := startNode
    numOnOpenList := 0
    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
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).x -= 1
            openList (numOnOpenList).f := (goalNode.x - openList (numOnOpenList).x) + abs (goalNode.y - openList (numOnOpenList).y)
        end if
        % DOWN
        if currentNode.y >= 2 and map (currentNode.x, currentNode.y - 1) not= 5 then
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).y -= 1
            openList (numOnOpenList).f := (goalNode.x - openList (numOnOpenList).x) + abs (goalNode.y - openList (numOnOpenList).y)
        end if
        % RIGHT
        if currentNode.x <= 4 and map (currentNode.x + 1, currentNode.y) not= 5 then
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).x += 1
            openList (numOnOpenList).f := (goalNode.x - openList (numOnOpenList).x) + abs (goalNode.y - openList (numOnOpenList).y)
        end if
        % UP
        if currentNode.y <= 4 and map (currentNode.x, currentNode.y + 1) not= 5 then
            numOnOpenList += 1
            openList (numOnOpenList) := currentNode
            openList (numOnOpenList).y += 1
            openList (numOnOpenList).f := (goalNode.x - openList (numOnOpenList).x) + abs (goalNode.y - openList (numOnOpenList).y)
        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
                    foundNode := true
                    currentNode := openList (i)
                    tempNum := i
                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
        pathCount += 1
        path (pathCount) := currentNode


        numOnOpenList := 1
        lowestFNum := 1
        lowestFVal := 100
        tieCount := 1
        foundNode := false
    end loop

    for i : 1 .. 20
        if path (i).x not= 0 then
            put i, ": ", path (i).x, " ", path (i).y
        end if
    end for
end findPath

var map : array 1 .. 5, 1 .. 5 of int
for i : 1 .. 5
    for j : 1 .. 5
        map (i, j) := 2
    end for
end for
var start, goal : Node
start.x := 1
start.y := 2
start.f := 2
goal.x := 5
goal.y := 5
findPath (map, start, goal)