CC: P++ GPPS Global Player Position System

os.loadAPI("rnc")

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

    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.nDistance
    local r2 = B.nDistance
    local r3 = C.nDistance

    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.vPosition + (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( 0.01 ), result2:round( 0.01 )
        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( 0.01 )

end

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

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

function locate(_sName, _nTimeout, _bDebug )
    if _nTimeout ~= nil and type( _nTimeout ) ~= "number" then
        error( "bad argument #1 (expected number, got " .. type( _nTimeout ) .. ")", 2 )
    end
    if _bDebug ~= nil and type( _bDebug ) ~= "boolean" then
        error( "bad argument #2 (expected boolean, got " .. type( _bDebug) .. ")", 2 )
    end
    -- Let command computers use their magic fourth-wall-breaking special abilities
    if commands then
        return commands.getBlockPosition()
    end

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

    if sModemSide == nil then
        if _bDebug then
            print( "No wireless modem attached" )
        end
        return nil
    end

    if _bDebug then
        print( "Finding position..." )
    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

    -- Send a ping to listening GPS hosts
    local msg = {}
    msg.type = "PING"
    msg.name = _sName

    rnc.setPort(65500)
    rnc.setIdenty(45)
    rnc.send(msg)

    -- Wait for the responses
    local tFixes = {}
    local pos1, pos2 = nil, nil
    while true do
        rnc.setPort(65500)
        rnc.setIdenty(46)
        rnc.setTimeOut(_nTimeout or 2)
        tMessage = rnc.resive()
        if tMessage ~= nil then
            local tFix = { vPosition = vector.new( tMessage[1], tMessage[2], tMessage[3] ), nDistance = tMessage[4] }
            if tFix.nDistance == nil then
                print( "No player founded" )
                return
            end
            if _bDebug then
                print( tFix.nDistance.." metres from "..tostring( tFix.vPosition ) )
            end
            if tFix.nDistance == 0 then
                pos1, pos2 = tFix.vPosition, nil
            else
                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
            end
            if pos1 and not pos2 then
                break
            end
        else
            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

local function printUsage()
    print( "Usages:" )
    print( "gpps host" )
    print( "gpps host <x> <y> <z> <player name>" )
    print( "gpps locate" )
    print( "gpps.locate(<player name>)" )
end

local tArgs = { ... }
if #tArgs < 1 then
    return
end

playerSensor = peripheral.find("playerSensor")

local function GetDistanceByName(name)
    p = playerSensor.getNearbyPlayers(2147483647)

    length = table.getn(p)
    for i=1, length do
        if p[i].player == name then
            return p[i].distance
        end
    end
    return nil
end

local function readNumber()
    local num = nil
    while num == nil do
        num = tonumber(read())
        if not num then
            write( "Not a number. Try again: " )
        end
    end
    return math.floor( num + 0.5 )
end

local function open()
    local bOpen, sFreeSide = false, nil
    for n,sSide in pairs(rs.getSides()) do
        if peripheral.getType( sSide ) == "modem" then
            sFreeSide = sSide
            if rednet.isOpen( sSide ) then
                bOpen = true
                break
            end
        end
    end

    if not bOpen then
        if sFreeSide then
            print( "No modem active. Opening "..sFreeSide.." modem" )
            rednet.open( sFreeSide )
            return true
        else
            print( "No modem attached" )
            return false
        end
    end
    return true
end

local sCommand = tArgs[1]
if sCommand == "locate" then
    if open() then
        gps.locate( 2, true, tArgs[5] )
    end

elseif sCommand == "host" then
    if turtle then
        print( "Turtles cannot act as GPS hosts." )
        return
    end

    if open() then
        local x,y,z
        if #tArgs >= 4 then
            x = tonumber(tArgs[2])
            y = tonumber(tArgs[3])
            z = tonumber(tArgs[4])
            if x == nil or y == nil or z == nil then
                printUsage()
                return
            end
            print( "Position is "..x..","..y..","..z )
        else
            x,y,z = gps.locate( 2, true )
            if x == nil then
                print( "Run \"gps host <x> <y> <z>\" to set position manually" )
                return
            end
        end

        print( "Serving GPPS requests" )

        local nServed = 0
        while true do
            rnc.setPort(65500)
            rnc.setIdenty(45)
            msg = rnc.resive()
            if msg ~= nil then
                if msg.type == "PING" then
                    rnc.setPort(65500)
                    rnc.setIdenty(46)
                    rnc.send( {x,y,z,GetDistanceByName(msg.name)} )

                    nServed = nServed + 1
                    if nServed > 1 then
                        local x,y = term.getCursorPos()
                        term.setCursorPos(1,y-1)
                    end
                    print( nServed.." GPPS Requests served" )
                end
            end
        end
    end

else
    printUsage()
    return
end