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

local mlocations={
	["modem_517"]={25,69,-139},
	["modem_518"]={29,73,-135},
	["modem_519"]={25,69,-131},
	["modem_520"]={21,73,-135},
}
for k,v in pairs(mlocations) do
	peripheral.call(k,"open",824)
end
local rt={}
local pos1={}
local pos2={}
while true do
	local p={os.pullEvent("modem_message")}
	if p[3]==824 then
		rt[p[4]]=rt[p[4]] or {}
		local tFixes=rt[p[4]]
		local tFix={position=vector.new(unpack(mlocations[p[2]])),distance=p[6]}
		table.insert(tFixes,tFix)
		if #tFixes >= 3 then
			if not pos1[p[4]] then
				pos1[p[4]],pos2[p[4]]=trilaterate(tFixes[1],tFixes[2],tFixes[#tFixes])
			else
				pos1[p[4]],pos2[p[4]]=narrow(pos1[p[4]],pos2[p[4]],tFixes[#tFixes])
			end
		end
		if pos1[p[4]] and not pos2[p[4]] then
			print("Position for "..p[4].." is "..pos1[p[4]].x..","..pos1[p[4]].y..","..pos1[p[4]].z)
			pos1[p[4]]=nil
			pos2[p[4]]=nil
			rt[p[4]]=nil
		end
	end
end