View.Set ("graphics:1280:768")

% Pathfinding

type node :
    record
        x, y, parentX, parentY : int
    end record

var nodeFirst, nodeLast, nodeNextLast, numNodes : int := 1
var nodeX, nodeY : int
var endNode : node
var retVal, pathVal : int
var reachEnd : boolean := false
var start, goal, parent : node
var atNode : array 1 .. 625 of node
var numTiles : int := 25
var map : array 1 .. numTiles, 1 .. numTiles of int
var mapVals : array 1 .. numTiles, 1 .. numTiles of int
var expandedNode : array 1 .. numTiles, 1 .. numTiles of boolean
var val, pathMinVal : int
var nodeNbr : int := 1
var pathToFollow : array 1 .. 401 of node
var foundPath : boolean := false
var moveCost := 2

var font := Font.New ("verdana:8")

for i : 1 .. upper (atNode)
    atNode (i).x := 0
    atNode (i).y := 0
end for

for i : 1 .. numTiles
    for j : 1 .. numTiles
        map (i, j) := 2
        mapVals (i, j) := 1
        expandedNode (i, j) := false
    end for
end for

function findCombinations (point : node, var arrayStart : int) : int
    if point.x - 1 = 0 or point.x + 1 = (numTiles + 1) or point.y - 1 = 0 or point.y + 1 = (numTiles + 1) then
        result 0
    end if
    if (point.x - 1 >= 1) and expandedNode (point.x - 1, point.y) = false then
        atNode (nodeNbr).x := point.x - 1
        atNode (nodeNbr).y := point.y
        nodeNbr += 1
        expandedNode (point.x - 1, point.y) := true
        atNode (nodeNbr).parentX := point.x
        atNode (nodeNbr).parentY := point.y
    end if
    if (point.y - 1 >= 1) and expandedNode (point.x, point.y - 1) = false then
        atNode (nodeNbr).x := point.x
        atNode (nodeNbr).y := point.y - 1
        expandedNode (point.x, point.y - 1) := true
        nodeNbr += 1
        atNode (nodeNbr).parentX := point.x
        atNode (nodeNbr).parentY := point.y
    end if
    if (point.x + 1 <= numTiles) and expandedNode (point.x + 1, point.y) = false then
        atNode (nodeNbr).x := point.x + 1
        atNode (nodeNbr).y := point.y
        expandedNode (point.x + 1, point.y) := true
        nodeNbr += 1
        atNode (nodeNbr).parentX := point.x
        atNode (nodeNbr).parentY := point.y
    end if
    if (point.y + 1 <= numTiles) and expandedNode (point.x, point.y + 1) = false then
        atNode (nodeNbr).x := point.x
        atNode (nodeNbr).y := point.y + 1
        expandedNode (point.x, point.y + 1) := true
        nodeNbr += 1
        atNode (nodeNbr).parentX := point.x
        atNode (nodeNbr).parentY := point.y
    end if
    result nodeNbr - arrayStart
end findCombinations

function isValid (num : int) : boolean
    if num >= 1 then
        result true
    end if
    result false
end isValid

function getNodeValue (x, y : int) : int
    pathMinVal := 500
    % Check if it is an impossible tile...
    if x = 0 or y = 0 then
        result - 2
    end if
    if map (x, y) = -1 then
        result - 1
    end if

    if (x - 1) >= 1 then
        val := mapVals (x - 1, y)
        if isValid (val) then
            if val <= pathMinVal or pathMinVal = 0 then
                pathMinVal := val
            end if
        end if
    end if
    if (x + 1) <= numTiles then
        val := mapVals (x + 1, y)
        if isValid (val) then
            if val <= pathMinVal or pathMinVal = 0 then
                pathMinVal := val
            end if
        end if
    end if
    if (y - 1) >= 1 then
        val := mapVals (x, y - 1)
        if isValid (val) then
            if val <= pathMinVal or pathMinVal = 0 then
                pathMinVal := val
            end if
        end if
    end if
    if (y + 1) <= numTiles then
        val := mapVals (x, y + 1)
        if isValid (val) then
            if val <= pathMinVal or pathMinVal = 0 then
                pathMinVal := val
            end if
        end if
    end if
    result pathMinVal
end getNodeValue

