TurtiLib smartGps

local pStorage
local pSave
local currentDirectionPrivate

local api = {}

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

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

-- based on https://de.wikipedia.org/wiki/A*-Algorithmus
function api.moveTo(pos, direction)
    local path = findPath(gpsAdapter.locate(), pos)
    if not path then
        error("no path found")
    end
    local cleared = false
    while not followPath(path) do
        path = findPath(gpsAdapter.locate(), pos)
        while not path do
            if not cleared then
                clearObstructedData()
                path = findPath(gpsAdapter.locate(), pos)
                cleared = true
            else
                error("no path found")
            end
        end
    end
    turnTowardsDirection(direction)
    pSave()
end

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

function api.getDirection()
    return getCurrentDirection()
end

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

function faceTowardAny(positions)
    local possiblePositions = {}
    for i, pos in ipairs(positions) do
        table.insert(possiblePositions, { pos[1] - 1, pos[2], pos[3] }) -- direction 0
        table.insert(possiblePositions, { pos[1], pos[2], pos[3] - 1 }) -- direction 1
        table.insert(possiblePositions, { pos[1] + 1, pos[2], pos[3] }) -- direction 2
        table.insert(possiblePositions, { pos[1], pos[2], pos[3] + 1 }) -- direction 3
    end

    local cleared = false

    while true do
        local path, index = findPathToAny(gpsAdapter.locate(), possiblePositions)

        if path then
            if followPath(path) then
                turnTowardsDirection((index - 1) % 4)
                return math.floor((index - 1) / 4)
            end
        else
            if not cleared then
                clearObstructedData()
                cleared = true
            else
                error("no path found")
            end
        end
    end
end

function api.faceToward(pos)
    local possiblePositions = {
        { pos[1] - 1, pos[2], pos[3] }, -- direction 0
        { pos[1], pos[2], pos[3] - 1 }, -- direction 1
        { pos[1] + 1, pos[2], pos[3] }, -- direction 2
        { pos[1], pos[2], pos[3] + 1 }  -- direction 3
    }

    while true do
        local path, index = findPathToAny(gpsAdapter.locate(), possiblePositions)

        if path then
            if followPath(path) then
                turnTowardsDirection(index - 1)
                return
            end
        else
            if not cleared then
                clearObstructedData()
                cleared = true
            else
                error("no path found")
            end
        end
    end
end

function api.faceTowardAny(...)
    faceTowardAny({ ... })
end

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

function followPath(path)
    for i = 2, #path do
        local direction = getDirectionFromPoints(path[i - 1], path[i])
        if not moveInDirection(direction) then
            markAsObstructed(path[i])
            return false
        end
        if isObstructed(getPosInDirection(path[i], direction)) and not turtle.detect() then
            print("mark as not obstructed!")
            markAsNotObstructed(getPosInDirection(path[i], direction))
        end
        if isObstructed(getPosInDirection(path[i], 4)) and not turtle.detectUp() then
            print("mark as not obstructed!")
            markAsNotObstructed(getPosInDirection(path[i], 4))
        end
        if isObstructed(getPosInDirection(path[i], 5)) and not turtle.detectDown() then
            print("mark as not obstructed!")
            markAsNotObstructed(getPosInDirection(path[i], 5))
        end
    end

    return true
end

function getPosInDirection(pos, direction, count)
    if count==nil 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

function moveInDirection(direction)
    if direction == 4 then
        if turtle.up() then
            return true
        end
    elseif direction == 5 then
        if turtle.down() then
            return true
        end
    else
        turnTowardsDirection(direction)
        if turtle.forward() then
            return true
        end
    end

    return false
end

function getCurrentDirection()
    if currentDirectionPrivate then
        return currentDirectionPrivate
    end
    local directionShift = 0
    local currentPos = gpsAdapter.locate()
    local testY = 3
    local currentY = 0
    for y = 1, testY do
        for i = 0, 5 do
            if turtle.forward() then
                currentDirectionPrivate = getDirectionFromPoints(currentPos, gpsAdapter.locate())
                turtle.back()
                break
            end
            turtle_unhooked_turnLeft()
            directionShift = directionShift + 1
        end
        if currentDirectionPrivate then
            break
        end
        if turtle.up() then
            currentY = currentY + 1
        else
            break
        end
    end
    while currentY > 0 do
        turtle.down()
        currentY = currentY - 1
    end
    if currentDirectionPrivate then
        for _ = 1, directionShift do
            turtle.turnRight()
        end
        return currentDirectionPrivate
    end
    for y = 1, testY do
        for i = 0, 5 do
            if turtle.forward() then
                currentDirectionPrivate = getDirectionFromPoints(currentPos, gpsAdapter.locate())
                turtle.back()
                break
            end
            turtle_unhooked_turnLeft()
            directionShift = directionShift + 1
        end
        if currentDirectionPrivate then
            break
        end
        if turtle.down() then
            currentY = currentY - 1
        else
            break
        end
    end
    while currentY < 0 do
        turtle.up()
        currentY = currentY + 1
    end

    if not currentDirectionPrivate then
        error("turtle is trapped!")
    end
    for _ = 1, directionShift do
        turtle.turnRight()
    end
    return currentDirectionPrivate
end

function setCurrentDirection(cD)
    while cD > 3 do
        cD = cD - 4
    end
    while cD < 0 do
        cD = cD + 4
    end
    currentDirectionPrivate = cD
end

