Light GPS

local pStorage
local pSave

local api = {}
local NAN = 0 / 0
local INDEX_DIMENSIONS = 1
local INDEX_ID = 2
local INDEX_ADJACENT = 3
local INDEX_CHILDREN = 4
local INDEX_PARENT = 5
local INDEX_IS_LEAF = 6
local NODE_INDEX_POS = 1
local NODE_INDEX_CUBOID = 2
local currentDirectionPrivate
local turtle_unhooked_turnLeft
local turtle_unhooked_turnRight
local logCuboids
local DODGE_PROTOCOL = "TURTI_LIGHT_GPS_DODGE_PROTOCOL"

-- region utility
local function isNan(v)
    return v ~= v
end

local function posEquals(p1, p2)
    return p1[1] == p2[1] and p1[2] == p2[2] and p1[3] == p2[3]
end

local function removeValue(tbl, value)
    for i, v in pairs(tbl) do
        if v == value then
            table.remove(tbl, i)
            return
        end
    end
end

local function isValueInRange(v, min, max)
    return (min ~= min or min <= v) and
            (max ~= max or v < max)
end

local function putValueInRange(v, min, max)
    if not isNan(min) and v < min then
        return min
    elseif not isNan(max) and v >= max then
        return max - 1
    else
        return v
    end
end

local function rangesOverlap(p1x1, p1x2, p2x1, p2x2)
    if isNan(p1x1) and isNan(p1x2) then
        return true
    elseif isNan(p2x1) and isNan(p2x2) then
        return true
    elseif isNan(p1x1) and isNan(p2x1) then
        return true
    elseif isNan(p1x2) and isNan(p2x2) then
        return true
    end

    if isNan(p1x1) then
        p1x1 = p2x1
    elseif isNan(p2x1) then
        p2x1 = p1x1
    end
    if isNan(p1x2) then
        p1x2 = p2x2
    elseif isNan(p2x2) then
        p2x2 = p1x2
    end

    return math.max(p1x2, p2x2) - math.min(p1x1, p2x1) < p2x2 - p2x1 + p1x2 - p1x1
end

local function isPointInCuboid(pos, cuboid)
    local dimensions = cuboid[INDEX_DIMENSIONS]
    return isValueInRange(pos[1], dimensions[1], dimensions[4]) and
            isValueInRange(pos[2], dimensions[2], dimensions[5]) and
            isValueInRange(pos[3], dimensions[3], dimensions[6])
end

local function isEmptyCuboid(cuboid)
    local dimensions = cuboid[INDEX_DIMENSIONS]
    return dimensions[1] == dimensions[4] or dimensions[2] == dimensions[5] or dimensions[3] == dimensions[6]
end

local function normalizeDirection(direction)
    while direction < 0 do
        direction = direction + 4
    end
    return direction % 4
end

local function getDirectionFromPoints(pStart, pEnd)
    local x1, y1, z1 = table.unpack(pStart)
    local x2, y2, z2 = table.unpack(pEnd)
    if x1 ~= x2 then
        if y1 == y2 and z1 == z2 then
            if x1 < x2 then
                return 0
            else
                return 2
            end
        end
    elseif y1 ~= y2 then
        if z1 == z2 then
            if y1 < y2 then
                return 4
            else
                return 5
            end
        end
    elseif z1 ~= z2 then
        if z1 < z2 then
            return 1
        else
            return 3
        end
    end
    printPoly(pStart[1], pStart[2], pStart[3], ",", pEnd[1], pEnd[2], pEnd[3])
    error("cannot determine direction from points")
end

local function getPosInDirection(pos, direction, count)
    if not count then
        count = 1
    end
    if direction == 0 then
        return { pos[1] + count, pos[2], pos[3] }
    elseif direction == 1 then
        return { pos[1], pos[2], pos[3] + count }
    elseif direction == 2 then
        return { pos[1] - count, pos[2], pos[3] }
    elseif direction == 3 then
        return { pos[1], pos[2], pos[3] - count }
    elseif direction == 4 then
        return { pos[1], pos[2] + count, pos[3] }
    elseif direction == 5 then
        return { pos[1], pos[2] - count, pos[3] }
    end
