A* Pathfinding

local thenodes
local master_node_table, mnt_index
local ai_max_range = math.huge

thenodes = workspace:findFirstChild("nodeDir", true)


function searchByBrick(brick)
    for i, j in pairs(master_node_table) do
        if j.Brick == brick then
            return j.ID
        end
    end
    return nil
end

function searchById(id)
    for i, j in pairs(master_node_table) do
        if j.ID == id then
            return j.Brick
        end
    end
    return nil
end



function nodeObjectFromBrick(brick)
    if brick.ClassName ~= "Part" then
        return nil
    end
    local node_index = mnt_index
    master_node_table[node_index] = {
        ID = mnt_index,
        Brick = brick,
        Connections = {}
    }
    for i, child in pairs(brick:GetChildren()) do
        if child.ClassName == "ObjectValue" and child.Name == "connection" then
            local brick2 = child.Value
            local ID = searchByBrick(brick2)
            if ID == nil then
                mnt_index = mnt_index + 1
                ID = nodeObjectFromBrick(brick2)
            end
            local con = {
                ID = ID,
                G = (master_node_table[ID].Brick.Position - brick.Position).magnitude
            }
            table.insert(master_node_table[node_index].Connections, con)
        end
    end

    return node_index
end


function collectNodes(model)
    master_node_table = {}
    mnt_index = 1
    for i, child in pairs(model:GetChildren()) do
        if child.ClassName == "Part" and searchByBrick(child) == nil then
            nodeObjectFromBrick(child)
            mnt_index = mnt_index + 1
        end
    end
end


function heuristic(id1, id2)
    local p1 = master_node_table[id1].Brick.Position
    local p2 = master_node_table[id2].Brick.Position
    return (p1 - p2).magnitude
end


function len(t)
    local l = 0
    for i, j in pairs(t) do
        if j ~= nil then
            l = l + 1
        end
    end
    return l
end


function getPath(t, n)
    if t[n] == nil then
        return {n}
    else
        local t2 = getPath(t, t[n])
        table.insert(t2, n)
        return t2
    end
end


function AStar(startID, endID)
    --local now = tick()
    local closed = {}
    local open = {startID}
    local previous = {}
    local g_score = {}
    local f_score = {}

    g_score[startID] = 0
    f_score[startID] = heuristic(startID, endID)

    while len(open) > 0 do
        local current, current_i = nil, nil
        for i, j in pairs(open) do
            if current == nil then
                current = j
                current_i = i
            else
                if j ~= nil then
                    if f_score[j] < f_score[current] then
                        current = j
                        current_i = i
                    end
                end
            end
        end

        if current == endID then
            local path = getPath(previous, endID)
            local ret = {}
            for i, j in pairs(path) do
                table.insert(ret, master_node_table[j].Brick)
            end
            --print("Time taken for AStar to run: "..tostring(tick() - now))
            return ret
        end

        open[current_i] = nil
        table.insert(closed, current)

        for i, j in pairs(master_node_table[current].Connections) do
            local in_closed = false
            for k, l in pairs(closed) do
                if l == j.ID then
                    in_closed = true
                    break
                end
            end
            if in_closed == false then
                local tentative_score = g_score[current] + j.G
                local in_open = false
                for k, l in pairs(open) do
                    if l == j.ID then
                        in_open = true
                        break
                    end
                end
                if in_open == false or tentative_score < g_score[j.ID] then
                    previous[j.ID] = current
                    g_score[j.ID] = tentative_score
                    f_score[j.ID] = g_score[j.ID] + heuristic(j.ID, endID)
                    if in_open == false then
                        table.insert(open, j.ID)
                    end
                end
            end
        end
    end
    --print("Time taken for AStar to run: "..tostring(tick() - now))
    return nil
end



function getNearestNode(position,returnBrick,dir)
    local nodeDir = dir
    local smallest = ai_max_range
    local nodes = {}
    if type(dir) ~= "table" then
        nodeDir = dir:children()
    end
    for k,v in pairs(nodeDir) do
        if v:IsA("BasePart") then
            local dist
            dist = (position.Position - v.Position).magnitude
            nodes[searchByBrick(v)] = dist
            if dist <= smallest then
                smallest = dist
            end
        end
    end
    for k,v in pairs(nodes) do
        print(k)
        if v <= smallest then
            if returnBrick then
                return searchById(k)
            else
                return k
            end
        end
    end
end



collectNodes(thenodes)  --generate nodegraph
local start = getNearestNode(workspace.Start, true, thenodes)
local goal = getNearestNode(workspace.Goal, true, thenodes)

local path = AStar(searchByBrick(start), searchByBrick(goal)) --returns table of parts(nodes), in order from start to goal