Reverse GPS

local ids = {[922]=true, [923]=true, [924]=true, [925]=true}

function lookup(id, time)
    for i,v in pairs(ids) do
        rednet.send(i, "RGPS LOOKUP "..id)
    end
    local timeout = os.startTimer(time or 0.1)
    local dists = {}
    while true do
        local evt, id, msg = os.pullEvent()
        if evt == "rednet_message" and ids[id] then
            table.insert(dists, textutils.unserialize(msg))
        elseif evt == "timer" and id == timeout then
            break
        end
    end
    return dists
end

local function trilaterate( A, B, C )
    local a2b = B.position - A.position
    local a2c = C.position - A.position

    if math.abs( a2b:normalize():dot( a2c:normalize() ) ) > 0.999 then
        return nil
    end

    local d = a2b:length()
    local ex = a2b:normalize( )
    local i = ex:dot( a2c )
    local ey = (a2c - (ex * i)):normalize()
    local j = ey:dot( a2c )
    local ez = ex:cross( ey )

    local r1 = A.distance
    local r2 = B.distance
    local r3 = C.distance

    local x = (r1*r1 - r2*r2 + d*d) / (2*d)
    local y = (r1*r1 - r3*r3 - x*x + (x-i)*(x-i) + j*j) / (2*j)

    local result = A.position + (ex * x) + (ey * y)

    local zSquared = r1*r1 - x*x - y*y
    if zSquared > 0 then
        local z = math.sqrt( zSquared )
        local result1 = result + (ez * z)
        local result2 = result - (ez * z)

        local rounded1, rounded2 = result1:round(), result2:round()
        if rounded1.x ~= rounded2.x or rounded1.y ~= rounded2.y or rounded1.z ~= rounded2.z then
            return rounded1, rounded2
        else
            return rounded1
        end
    end
    return result:round()

end

local function narrow( p1, p2, fix )
    local dist1 = math.abs( (p1 - fix.position):length() - fix.distance )
    local dist2 = math.abs( (p2 - fix.position):length() - fix.distance )

    if math.abs(dist1 - dist2) < 0.05 then
        return p1, p2
    elseif dist1 < dist2 then
        return p1:round()
    else
        return p2:round()
    end
end

function locate( id, timeout, _bDebug )
    if _bDebug then
        print( "Finding position..." )
    end

    -- Find a modem
    local sModemSide = nil
    for n,sSide in ipairs( rs.getSides() ) do
        if peripheral.getType( sSide ) == "modem" then
            sModemSide = sSide
            break
        end
    end

    if sModemSide == nil then
        return nil
    end

    -- Open a channel
    local modem = peripheral.wrap( sModemSide )
    local bCloseChannel = false
    if not modem.isOpen( os.getComputerID() ) then
        modem.open( os.getComputerID() )
        bCloseChannel = true
    end

    -- Wait for the responses
    local tFixes = {}
    local pos1, pos2 = nil

    local dists = lookup(id, timeout)
    for i, v in pairs(dists) do
        local tFix = { position = vector.new( v[1], v[2], v[3] ), distance = v[4] }
        if _bDebug then
            print( tFix.distance.." metres from "..tostring( tFix.position ) )
        end
        table.insert( tFixes, tFix )

        if #tFixes >= 3 then
            if not pos1 then
                pos1, pos2 = trilaterate( tFixes[1], tFixes[2], tFixes[#tFixes] )
            else
                pos1, pos2 = narrow( pos1, pos2, tFixes[#tFixes] )
            end
        end

        if pos1 and not pos2 then
            break
        end
    end

    -- Close the channel, if we opened one
    if bCloseChannel then
        modem.close( os.getComputerID() )
    end

    -- Return the response
    if pos1 and pos2 then
        if _bDebug then
            print( "Ambiguous position" )
            print( "Could be "..pos1.x..","..pos1.y..","..pos1.z.." or "..pos2.x..","..pos2.y..","..pos2.z )
        end
        return nil
    elseif pos1 then
        if _bDebug then
            print( "Position is "..pos1.x..","..pos1.y..","..pos1.z )
        end
        return pos1.x, pos1.y, pos1.z
    else
        if _bDebug then
            print( "Could not determine position" )
        end
        return nil
    end
end