end

local function arePointsInLine(p1, p2)
    local num = 0
    if p1[1] == p2[1] then
        num = num + 1
    end
    if p1[2] == p2[2] then
        num = num + 1
    end
    if p1[3] == p2[3] then
        num = num + 1
    end
    return num == 2
end

local function arePointsInSurface(p1, p2)
    local num = 0
    if p1[1] == p2[1] then
        num = num + 1
    end
    if p1[2] == p2[2] then
        num = num + 1
    end
    if p1[3] == p2[3] then
        num = num + 1
    end
    return num == 1
end

-- endregion

local function resetCuboids()
    print("reset navigation data!")
    pStorage.currentCuboidIndex = 2
    pStorage.cuboids = {}
    -- dimensions, id in cuboids, adjacent cuboids, children cuboids, parent cuboid, is leaf
    local infiniteCuboid = {}
    infiniteCuboid[INDEX_DIMENSIONS] = { NAN, NAN, NAN, NAN, NAN, NAN }
    infiniteCuboid[INDEX_ID] = 1
    infiniteCuboid[INDEX_CHILDREN] = {}
    infiniteCuboid[INDEX_ADJACENT] = {}
    infiniteCuboid[INDEX_IS_LEAF] = true
    pStorage.cuboids[1] = infiniteCuboid
end

local function getCuboid(id)
    return pStorage.cuboids[id]
end

local function findCuboidContainingPos(pos, suppressError)
    local currentCuboid = pStorage.cuboids[1]
    while not currentCuboid[INDEX_IS_LEAF] do
        local targetChild
        for _, childId in pairs(currentCuboid[INDEX_CHILDREN]) do
            local child = getCuboid(childId)
            if isPointInCuboid(pos, child) then
                targetChild = child
                break
            end
        end
        if targetChild == nil then
            if suppressError then
                return nil
            end
            print("corrupted cuboid data, resetting storage")
            resetCuboids()
            return nil
        end
        currentCuboid = targetChild
    end
    return currentCuboid
end

local function logCuboidStructure(cuboid, prefix)
    local dimensions = cuboid[INDEX_DIMENSIONS]
    printPoly(prefix, dimensions[1], dimensions[2], dimensions[3],
            dimensions[4], dimensions[5], dimensions[6])

    for _, id in pairs(cuboid[INDEX_CHILDREN]) do
        logCuboidStructure(getCuboid(id), prefix .. " ")
    end
end

function logCuboids()
    logCuboidStructure(pStorage.cuboids[1], "")
end

local function registerCuboid(cuboid)
    if cuboid[INDEX_ID] ~= nil then
        return
    end
    local index = pStorage.currentCuboidIndex
    pStorage.currentCuboidIndex = index + 1
    pStorage.cuboids[index] = cuboid
    cuboid[INDEX_ID] = index
end

local function areCuboidsAdjacent(c1, c2)
    local dim1 = c1[INDEX_DIMENSIONS]
    local dim2 = c2[INDEX_DIMENSIONS]
    local sum1 = 0
    local sum2 = 0
    if rangesOverlap(dim1[1], dim1[4], dim2[1], dim2[4]) then
        sum1 = sum1 + 1
    elseif dim1[1] == dim2[4] or dim1[4] == dim2[1] then
        sum2 = sum2 + 1
    end
    if rangesOverlap(dim1[2], dim1[5], dim2[2], dim2[5]) then
        sum1 = sum1 + 1
    elseif dim1[2] == dim2[5] or dim1[5] == dim2[2] then
        sum2 = sum2 + 1
    end
    if rangesOverlap(dim1[3], dim1[6], dim2[3], dim2[6]) then
        sum1 = sum1 + 1
    elseif dim1[3] == dim2[6] or dim1[6] == dim2[3] then
        sum2 = sum2 + 1
    end
    if sum1 > 2 then
        printTable(dim1)
        printTable(dim2)
        error("impossible case " .. sum1 .. ", " .. sum2)
    end
    return sum1 == 2 and sum2 == 1
