starNav

if not tinyMap then
    if not os.loadAPI("tinyMap") then
        error("could not load tinyMap API")
    end
end
if not aStar then
    if not os.loadAPI("aStar") then
        error("could not load aStar API")
    end
end
if not location then
    if not os.loadAPI("location") then
        error("could not load location API")
    end
end

local position

local SESSION_MAX_DISTANCE_DEFAULT = 32

local SESSION_COORD_CLEAR = 1
local SESSION_COORD_BLOCKED = 2
local SESSION_COORD_DISTANCE = math.huge
local MAIN_COORD_CLEAR = nil
local MAIN_COORD_BLOCKED = 1
local MAIN_COORD_DISTANCE = 1024

local sessionMidPoint
local sessionMaxDistance
local sessionMap
local mainMap

local function sup_norm(a, b)
    return math.max(math.abs(a.x - b.x), math.abs(a.y - b.y), math.abs(a.z - b.z))
end

local function distanceFunc(a, b)
    local sessionMapA, sessionMapB = sessionMap:get(a), sessionMap:get(b)
    if sup_norm(a, sessionMidPoint) > sessionMaxDistance then
        return SESSION_COORD_DISTANCE -- first coord is outside the search region
    elseif sup_norm(b, sessionMidPoint) > sessionMaxDistance then
        return SESSION_COORD_DISTANCE -- second coord is outside the search region
    elseif sessionMapA == SESSION_COORD_BLOCKED or sessionMapB == SESSION_COORD_BLOCKED then
        return SESSION_COORD_DISTANCE -- one of the coords has been found to be blocked this during this session
    elseif sessionMapA == SESSION_COORD_CLEAR and sessionMapA == SESSION_COORD_CLEAR then
        return aStar.distance(a, b) -- both coords has been found to be clear this during this session
    elseif mainMap:get(a) or mainMap:get(b) then
        return MAIN_COORD_DISTANCE
    end
    return aStar.distance(a, b) -- we are assuming both coords are clear
end

local directions = {
    [vector.new(0, 0, 1)] = 0,
    [vector.new(-1, 0, 0)] = 1,
    [vector.new(0, 0, -1)] = 2,
    [vector.new(1, 0, 0)] = 3,
    [vector.new(0, 1, 0)] = 4,
    [vector.new(0, -1, 0)] = 5,
}

local function deltaToDirection(delta)
    for vec, dir in pairs(directions) do
        if aStar.vectorEquals(delta, vec) then
            return dir
        end
    end
end

local function tryMove()
    for i = 1, 4 do
        if turtle.forward() then
            return true
        end
        turtle.turnRight()
    end
    return false
end

local function findPosition()
    local move = turtle.up
    while not tryMove() do
        if not move() then
            if move == turtle.up then
                move = turtle.down
                move()
            else
                error("trapped in a ridiculous place")
            end
        end
    end

    local p1 = {gps.locate()}
    if #p1 == 3 then
        p1 = vector.new(unpack(p1))
    else
        error("no gps signal - phase 1")
    end

    if not turtle.back() then
        error("couldn't move to determine direction")
    end

    local p2 = {gps.locate()}
    if #p2 == 3 then
        p2 = vector.new(unpack(p2))
    else
        error("no gps signal - phase 2")
    end

    local direction = deltaToDirection(p1 - p2)
    if direction and direction < 4 then
        return location.new(p2.x, p2.y, p2.z, direction)
    else
        return false
    end
end

local function detect(currPos, adjPos)
    local direction = deltaToDirection(adjPos - currPos)
    if direction then
        position:setHeading(direction)
        if direction == 4 then
            return turtle.detectUp()
        elseif direction == 5 then
            return turtle.detectDown()
        else
            return turtle.detect()
        end
    end
    return false
end

local function inspect(currPos, adjPos)
    local direction = deltaToDirection(adjPos - currPos)
    if direction then
        position:setHeading(direction)
        if direction == 4 then
            return turtle.inspectUp()
        elseif direction == 5 then
            return turtle.inspectDown()
        else
            return turtle.inspect()
        end
    end
    return false
end

local function updateCoord(coord, isBlocked)
    if isBlocked then
        sessionMap:set(coord, SESSION_COORD_BLOCKED)
        mainMap:set(coord, MAIN_COORD_BLOCKED)
    else
        sessionMap:set(coord, SESSION_COORD_CLEAR)
        mainMap:set(coord, MAIN_COORD_CLEAR)
    end
end