function turnTowardsDirection(direction)
    while direction > 3 do
        direction = direction - 4
    end
    while direction < 0 do
        direction = direction + 4
    end
    if direction == 4 or direction == 5 then
        return
    end
    local turnLeftDirection = getCurrentDirection() + 3
    if turnLeftDirection > 3 then
        turnLeftDirection = turnLeftDirection - 4
    end
    if turnLeftDirection == direction then
        turtle.turnLeft()
    end

    while getCurrentDirection() ~= direction do
        turtle.turnRight()
    end
end

function getDirectionFromPoints(pos1, pos2)
    if pos2[1] == pos1[1] + 1 then
        return 0
    elseif pos2[3] == pos1[3] + 1 then
        return 1
    elseif pos2[1] == pos1[1] - 1 then
        return 2
    elseif pos2[3] == pos1[3] - 1 then
        return 3
    elseif pos2[2] == pos1[2] + 1 then
        return 4
    elseif pos2[2] == pos1[2] - 1 then
        return 5
    else
        printTable(pos1)
        printTable(pos2)
        error("cannot determine direction from points")
    end
end

function yield()
    os.queueEvent("randomEvent")
    os.pullEvent()
end

function findPathToAny(start, destinations)
    local allOpenNodesQueues = {}
    local allOpenNodes = {}
    local allClosedNodes = {}
    local activeRoutes = {}

    for i, destination in pairs(destinations) do
        local openNodesQueue = {}
        local openNodes = {}
        table.insert(openNodesQueue, { pos = start, identifier = posToIdentifier(start), f = 0, g = 0, pre = nil })
        openNodes[openNodesQueue[1].identifier] = openNodesQueue[1]

        table.insert(allOpenNodesQueues, openNodesQueue)
        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 > 1000 then
                    activeRoutes[i] = false
                else
                    local currentNode = table.remove(openNodesQueue, 1)

                    if not currentNode then
                        activeRoutes[i] = false
                    else
                        if posEquals(currentNode.pos, destinations[i]) then
                            return getPathFromNode(currentNode), i
                        end

                        closedNodes[currentNode.identifier] = true

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

end

function findPath(start, destination)
    local openNodesQueue = {}
    local openNodes = {}
    local closedNodes = {}
    table.insert(openNodesQueue, { pos = start, identifier = posToIdentifier(start), 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, 1)

        if not currentNode then
            return nil
        end

        if posEquals(currentNode.pos, destination) then
            return getPathFromNode(currentNode)
        end

        closedNodes[currentNode.identifier] = true

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

function getPathFromNode(endNode)
    local path = {}
    local currentNode = endNode
    while currentNode do
        table.insert(path, 1, currentNode.pos)
        currentNode = currentNode.pre
    end
    return path
end

function aStarExpandNode(currentNode, openNodesQueue, openNodes, closedNodes, destination)
    for i, successor in ipairs(getSuccessors(currentNode)) do
        local identifier = posToIdentifier(successor)
        if not closedNodes[identifier] then
            local g = currentNode.g + 1

            if openNodes[identifier] and g > openNodes[identifier].g then

            else
                local f = g + aStarH(successor, destination)
                if openNodes[identifier] then
                    local entry = openNodes[identifier]
                    entry.g = g
                    entry.f = f
                    entry.pre = currentNode
                else
                    local entry = { pos = successor, identifier = identifier, f = f, g = g, pre = currentNode }
                    table.insert(openNodesQueue, entry)
                    openNodes[identifier] = entry
                end
            end
        end
    end
end

function aStarH(start, destination)
    return math.abs(destination[1] - start[1]) + math.abs(destination[2] - start[2]) + math.abs(destination[3] - start[3])
end

function getSuccessors(node)
    local pos = node.pos
    local x = pos[1]
    local y = pos[2]
    local z = pos[3]
    local potentialSuccessors = {
        { x + 1, y, z },
        { x - 1, y, z },
        { x, y + 1, z },
        { x, y - 1, z },
        { x, y, z + 1 },
        { x, y, z - 1 }
    }
    local successors = {}
    for i, v in ipairs(potentialSuccessors) do
        if not isObstructed(v) then
            table.insert(successors, v)
        end
    end
    return successors
end

function posToIdentifier(pos)
    return pos[1] .. ":" .. pos[2] .. ":" .. pos[3]
end

function posEquals(pos1, pos2)
    return pos1[1] == pos2[1] and pos1[2] == pos2[2] and pos1[3] == pos2[3]
end

function isObstructed(pos)
    local blocks = pStorage.blocks
    if not blocks then
        return false
    end
    blocks = blocks[pos[1]]
    if not blocks then
        return false
    end
    blocks = blocks[pos[2]]
    if not blocks then
        return false
    end
    blocks = blocks[pos[3]]
    if not blocks then
        return false
    end
    return true
end

function clearObstructedData()
    print("reset environment knowledge")
    pStorage.blocks = {}
end

function markAsObstructed(pos)
    local x = pos[1]
    local y = pos[2]
    local z = pos[3]
    if not pStorage.blocks then
        pStorage.blocks = {}
    end
    local blocksX = getAtKeyOrCreateTable(pStorage.blocks, x)
    local blocksY = getAtKeyOrCreateTable(blocksX, y)
    blocksY[z] = true
end

function markAsNotObstructed(pos)
    local blocks = pStorage.blocks
    if not blocks then
        return
    end
    blocks = blocks[pos[1]]
    if not blocks then
        return
    end
    blocks = blocks[pos[2]]
    if not blocks then
        return
    end
    table.remove(blocks, pos[3])
end

function getAtKeyOrCreateTable(tbl, key)
    local v = tbl[key]
    if not v then
        v = {}
        tbl[key] = v
    end
    return v
end

return {
    name = 'smartGPS',
    api = api,
    onInitPersistentStorage = function(_pStorage, _pSave)
        pStorage = _pStorage
        pSave = _pSave
    end
}