end

-- region transform cuboids

local function splitCuboidAtPos(cuboid, pos)
    local x1, y1, z1, x2, y2, z2 = table.unpack(cuboid[INDEX_DIMENSIONS])
    local pX, pY, pZ = table.unpack(pos)

    local cuboidYM = { x1, y1, z1, x2, pY, z2 }
    local cuboidYP = { x1, pY + 1, z1, x2, y2, z2 }
    local cuboidXM = { x1, pY, z1, pX, pY + 1, z2 }
    local cuboidXP = { pX + 1, pY, z1, x2, pY + 1, z2 }
    local cuboidZM = { pX, pY, z1, pX + 1, pY + 1, pZ }
    local cuboidZP = { pX, pY, pZ + 1, pX + 1, pY + 1, z2 }
    local allCuboids = { cuboidYM, cuboidYP, cuboidXM, cuboidXP, cuboidZM, cuboidZP }

    local cuboids = {}
    for _, dimensions in pairs(allCuboids) do
        local newCuboid = {}
        newCuboid[INDEX_IS_LEAF] = true
        newCuboid[INDEX_ADJACENT] = {}
        newCuboid[INDEX_CHILDREN] = {}
        newCuboid[INDEX_DIMENSIONS] = dimensions
        if not isEmptyCuboid(newCuboid) then
            table.insert(cuboids, newCuboid)
        end
    end

    return cuboids
end

local function markAsObstructed(pos)
    local cuboid = findCuboidContainingPos(pos)
    if cuboid == nil then
        return
    end
    local newCuboids = splitCuboidAtPos(cuboid, pos)

    if not cuboid[INDEX_IS_LEAF] then
        error("corrupted cuboid data (can't split non-leaf node)")
    end

    -- register new cuboids
    for _, c in pairs(newCuboids) do
        registerCuboid(c)
        c[INDEX_IS_LEAF] = true
        c[INDEX_PARENT] = cuboid[INDEX_ID]
        table.insert(cuboid[INDEX_CHILDREN], c[INDEX_ID])
    end
    cuboid[INDEX_IS_LEAF] = false

    -- add adjacent relations between child cuboids
    for _, newCuboid in pairs(newCuboids) do
        for _, otherNewCuboid in pairs(newCuboids) do
            if otherNewCuboid[INDEX_ID] ~= newCuboid[INDEX_ID] then
                if areCuboidsAdjacent(newCuboid, otherNewCuboid) then
                    table.insert(newCuboid[INDEX_ADJACENT], otherNewCuboid[INDEX_ID])
                    table.insert(otherNewCuboid[INDEX_ADJACENT], newCuboid[INDEX_ID])
                end
            end
        end
    end

    -- remove adjacent relations of parent cuboid and add new adjacent relations for child cuboids
    for _, adjacentCuboidId in pairs(cuboid[INDEX_ADJACENT]) do
        local adjacentCuboid = getCuboid(adjacentCuboidId)
        removeValue(adjacentCuboid[INDEX_ADJACENT], cuboid[INDEX_ID])
        for _, newCuboid in pairs(newCuboids) do
            if areCuboidsAdjacent(newCuboid, adjacentCuboid) then
                table.insert(newCuboid[INDEX_ADJACENT], adjacentCuboid[INDEX_ID])
                table.insert(adjacentCuboid[INDEX_ADJACENT], newCuboid[INDEX_ID])
            end
        end
    end
end

-- endregion

-- region A*

