Turtle Pathfinder (ComputerCraft)

-- Path Finder
-- ----------------------------------------------------------

-- By Jeffrey Alexander, aka Bomb Bloke.
-- Tries to get from A to B.

-- If a GPS is available, will prompt for the co-ords of its
-- destination and attempt to pathfind there.

-- Otherwise, it will search its inventory for a block, and
-- if anything is present it will attempt to find a match for
-- it within the world.

-- Failing both of the above, the turtle will wander until
-- its exploration database overloads...

-- ----------------------------------------------------------

-- Initialise important values:

-- ----------------------------------------------------------

local facing = {"north","east","south","west","up","down"}
local node, direction, curnode, x, y, z, targetX, targetY, targetZ, pathList, gpsAccess, comparing = {}

-- ----------------------------------------------------------

-- Functions and stuff:

-- ----------------------------------------------------------

-- :)
local function win()
    print("Looks like I made it out. Done! I'M FREE!")
    error()
end

-- Returns true if the turtle is at the specified node.
local function atNode(tnode) return (x == node[tnode][1] and y == node[tnode][2] and z == node[tnode][3]) end

-- Returns the index of a given value if it exists in a given table, or nil if it does not.
local function whereIn(targetTable, targetValue) for i=1,#targetTable do if targetTable[i] == targetValue then return i end end end

-- Returns the best direction to explore in from a given node. Sorta.
local function bestDirection(thisNode)
    if gpsAccess then
        local validDir = {}
        for i=1,6 do validDir[facing[i]] = whereIn(node[thisNode].explore, facing[i]) end

        if z < node[1][3] and validDir["up"]    then return table.remove(node[thisNode].explore, validDir["up"])    end
        if z > node[1][3] and validDir["down"]  then return table.remove(node[thisNode].explore, validDir["down"])  end
        if x < node[1][1] and validDir["east"]  then return table.remove(node[thisNode].explore, validDir["east"])  end
        if x > node[1][1] and validDir["west"]  then return table.remove(node[thisNode].explore, validDir["west"])  end
        if y < node[1][2] and validDir["south"] then return table.remove(node[thisNode].explore, validDir["south"]) end
        if y > node[1][2] and validDir["north"] then return table.remove(node[thisNode].explore, validDir["north"]) end
    end

    return table.remove(node[thisNode].explore)
end

-- Accepts strings representing compass-facings to turn the turtle.
local function faceDirection(targetdirection)
    local tardir
    for i=1,4 do if targetdirection == facing[i] then
        tardir = i
        break
    end end

    if tardir < direction then
        if tardir == 1 and direction == 4 then while not turtle.turnRight() do end
        else for i=1,direction-tardir do while not turtle.turnLeft() do end end end
    elseif tardir > direction then
        if tardir == 4 and direction == 1 then while not turtle.turnLeft() do end
        else for i=1,tardir-direction do while not turtle.turnRight() do end end end
    end

    direction = tardir
end

-- Travel to a co-ordinate.
local function goToPos(targetx,targety,targetz)
    while x ~= targetx or y ~= targety or z ~= targetz do
        if z > targetz then if turtle.down() then z = z - 1 end
        elseif z < targetz then if turtle.up() then z = z + 1 end end

        if x > targetx then
            if direction ~= 4 then faceDirection("west") end
            if turtle.forward() then x = x - 1 end
        elseif x < targetx then
            if direction ~= 2 then faceDirection("east") end
            if turtle.forward() then x = x + 1 end
        end

        if y > targety then
            if direction ~= 1 then faceDirection("north") end
            if turtle.forward() then y = y - 1 end
        elseif y < targety then
            if direction ~= 3 then faceDirection("south") end
            if turtle.forward() then y = y + 1 end
        end
    end
end

-- Travel directly to a node and update the node tracker.
local function goToNode(desnode)
    goToPos(node[desnode][1],node[desnode][2],node[desnode][3])
    curnode = desnode
end

