navigation.lua

function newNav()
    inav = {}

    -- location: {x, y, z, direction(0123 = s w n e)}
    inav.loc = {nil, nil, nil, 0}

    -- file operation functions to save location and progress

    function inav.save(name, data)
        local file = fs.open(name, "w")
        file.write(textutils.serialize(data))
        file.close()
    end

    function inav.load(name)
        local file = fs.open(name, "r")
        local data = file.readAll()
        file.close()
        return textutils.unserialize(data)
    end

    -- basic movement functions --
    function inav.forward()
        if turtle.forward() then
            if inav.loc[4]%4 == 0 then inav.loc[3] = inav.loc[3] + 1
            elseif inav.loc[4]%4 == 1 then inav.loc[1] = inav.loc[1] - 1
            elseif inav.loc[4]%4 == 2 then inav.loc[3] = inav.loc[3] - 1
            elseif inav.loc[4]%4 == 3 then inav.loc[1] = inav.loc[1] + 1
            else print("moved, but location not updated. ERROR!") end
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    function inav.back()
        if turtle.back() then
            if inav.loc[4]%4 == 0 then inav.loc[3] = inav.loc[3] - 1
            elseif inav.loc[4]%4 == 1 then inav.loc[1] = inav.loc[1] + 1
            elseif inav.loc[4]%4 == 2 then inav.loc[3] = inav.loc[3] + 1
            elseif inav.loc[4]%4 == 3 then inav.loc[1] = inav.loc[1] - 1
            else print("moved, but location not updated. ERROR!") end
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    function inav.up()
        if turtle.up() then
            inav.loc[2] = inav.loc[2] + 1
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    function inav.down()
        if turtle.down() then
            inav.loc[2] = inav.loc[2] - 1
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    function inav.turnLeft()
        if turtle.turnLeft() then
            inav.loc[4] = inav.loc[4] - 1
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    function inav.turnRight()
        if turtle.turnRight() then
            inav.loc[4] = inav.loc[4] + 1
            inav.save("location", inav.loc)
            return true
        end
        return false
    end

    -- set current coordinates
    function inav.setPos(x, y, z, direction)
        inav.loc = {x, y, z, direction}
        inav.save("location", inav.loc)
    end

    -- get current coordinates
    function inav.getPos()
        return {inav.loc[1], inav.loc[2], inav.loc[3], inav.loc[4]%4}
    end

    -- read location from save
    function inav.readLocationSave()
        inav.loc = inav.load("location")
    end

    -- rotate to direction
    function inav.rotate(direction)
        local curDir = inav.loc[4]%4
        local dir = direction%4
        if math.abs(curDir - dir) == 2 then
            while inav.turnLeft() == false do end
            while inav.turnLeft() == false do end
        elseif (curDir - 1)%4 == dir then
            while inav.turnLeft() == false do end
        elseif (curDir + 1)%4 == dir then
            while inav.turnRight() == false do end
        end
    end

    -- move one step in direction (dig)
    function inav.move(direction)
        inav.rotate(direction)
        turtle.dig()
        return inav.forward()
    end

    -- move up (dig)
    function inav.moveUp()
        turtle.digUp()
        return inav.up()
    end

    -- move down (dig)
    function inav.moveDown()
        turtle.digDown()
        return inav.down()
    end

    -- internal function to convert vector to unit vector
    function inav.getUnitVector(vector)
        local unitVec = {0, 0, 0,}
        for i = 1,3 do
            if vector[i] ~= 0 then
                unitVec[i] = vector[i]/math.abs(vector[i])
            end
        end
        return unitVec
    end

    -- internal function to convert direction to vector
    function inav.getRotationVector(direction)
        local vector = {0, 0}
        if direction%4 == 0 then vector[2] = 1
        elseif direction%4 == 1 then vector[1] = -1
        elseif direction%4 == 2 then vector[2] = -1
        elseif direction%4 == 3 then vector[1] = 1 end
        return vector
    end

    -- internal function to choose between east/west or north/south
    function inav.getShortestDirections(unitVector, dirVector)
        local shortest = {3, 3}
        if unitVector[1] ~= 0 then -- if not only going east/west
            shortest[1] = math.abs(unitVector[1] - dirVector[1])
        end
        if unitVector[3] ~= 0 then -- if not only going north/south
            shortest[2] = math.abs(unitVector[3] - dirVector[2])
        end
        if unitVector[1] == 0 and unitVector[3] == 0 then -- if not moving at all
            return nil
        end
        return shortest
    end

    -- internal function to choose quickest direction
    function inav.chooseShortestRotation(vector, curDir)
        local deltaVector = inav.getUnitVector(vector)
        local curVector = inav.getRotationVector(curDir)
        local shortest = inav.getShortestDirections(deltaVector, curVector)

        if shortest == nil then return curDir end

        if shortest[1] < shortest[2] then -- if we're choosing east/west
            if deltaVector[1] > 0 then -- if we're choosing east
                return 3
            else -- else we choose west
                return 1
            end
        else -- else it is north/south
            if deltaVector[3] > 0 then -- if we're choosing south
                return 0
            else --else we choose north
                return 2
            end
        end
        print("somthing went wrong when choosing direction")
    end

    -- internal function to rotate to the other shortest direction
    function inav.otherTurn(vector, curDir)
        local unitVector = inav.getUnitVector(vector)
        local dirVector = inav.getRotationVector(curDir)

        if dirVector[1] == 1 or dirVector[1] == -1 then
            if unitVector[3] == 1 then return 0 end
            if unitVector[3] == -1 then return 2 end
        elseif dirVector[2] == 1 or dirVector[2] == -1 then
            if unitVector[1] == 1 then return 3 end
            if unitVector[1] == -1 then return 1 end
        end
        return curDir -- no better turns
    end

    -- move to position and hopefully quickest way
    function inav.moveTo(x, y, z)
        local target = {}
        if type(x) == "table" then target = x
        else target = {x, y, z} end

        inav.save("lastMoveJob", target) -- save in case of interruption

        local vector = {target[1] - inav.loc[1], target[2] - inav.loc[2], target[3] - inav.loc[3]}
        local bestRotation = nil
        local newRotation = nil
        local vertical = nil -- vertical toggle to know vertical direction
        if vector[2] > 0 then vertical = 1
        elseif vector[2] < 0 then vertical = -1
        else vertical = 0 end

        -- loop to target
        while vector[1] ~= 0 or vector[2] ~= 0 or vector[3] ~= 0 do
            bestRotation = inav.chooseShortestRotation(vector, inav.loc[4])
            inav.rotate(bestRotation)

            if vector[1] ~= 0 or vector[3] ~= 0 then -- try to move horisontal first
                if turtle.detect() then -- if block in front
                    if vertical == 1 and not turtle.detectUp() then -- can we go up?
                        inav.moveUp()
                    elseif vertical == -1 and not turtle.detectDown() then -- can we go down?
                        inav.moveDown()
                    else -- try to turn
                        newRotation = inav.otherTurn(vector, bestRotation)
                        if newRotation == bestRotation then -- if no other turns
                            inav.move(bestRotation)
                        else
                            inav.move(newRotation) -- move (and dig) the last direction
                        end
                    end
                else -- if no blocks, then move
                    inav.move(bestRotation)
                end
            else --by now we should be on x and z target
                if vertical == 1 then
                    inav.moveUp()
                elseif vertical == -1 then
                    inav.moveDown()
                end
            end

            -- update vector and vertical toggle
            vector = {target[1] - inav.loc[1], target[2] - inav.loc[2], target[3] - inav.loc[3]}
            if vector[2] > 0 then vertical = 1
            elseif vector[2] < 0 then vertical = -1
            else vertical = 0 end
        end
    end

    function inav.resumeMove()
        local lastMove = inav.load("lastMoveJob")
        inav.moveTo(lastMove)
    end

    if fs.exists("location") then
        inav.readLocationSave()
    end

    return inav
end