local function getPointDistance(p1, p2)
    return math.abs(p2[1] - p1[1]) + math.abs(p2[2] - p1[2]) + math.abs(p2[3] - p1[3])
end

local function getClosestPositionInCuboid(pos, cuboid)
    local dim = cuboid[INDEX_DIMENSIONS]
    return { putValueInRange(pos[1], dim[1], dim[4]), putValueInRange(pos[2], dim[2], dim[5]), putValueInRange(pos[3], dim[3], dim[6]) }
end

local function nodeToIdentifier(node)
    local pos = node[NODE_INDEX_POS]
    local cuboid = node[NODE_INDEX_CUBOID]
    return pos[1] .. "_" .. pos[2] .. "_" .. pos[3] .. "_" .. cuboid[INDEX_ID]
end

local function getSuccessorNodes(currentNode, destinationNode)
    local pos = currentNode[NODE_INDEX_POS]
    local cuboid = currentNode[NODE_INDEX_CUBOID]
    local successors = {}
    for _, adjacentCuboidId in ipairs(cuboid[INDEX_ADJACENT]) do
        local adjacentCuboid = getCuboid(adjacentCuboidId)
        if adjacentCuboid~=nil then
            local successor = {}
            successor[NODE_INDEX_POS] = getClosestPositionInCuboid(pos, adjacentCuboid)
            successor[NODE_INDEX_CUBOID] = adjacentCuboid

            table.insert(successors, successor)
            if adjacentCuboid[INDEX_ID] == destinationNode[NODE_INDEX_CUBOID][INDEX_ID] then
                table.insert(successors, destinationNode)
            end
        end
    end
    return successors
end

local function aStarExpandNode(currentEntry, openNodesQueue, openNodes, closedNodes, destinationNode)
    for _, successorNode in ipairs(getSuccessorNodes(currentEntry.node, destinationNode)) do
        local identifier = nodeToIdentifier(successorNode)
        if not closedNodes[identifier] then
            local g = currentEntry.g + getPointDistance(currentEntry.node[NODE_INDEX_POS], successorNode[NODE_INDEX_POS])
            if openNodes[identifier] and g > openNodes[identifier].g then

            else
                local f = g + getPointDistance(successorNode[NODE_INDEX_POS], destinationNode[NODE_INDEX_POS])
                if openNodes[identifier] then
                    local entry = openNodes[identifier]
                    entry.g = g
                    entry.f = f
                    entry.pre = currentEntry
                else
                    local entry = { node = successorNode, identifier = identifier, f = f, g = g, pre = currentEntry }
                    table.insert(openNodesQueue, entry)
                    openNodes[identifier] = entry
                end
            end
        end
    end
end

local function addIntermediatePathPoints(pos1, pos2, path)
    if arePointsInSurface(pos1, pos2) then
        local intermediatePos = { pos1[1], pos1[2], pos1[3] }
        for i1 = 1, 3 do
            if pos1[i1] ~= pos2[i1] then
                intermediatePos[i1] = pos2[i1]
                break
            end
        end
        table.insert(path, intermediatePos)
    elseif not arePointsInLine(pos1, pos2) then
        table.insert(path, { pos2[1], pos1[2], pos1[3] })
        table.insert(path, { pos2[1], pos2[2], pos1[3] })
    end
end