-- Used by the next function to determine the cheapest route to a given destination.
local function checkNode(lastNode,thisNode, desnode)
    local hopDistance = math.abs(node[thisNode][1] - node[lastNode][1]) + math.abs(node[thisNode][2] - node[lastNode][2]) + math.abs(node[thisNode][3] - node[lastNode][3])

    if pathList.bestDistance[thisNode] and pathList.bestDistance[thisNode] <= pathList.curDistance + hopDistance then return end

    os.queueEvent("Wake up!")
    coroutine.yield()

    pathList.curDistance = pathList.curDistance + hopDistance
    pathList.bestDistance[thisNode] = pathList.curDistance
    pathList[#pathList+1] = thisNode

    -- If this is the target node, then record the path.
    if thisNode == desnode then
        pathList.bestPath = {}
        for i=2,#pathList do pathList.bestPath[i-1] = pathList[i] end

        pathList[#pathList] = nil
        pathList.curDistance = pathList.curDistance - hopDistance
        return
    end

    -- Do a quick search in case the target is right next to us:
    for i=4,#node[thisNode] do if node[thisNode][i] == desnode then
        checkNode(thisNode,node[thisNode][i],desnode)

        pathList[#pathList] = nil
        pathList.curDistance = pathList.curDistance - hopDistance
        return
    end end

    -- Check the validity of all attached nodes:
    for i=4,#node[thisNode] do
        alreadyChecked = false

        for j=1,#pathList do if pathList[j] == node[thisNode][i] then
            alreadyChecked = true
            break
        end end

        if not alreadyChecked then checkNode(thisNode,node[thisNode][i],desnode) end
    end

    pathList[#pathList] = nil
    pathList.curDistance = pathList.curDistance - hopDistance
    return
end

-- Travels to a given node, pathing from the current node.
local function pathToNode(desnode)
    goToNode(curnode)
    if curnode == desnode then return end

    -- Quick check:
    for i=4,#node[curnode] do if node[curnode][i] == desnode then
        goToNode(desnode)
        return
    end end

    -- Long check:
    pathList = {curnode, ["curDistance"] = 0, ["bestDistance"] = {}}
    for i=4,#node[curnode] do checkNode(curnode,node[curnode][i],desnode) end
    if not pathList.bestPath then error("I couldn't determine any path from nodes "..curnode.." to "..desnode..".") end
    for i=1,#pathList.bestPath do goToNode(pathList.bestPath[i]) end
end

-- Travels in a given direction, until such time as a new node is discovered.
local function goExplore(targetDirection)
    local nextX, nextY, nextZ, newExplore, reverse = x, y, z

    if targetDirection == "up" then reverse = "down"
    elseif targetDirection == "down" then reverse = "up"
    elseif targetDirection == "north" then reverse = "south"
    elseif targetDirection == "south" then reverse = "north"
    elseif targetDirection == "east" then reverse = "west"
    elseif targetDirection == "west" then reverse = "east" end

    repeat
        if targetDirection == "up" then nextZ = nextZ + 1
        elseif targetDirection == "down" then nextZ = nextZ - 1
        elseif targetDirection == "north" then nextY = nextY - 1
        elseif targetDirection == "south" then nextY = nextY + 1
        elseif targetDirection == "east" then nextX = nextX + 1
        elseif targetDirection == "west" then nextX = nextX - 1 end

        goToPos(nextX,nextY,nextZ)

        newExplore = {}

        if comparing and (turtle.compareUp() or turtle.compareDown()) then win() end

        for i=direction,direction+3 do
            faceDirection(facing[bit.band(i-1,3)+1])
            if comparing and turtle.compare() then win() end
            if facing[bit.band(i-1,3)+1] ~= reverse and not turtle.detect() then newExplore[#newExplore+1] = facing[bit.band(i-1,3)+1] end
        end

        if reverse ~= "up" and not turtle.detectUp() then newExplore[#newExplore+1] = "up" end
        if reverse ~= "down" and not turtle.detectDown() then newExplore[#newExplore+1] = "down" end

        -- Have we found a junction?
        if #newExplore > 1 or newExplore[1] ~= targetDirection then
            for i=1,table.maxn(node) do if node[i] and atNode(i) then
                for j=1,#node[i].explore do if node[i].explore[j] == reverse then
                    table.remove(node[i].explore,j)
                    break
                end end

                node[i][#node[i]+1] = curnode
                node[curnode][#node[curnode]+1] = i
                curnode = i

                return
            end end

            if #node[curnode] > 3 or #node[curnode].explore > 0 then
                local newnode = #node+1
                node[newnode] = {x,y,z,curnode,["explore"] = newExplore}
                node[curnode][#node[curnode]+1] = newnode
                curnode = newnode
            else node[curnode] = {x,y,z,["explore"] = newExplore} end

            return
        end
    until #newExplore == 0

    goToNode(curnode)
end

-- ----------------------------------------------------------

-- I've just booted up. Where am I? Who am I? etc...

-- ----------------------------------------------------------

print("I've been supplied with "..turtle.getFuelLevel().." fuel.")

-- Determine method of travel (target GPS location / hunt for a block / random exploration):
do
    local tempx, tempz, tempy

    for i=1,2 do
        tempx, tempz, tempy = gps.locate(5)

        if tempy then
            gpsAccess = true
            break
        end

        print("GPS cluster not answering; pinging again...")
        sleep(5)
    end

    if gpsAccess then
        print("So... Where are we going? (or mash enter to quit)")

        write("X: ")
        targetX = tonumber(read())

        write("Y: ")
        targetZ = tonumber(read())  -- I use Z for depth, like a sensible person.

        write("Z: ")
        targetY = tonumber(read())

        print("")

        if not (targetX and targetY and targetZ) then
            print("At least one of those co-ords was invalid. Exiting...")
            error()
        end

        local moved, goingUp = false, true

        while not moved do
            for i=1,4 do if turtle.forward() then
                moved = true
                break
            else turtle.turnLeft() end end

            if not moved then
                if goingUp then
                    if not turtle.up() then
                        goingUp = false
                        turtle.down()
                    end
                elseif not turtle.down() then
                    print("Cannot determine facing: I'm boxed in.")
                    print("I'M BOXED IN.")
                    print("CAN'T BREATH!")
                    print("AAAAUUUUUGGGHHH")
                    error("Suffocated")
                end
            end
        end

        x, z, y = gps.locate(5)

        if x < tempx then direction = 4
        elseif x > tempx then direction = 2
        elseif y < tempy then direction = 1
        else direction = 3 end

        print("I'm at "..x..","..z..","..y.." and I'm facing "..facing[direction]..".")

        -- Register target as a node:
        node[1] = {targetX,targetY,targetZ,["explore"]={}}
    else
        x, z, y, direction = 0, 0, 0, 1

        write("I can't find a valid GPS cluster; I don't really know where I am. ")

        -- Check to see if there's an item in our inventory we can search for in the maze:
        for i=1,16 do if turtle.getItemCount(i) > 0 then
            print("But since there's something in my inventory, I'll assume I should search for a match for it within a maze.")

            comparing = true
            turtle.select(i)
            break
        end end

        if not comparing then print("And since there's nothing in my inventory, that means I'll be flying blind. Pickaxe me or something if I ever make it out of here.") end
    end
end

-- Register starting position as a node:
node[2] = {x,y,z,["explore"]={}}

for i=direction,direction+3 do
    faceDirection(facing[bit.band(i-1,3)+1])
    if not turtle.detect() then node[2].explore[#node[2].explore+1] = facing[bit.band(i-1,3)+1] end
end

if not turtle.detectUp() then node[2].explore[#node[2].explore+1] = "up" end
if not turtle.detectDown() then node[2].explore[#node[2].explore+1] = "down" end

curnode = 2

print("")
print("Exploring in progress...")
print("")

-- ----------------------------------------------------------

-- Main program loop:

-- ----------------------------------------------------------

while true do
    local nextnode = #node[curnode].explore == 0 and table.maxn(node) or curnode

    while node[nextnode] == nil or #node[nextnode].explore == 0 do
        nextnode = nextnode - 1
        if nextnode == 0 then error("Hrm... Doesn't look like there's any way out of here...") end
    end

    pathToNode(nextnode)
    goExplore(bestDirection(nextnode))

    if gpsAccess and atNode(1) then win() end

    while #node[curnode].explore == 0 and #node[curnode] == 4 do
        local deadnode = curnode
        goToNode(node[curnode][4])

        for i=4,#node[curnode] do if node[curnode][i] == deadnode then
            table.remove(node[curnode],i)
            break
        end end

        node[deadnode] = nil
    end
end