local function detectAll(currPos)
    for _, pos in ipairs(aStar.adjacent(currPos)) do -- better order of checking directions
        updateCoord(pos, detect(currPos, pos))
    end
end

local function findSensor()
    for _, side in ipairs(peripheral.getNames()) do
        if peripheral.getType(side) == "turtlesensorenvironment" then
            return side
        end
    end
    return false
end

local function scan(currPos)
    local sensorSide = findSensor()
    if sensorSide then
        local rawBlockInfo = peripheral.call(sensorSide, "sonicScan")
        local sortedBlockInfo = aStar.newMap()
        for _, blockInfo in ipairs(rawBlockInfo) do
            sortedBlockInfo:set(currPos + vector.new(blockInfo.x, blockInfo.y, blockInfo.z), blockInfo)
        end
        local toCheckQueue = {}
        for _, pos in ipairs(aStar.adjacent(currPos)) do
            if sortedBlockInfo:get(pos) then
                table.insert(toCheckQueue, pos)
            end
        end
        while toCheckQueue[1] do
            local pos = table.remove(toCheckQueue, 1)
            local blockInfo = sortedBlockInfo:get(pos)
            if blockInfo.type == "AIR" then
                for _, pos2 in ipairs(aStar.adjacent(pos)) do
                    local blockInfo2 = sortedBlockInfo:get(pos2)
                    if blockInfo2 and not blockInfo2.checked then
                        table.insert(toCheckQueue, pos2)
                    end
                end
                updateCoord(pos, false)
            else
                updateCoord(pos, true)
            end
            blockInfo.checked = true
        end
    else
        detectAll(currPos)
    end
    mainMap:save()
end

local function move(currPos, adjPos)
    local direction = deltaToDirection(adjPos - currPos)
    if direction then
        position:setHeading(direction)
        if direction == 4 then
            return position:up()
        elseif direction == 5 then
            return position:down()
        else
            return position:forward()
        end
    end
    return false
end

function goto(x, y, z, maxDistance)
    if not mainMap then
        error("mainMap has not been specified")
    end
    if turtle.getFuelLevel() == 0 then
        return false, "ran out of fuel"
    end
    if not position then
        position = findPosition()
        if not position then
            return false, "couldn't determine location"
        end
    end

    local goal = vector.new(tonumber(x), tonumber(y), tonumber(z))

    sessionMap = aStar.newMap()
    sessionMidPoint = vector.new(math.floor((goal.x + position.x)/2), math.floor((goal.y + position.y)/2), math.floor((goal.z + position.z)/2))
    sessionMaxDistance = (type(maxDistance) == "number" and maxDistance) or math.max(2*sup_norm(sessionMidPoint, goal), SESSION_MAX_DISTANCE_DEFAULT)

    local path = aStar.compute(distanceFunc, position, goal)
    if not path then
        return false, "no known path to goal"
    end
    while not aStar.vectorEquals(position, goal) do
        local movePos = table.remove(path)
        while not move(position, movePos) do
            local blockPresent, blockData = inspect(position, movePos)
            local recalculate, isTurtle = false, false
            if blockPresent and (blockData.name == "ComputerCraft:CC-TurtleAdvanced" or blockData.name == "ComputerCraft:CC-Turtle") then -- there is a turtle in the way
                sleep(math.random(0, 3))
                local blockPresent2, blockData2 = inspect(position, movePos)
                if blockPresent2 and (blockData2.name == "ComputerCraft:CC-TurtleAdvanced" or blockData2.name == "ComputerCraft:CC-Turtle") then -- the turtle is still there
                    recalculate, isTurtle = true, true
                end
            elseif blockPresent then
                recalculate = true
            elseif turtle.getFuelLevel() == 0 then
                return false, "ran out of fuel"
            else
                sleep(1)
            end
            if recalculate then
                scan(position)
                if sessionMap:get(goal) == SESSION_COORD_BLOCKED then return false, "goal is blocked" end
                path = aStar.compute(distanceFunc, position, goal)
                if not path then
                    return false, "no known path to goal"
                end
                if isTurtle then
                    sessionMap:set(movePos, nil)
                end
                movePos = table.remove(path)
            end
        end
        if mainMap:get(movePos) then
            mainMap:set(movePos, MAIN_COORD_CLEAR)
        end
    end
    return true
end

function setMap(mapName)
    if type(mapName) ~= "string" then
        error("mapName must be string")
    end
    mainMap = tinyMap.new(mapName)
end

function getMap()
    return mainMap
end

function getPosition()
    if position then
        return position:value()
    end
end