local function getPathFromNode(node)
    local nodes = {}
    while node do
        table.insert(nodes, 1, node)
        node = node.pre
    end
    local path = { nodes[1].node[NODE_INDEX_POS] }
    for i = 2, #nodes do
        local node1 = nodes[i - 1]
        local node2 = nodes[i]
        local pos1 = node1.node[NODE_INDEX_POS]
        local pos2 = node2.node[NODE_INDEX_POS]
        local cuboid1 = node1.node[NODE_INDEX_CUBOID]
        local cuboid2 = node2.node[NODE_INDEX_CUBOID]

        if arePointsInLine(pos1, pos2) then
            table.insert(path, pos2)
        elseif cuboid1 == cuboid2 then
            addIntermediatePathPoints(pos1, pos2, path)
            table.insert(path, pos2)
        else
            local closestInC1 = getClosestPositionInCuboid(pos2, cuboid1)
            if not posEquals(pos1, closestInC1) then
                addIntermediatePathPoints(pos1, closestInC1, path)
                table.insert(path, closestInC1)
            end
            local closestInC2 = getClosestPositionInCuboid(closestInC1, cuboid2)
            addIntermediatePathPoints(closestInC1, closestInC2, path)
            table.insert(path, closestInC2)
            if not posEquals(closestInC2, pos2) then
                addIntermediatePathPoints(closestInC2, pos2, path)
                table.insert(path, pos2)
            end
        end
    end
    return path
end

local function getPathFromPointsWithinCuboid(pos1, pos2)
    local path = { pos1 }
    addIntermediatePathPoints(pos1, pos2, path)
    table.insert(path, pos2)
    return path
end

local function findPath(start, destination)
    local startNode = { }
    startNode[NODE_INDEX_POS] = start
    startNode[NODE_INDEX_CUBOID] = findCuboidContainingPos(start)
    if startNode[NODE_INDEX_CUBOID] == nil then
        return nil
    end
    local destinationNode = { }
    destinationNode[NODE_INDEX_POS] = destination
    local destinationCuboid = findCuboidContainingPos(destination, true)
    if destinationCuboid == nil then
        -- destination obstructed
        return nil
    end
    destinationNode[NODE_INDEX_CUBOID] = destinationCuboid

    if startNode[NODE_INDEX_CUBOID][INDEX_ID] == destinationNode[NODE_INDEX_CUBOID][INDEX_ID] then
        return getPathFromPointsWithinCuboid(startNode[NODE_INDEX_POS], destinationNode[NODE_INDEX_POS])
    end

    local openNodesQueue = {}
    local openNodes = {}
    local closedNodes = {}
    table.insert(openNodesQueue, {
        node = startNode,
        identifier = nodeToIdentifier(startNode),
        f = 0, g = 0, pre = nil
    })
    openNodes[openNodesQueue[1].identifier] = openNodesQueue[1]

    while true do
        table.sort(openNodesQueue, function(a, b)
            return a.f > b.f
        end)
        if #openNodesQueue > 10000 then
            return nil
        end
        yield()
        local currentNode = table.remove(openNodesQueue)

        if not currentNode then
            return nil
        end
        if posEquals(currentNode.node[NODE_INDEX_POS], destination) then
            return getPathFromNode(currentNode)
        end

        closedNodes[currentNode.identifier] = true

        aStarExpandNode(currentNode, openNodesQueue, openNodes, closedNodes, destinationNode)
    end
end