function tracePath (point, start, goal : node) : node
    var xFactor, yFactor, absVal, absTestVal, lowestAbsVal : int := 0
    pathMinVal := 500
    lowestAbsVal := 500
    var resultPt : node
    resultPt.x := point.x
    resultPt.y := point.y
    % X VALUES
    % Is it in the bounds?
    if point.x - 1 = goal.x and point.y = goal.y then
        result goal
    elsif (point.x - 1) >= 1 and expandedNode (point.x - 1, point.y) = true then
        % Find value
        val := mapVals (point.x - 1, point.y)
        locate (3, maxcol - 2)
        if isValid (val) and val > 1 then
            put val
            absTestVal := abs ((start.x - (point.x - 1)) ** 2) + abs ((start.y - point.y) ** 2)
            if val < pathMinVal or pathMinVal = 0 then
                pathMinVal := val
                xFactor := -1
                yFactor := 0
                lowestAbsVal := absTestVal
            elsif val = pathMinVal and absTestVal <= lowestAbsVal then
                pathMinVal := val
                xFactor := -1
                yFactor := 0
                lowestAbsVal := absTestVal
            end if
        end if
    end if

    if point.x + 1 = goal.x and point.y = goal.y then
        result goal
    elsif (point.x + 1) <= numTiles and expandedNode (point.x + 1, point.y) = true then
        % Find value
        val := mapVals (point.x + 1, point.y)
        if isValid (val) and val > 1 then
            absTestVal := abs ((start.x - (point.x + 1)) ** 2) + abs ((start.y - point.y) ** 2)
            if val < pathMinVal or pathMinVal = 0 then
                pathMinVal := val
                xFactor := 1
                yFactor := 0
            elsif val = pathMinVal and absTestVal <= lowestAbsVal then
                pathMinVal := val
                xFactor := 1
                yFactor := 0
                lowestAbsVal := absTestVal
            end if
        end if
    end if
    if point.x = goal.x and point.y - 1 = goal.y then
        result goal
    elsif (point.y - 1) >= 1 and expandedNode (point.x, point.y - 1) = true then
        % Find value
        val := mapVals (point.x, point.y - 1)
        if isValid (val) and val > 1 then
            absTestVal := abs ((start.x - point.x) ** 2) + abs ((start.y - (point.y - 1)) ** 2)
            if val < pathMinVal or pathMinVal = 0 then
                pathMinVal := val
                xFactor := 0
                yFactor := -1
            elsif val = pathMinVal and absTestVal <= lowestAbsVal then
                pathMinVal := val
                xFactor := 1
                yFactor := 0
                lowestAbsVal := absTestVal
            end if
        end if
    end if
    if point.x = goal.x and point.y + 1 = goal.y then
        result goal
    elsif (point.y + 1) <= numTiles and expandedNode (point.x, point.y + 1) = true then
        % Find value
        val := mapVals (point.x, point.y + 1)
        if isValid (val) and val < 1 then
            absTestVal := abs ((start.x - point.x) ** 2) + abs ((start.y - (point.y + 1)) ** 2)
            if val < pathMinVal or pathMinVal = 0 then
                pathMinVal := val
                xFactor := 0
                yFactor := 1
            elsif val = pathMinVal and absTestVal <= lowestAbsVal then
                pathMinVal := val
                xFactor := 1
                yFactor := 0
                lowestAbsVal := absTestVal
            end if
        end if
    end if
    if pathMinVal not= 0 then
        resultPt.x += xFactor
        resultPt.y += yFactor
    end if
    result resultPt
end tracePath

proc choosePath (start, goal : node, points : array 1 .. * of node)
    var currentPoint : node
    var count := 1
    currentPoint := start

    loop
        if foundPath = false then
            locate (1, maxcol)
            drawfillbox (1 + currentPoint.x * 16, 1 + currentPoint.y * 16, 15 + currentPoint.x * 16, 15 + currentPoint.y * 16, blue)
            drawfillbox (goal.x * 16, goal.y * 16, 16 + goal.x * 16, 16 + goal.y * 16, red)
            currentPoint := tracePath (currentPoint, start, goal)
            drawfillbox (1 + currentPoint.x * 16, 1 + currentPoint.y * 16, 15 + currentPoint.x * 16, 15 + currentPoint.y * 16, green)
            exit when currentPoint.x = goal.x and currentPoint.y = goal.y
        end if
    end loop
end choosePath

proc pathSearch (start, goal : node)
    % Prepare
    endNode.x := goal.x
    endNode.y := goal.y

    % Initialize first node (starting location)
    atNode (1).x := start.x
    atNode (1).y := start.y
    atNode (1).parentX := start.x
    atNode (1).parentY := start.y
    nodeFirst := 1
    nodeLast := findCombinations (start, numNodes)
    mapVals (start.x, start.y) := map (start.x, start.y)
    parent := atNode (1)
    loop
        nodeNextLast := nodeLast
        for a : nodeFirst .. nodeLast
            % Store the x and y values for speed
            nodeX := atNode (a).x
            nodeY := atNode (a).y

            pathVal := getNodeValue (nodeX, nodeY)
            if pathVal = -1 then
                mapVals (nodeX, nodeY) := -1
            elsif pathVal > 0 then
                % Adds the lowest value of closest tile to the current value of my tile
                mapVals (nodeX, nodeY) := mapVals (atNode (a).parentX, atNode (a).parentY) + pathVal

                % Draw it in
                Font.Draw (intstr (mapVals (nodeX, nodeY)), nodeX * 16 + 2, nodeY * 16 + 2, font, black)

                % Find the next 4 nodes
                retVal := findCombinations (atNode (a), nodeNextLast)
                nodeNextLast := nodeNextLast + retVal

                % Destination reached?
                if nodeX = endNode.x and nodeY = endNode.y then
                    reachEnd := true
                end if
            end if
        end for
        nodeFirst := nodeLast + 1
        nodeLast := nodeNextLast
        exit when reachEnd = true
        exit when nodeFirst > nodeLast
    end loop
    if reachEnd = true then
        choosePath (goal, start, atNode)
    end if
end pathSearch

var temp, mx, my, mb : int

start.x := 15
start.y := 2
goal.x := 7
goal.y := 8
for i : 1 .. numTiles
    for j : 1 .. numTiles
        drawbox (i * 16, j * 16, 16 + i * 16, 16 + j * 16, black)
    end for
end for

buttonchoose ("multibutton")

loop
    mousewhere (mx, my, mb)
    if mx < (16 * numTiles) and my < (16 * numTiles) and mb = 1 then
        map (mx div 16, my div 16) := -1
        drawfillbox ((mx div 16) * 16, (my div 16) * 16, 16 + (mx div 16) * 16, 16 + (my div 16) * 16, red)
    end if
    exit when mb = 100
end loop

pathSearch (start, goal)
% for i : 1 .. 10
%     put pathToFollow (i).x, " ", pathToFollow (i).y
% end for