geoid

rednet.open("top")
clientIDs={}
clientIDs[13]=true
clientIDs[14]=true
clientIDs[15]=true

local locFixes = {}
local locs = {}
local myLoc = {296,66,733}

function getLoc(id)
    local loc = locs[id]
    if loc == nil then return nil end
    return {locType=loc[1],x=loc[2].x,y=loc[2].y,z=loc[2].z}
end

function saveLocs()
    local file = fs.open("/locs","w")
    file.write(textutils.serialize(locs))
    file.close()
end

function loadLocs()
    if not fs.exists("/locs") then return end

    local file = fs.open("/locs","r")
    local s = file.readAll()
    locs = textutils.unserialize(s)
    file.close()
end

loadLocs()

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 result1, result2
        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 processID(tID)
    print("PROC"..tID)
  local p1,p2 = trilaterate(locFixes[tID][1],locFixes[tID][2],locFixes[tID][3])
  print("TRILD")
  if p2 then
    prinT("NAR")
    p1,p2 = narrow( p1,p2,locFixes[tID][4] )
  end
  print("OD")
  if p1 then
    if p2 then
      if locs[tID][1] ~= "exact" then
        locs[tID] = {"ambigous",p1,p2}
        print("Ambi: " .. p1.x .. "/".. p1.y .. "/"..p1.z)
        saveLocs()
      end
    else
      locs[tID] = {"exact",p1}
      print("Exacted: " .. p1.x .. "/".. p1.y .. "/"..p1.z)
      saveLocs()
    end
  end
  return p1,p2
end

function insertFix(tID,dist,hostLoc)
    print("INSFIX"..tID.."/"..hostLoc.x)
  if locFixes[tID] then
    table.insert(locFixes[tID],{sender=tID,position=hostLoc,distance=dist})
    if #locFixes[tID] >= 4 then
      processID(tID)
      locFixes[tID] = nil
    end
  else
    locFixes[tID] = {{sender=tID,position=hostLoc,distance=dist}}
  end
end
function detect()
while true do
  id,msg,dist = rednet.receive()
  if clientIDs[id] then
    local tab = textutils.unserialize(msg)
    insertFix(tab[1],tab[2],vector.new(tab[3],tab[4],tab[5]))
  else
    insertFix(id,dist,vector.new(myLoc[1],myLoc[2],myLoc[3]))
    --print(id.."=>"..msg)
  end
end
end