local function findPathToAny(start, destinations)
    local startNode = {}
    startNode[NODE_INDEX_POS] = start
    startNode[NODE_INDEX_CUBOID] = findCuboidContainingPos(start)
    if startNode[NODE_INDEX_CUBOID] == nil then
        return nil
    end

    local destinationNodes = {}
    for i, destination in ipairs(destinations) do
        local destinationNode = {}
        destinationNode[NODE_INDEX_POS] = destination
        destinationNode[NODE_INDEX_CUBOID] = findCuboidContainingPos(destination, true)

        if destinationNode[NODE_INDEX_CUBOID] ~= nil then
            table.insert(destinationNodes, destinationNode)
            if startNode[NODE_INDEX_CUBOID][INDEX_ID] == destinationNode[NODE_INDEX_CUBOID][INDEX_ID] then
                return getPathFromPointsWithinCuboid(startNode[NODE_INDEX_POS], destinationNode[NODE_INDEX_POS]), i
            end
        end
    end
    if #destinationNodes == 0 then
        return nil
    end
    local allOpenNodesQueues = {}
    local allOpenNodes = {}
    local allClosedNodes = {}
    local activeRoutes = {}
    for _, _ in ipairs(destinationNodes) do
        local startEntry = {
            node = startNode,
            identifier = nodeToIdentifier(startNode),
            f = 0, g = 0, pre = nil
        }
        local openNodes = {}
        openNodes[startEntry.identifier] = startEntry
        table.insert(allOpenNodesQueues, { startEntry })
        table.insert(allOpenNodes, openNodes)
        table.insert(allClosedNodes, {})
        table.insert(activeRoutes, true)
    end

    while true do
        local anyActive = false
        for i = 1, #activeRoutes do
            if activeRoutes[i] then
                anyActive = true
                local openNodesQueue = allOpenNodesQueues[i]
                local openNodes = allOpenNodes[i]
                local closedNodes = allClosedNodes[i]
                table.sort(openNodesQueue, function(a, b)
                    return a.f > b.f
                end)
                yield()
                if #openNodesQueue > 10000 then
                    activeRoutes[i] = false
                else
                    local currentNode = table.remove(openNodesQueue)
                    if not currentNode then
                        activeRoutes[i] = false
                    else
                        if posEquals(currentNode.node[NODE_INDEX_POS], destinations[i]) then
                            return getPathFromNode(currentNode), i
                        end
                        closedNodes[currentNode.identifier] = true

                        aStarExpandNode(currentNode, openNodesQueue, openNodes, closedNodes, destinationNodes[i])
                    end
                end
            end
        end
        if not anyActive then
            return nil
        end
    end
end

-- endregion

local function getDirection()
    if currentDirectionPrivate then
        return currentDirectionPrivate
    end
    local directionShift = 0
    local currentPos = gpsAdapter.locate()
    local currentY = 0
    for dir = 1, 2 do
        local fUp = turtle.up
        local fDown = turtle.down
        if dir == 2 then
            fUp = turtle.down
            fDown = turtle.up
        end

        for _ = 1, 256 do
            for _ = 1, 4 do
                if turtle.forward() then
                    local newPos = gpsAdapter.locate()
                    currentDirectionPrivate = getDirectionFromPoints(
                            { currentPos[1], newPos[2], currentPos[3] }, newPos)
                    turtle.back()
                    break
                end
                turtle_unhooked_turnLeft()
                directionShift = directionShift + 1
            end
            if currentDirectionPrivate then
                break
            end
            if fUp() then
                currentY = currentY + 1
            else
                break
            end
        end
        while currentY > 0 do
            fDown()
            currentY = currentY - 1
        end
        if currentDirectionPrivate then
            for _ = 1, directionShift do
                turtle.turnRight()
            end
            return currentDirectionPrivate
        end
    end
    if not currentDirectionPrivate then
        error("turtle is trapped!")
    end
end

local function turnTowardDirection(direction)
    if direction == 4 or direction == 5 then
        return
    end
    direction = normalizeDirection(direction)
    local turnLeftDirection = normalizeDirection(getDirection() + 3)
    if turnLeftDirection == direction then
        turtle.turnLeft()
    end
    while getDirection() ~= direction do
        turtle.turnRight()
    end
end

local function moveInDirection(direction, count)
    if count == 0 then
        return 0, nil
    end
    if direction == 4 then
        for i = 1, count do
            if turtle.inspectUp() then
                return i - 1
            end
            if not turtle.up() then
                return i - 1
            end
        end
    elseif direction == 5 then
        for i = 1, count do
            if turtle.inspectDown() then
                return i - 1
            end
            if not turtle.down() then
                return i - 1
            end
        end
    else
        turnTowardDirection(direction)
        for i = 1, count do
            if turtle.inspect() then
                return i - 1
            end
            if not turtle.forward() then
                return i - 1
            end
        end
    end
    return count
end

local function getReverseDirection(direction)
    while direction < 6 do
        direction = direction + 6
    end
    direction = direction % 6
    if direction < 4 then
        return (direction + 2) % 4
    elseif direction == 4 then
        return 5
    else
        return 4
    end
end

local function receiveRednet(idFilter, msgFilter, protocolFilter, timeout)
    print("waiting on ", idFilter, msgFilter, protocolFilter, timeout)
    local start = os.clock()
    local stop = os.clock() + timeout
    while stop > start do
        local id, msg = rednet.receive(protocolFilter, stop - start)
        if id == idFilter and msg == msgFilter then
            print("received!")
            return id
        end
        start = os.clock()
    end
    print("not received")
    return nil
end

local function dodgeProtocol()
    local opponent = peripheral.wrap("front")
    if not opponent then
        return
    end
    local opponentId = opponent.getID()
    local ownId = os.getComputerID()
    if opponentId < ownId then
        local lastDirection = (getDirection() + 2) % 4
        for originalDirection = 0, 5 do
            local direction = originalDirection
            if originalDirection == lastDirection then
                direction = 5
            elseif originalDirection == 5 then
                direction = lastDirection
            end

            local moved = moveInDirection(direction, 1)
            if moved > 0 then
                sleep(2)
                local movedBack = moveInDirection(getReverseDirection(direction), 1)
                turnTowardDirection(lastDirection)
                if movedBack then
                    return true
                end
            end
        end
        return false
    else
        sleep(1)
        return true
    end
end

local function followPath(path, filterMethod)
    for i = 2, #path do
        local pStart = path[i - 1]
        local pEnd = path[i]
        local steps = math.max(math.abs(pEnd[1] - pStart[1]), math.abs(pEnd[2] - pStart[2]), math.abs(pEnd[3] - pStart[3]))
        if steps ~= 0 then
            local direction = getDirectionFromPoints(pStart, pEnd)
            local toMove = steps
            local try = 0
            while toMove > 0 do
                local moved = moveInDirection(direction, toMove)
                if moved > 0 then
                    try = 0
                end
                if moved ~= toMove then
                    local isBlock = false
                    local inspectResult
                    if direction == 4 then
                        isBlock, inspectResult = turtle.inspectUp()
                    elseif direction == 5 then
                        isBlock, inspectResult = turtle.inspectDown()
                    else
                        isBlock, inspectResult = turtle.inspect()
                    end
                    if inspectResult then
                        if inspectResult.name and inspectResult.name:sub(1, 21) == "computercraft:turtle_" and try < 10 then
                            print("dodging (attempt "..tostring(try+1).."/10)")
                            if not dodgeProtocol() then
                                return false, true
                            end
                        elseif filterMethod then
                            local filterResult = invokeHook(filterMethod.name, inspectResult.name, direction)
                            if filterResult then
                                if direction == 4 then
                                    turtle.digUp()
                                elseif direction == 5 then
                                    turtle.digDown()
                                else
                                    turtle.dig()
                                end
                            else
                                markAsObstructed(getPosInDirection(gpsAdapter.locate(), direction))
                                return false, isBlock
                            end
                        else
                            markAsObstructed(getPosInDirection(gpsAdapter.locate(), direction))
                            return false, isBlock
                        end
                    end
                end
                try = try + 1
                toMove = toMove - moved
            end
        end
    end
    return true
end

local function moveFwd(steps)
    for _ = 1, steps do
        while not turtle.forward() do
            if not turtle.dig() then
                turtle.attack()
            end
        end
    end
end

local function moveUp(steps)
    for _ = 1, steps do
        while not turtle.up() do
            if not turtle.digUp() then
                turtle.attackUp()
            end
        end
    end
end

local function moveDown(steps)
    for _ = 1, steps do
        while not turtle.down() do
            if not turtle.digDown() then
                turtle.attackDown()
            end
        end
    end
end

api.moveToRuthless = {
    path = { 1, 2 },
    fct = function(destination, direction)
        local start = gpsAdapter.locate()

        local diffX = destination[1] - start[1]
        local diffY = destination[2] - start[2]
        local diffZ = destination[3] - start[3]

        if diffX < 0 then
            turnTowardDirection(2)
            moveFwd(-diffX)
        elseif diffX > 0 then
            turnTowardDirection(0)
            moveFwd(diffX)
        end

        if diffY > 0 then
            moveUp(diffY)
        elseif diffY < 0 then
            moveDown(-diffY)
        end

        if diffZ < 0 then
            turnTowardDirection(3)
            moveFwd(-diffZ)
        elseif diffZ > 0 then
            turnTowardDirection(1)
            moveFwd(diffZ)
        end

        if direction ~= nil then
            turnTowardDirection(direction)
        end
    end
}

api.moveTo = {
    pars = { 1, 3 },
    fct = function(pos, direction, filterMethod)
        if filterMethod then
            resetCuboids()
        end
        local path

        local cleared = false
        local savedLast = os.clock()
        while true do
            local success = false
            if path then
                success = followPath(path, filterMethod)
            end
            if success then
                break
            end
            path = findPath(gpsAdapter.locate(), pos)
            while not path do
                if not cleared then
                    resetCuboids()
                    path = findPath(gpsAdapter.locate(), pos)
                    cleared = true
                else
                    print("no path found, retrying in 5s")
                    sleep(5)
                    cleared = false
                end
            end
            if os.clock() - savedLast > 10 then
                pSave()
                savedLast = os.clock()
            end
        end
        if direction ~= nil then
            turnTowardDirection(direction)
        end

        pSave()
    end
}

function api.locate()
    return toTurtiArray(gpsAdapter.locate())
end

function api.getDirection()
    return getDirection()
end

function api.setDirection(direction)
    turnTowardDirection(direction)
end

api.faceToward = {
    pars = { 1, 2 },
    fct = function(pos, filterMethod)
        if filterMethod then
            resetCuboids()
        end
        local possiblePositions = {
            { pos[1] - 1, pos[2], pos[3] },
            { pos[1], pos[2], pos[3] - 1 },
            { pos[1] + 1, pos[2], pos[3] },
            { pos[1], pos[2], pos[3] + 1 }
        }

        local path
        local index
        local savedLast = os.clock()
        while true do
            local success = false
            if path then
                success = followPath(path, filterMethod)
            end
            if success then
                turnTowardDirection(index - 1)
                break
            end
            path, index = findPathToAny(gpsAdapter.locate(), possiblePositions)
            local tries = 0
            while not path do
                if tries < 5 then
                    if tries == 3 then
                        resetCuboids()
                    end
                    path, index = findPathToAny(gpsAdapter.locate(), possiblePositions)
                else
                    print("no path found, retrying in 5s")
                    sleep(5)
                end
                tries = tries + 1
            end
            if os.clock() - savedLast > 10 then
                pSave()
                savedLast = os.clock()
            end
        end
    end
}

api.getPosInDirection = {
    pars = { 2, 3 },
    fct = function(pos, direction, count)
        return toTurtiArray(getPosInDirection(pos, direction, count))
    end
}

turtle_unhooked_turnLeft = installPreExecutionHook(turtle, "turnLeft", "smartGPS", function()
    if currentDirectionPrivate then
        currentDirectionPrivate = normalizeDirection(getDirection() - 1)
    end
end)

turtle_unhooked_turnRight = installPreExecutionHook(turtle, "turnRight", "smartGPS", function()
    if currentDirectionPrivate then
        currentDirectionPrivate = normalizeDirection(getDirection() + 1)
    end
end)

return {
    name = "lightGPS",
    api = api,
    onInitPersistentStorage = function(_pStorage, _pSave)
        pStorage = _pStorage
        if not pStorage.cuboids then
            resetCuboids()
        end
        pSave = _pSave
    end
}