gps_controller

local component=require("component")
local event=require("event")
local computer=require("computer")

--(детерминант для метода Крамера)
function det(a1,a2,a3,a4,a5,a6,a7,a8,a9)
  return a1*a5*a9+a2*a6*a7+a3*a4*a8-a1*a6*a8-a2*a4*a9-a3*a5*a7
end

--(Сам метод Крамера)
function cram(a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4)
  local det_coeff=det(a1,a2,a3,b1,b2,b3,c1,c2,c3)
  if not det_coeff then return nil end
  local det1=det(a4,a2,a3,b4,b2,b3,c4,c2,c3)
  local det2=det(a1,a4,a3,b1,b4,b3,c1,c4,c3)
  local det3=det(a1,a2,a4,b1,b2,b4,c1,c2,c4)
  return det1/det_coeff,det2/det_coeff,det3/det_coeff
end

--(Применение к нашей задаче)
function triangulate(x1,y1,z1,d1,x2,y2,z2,d2,x3,y3,z3,d3,x4,y4,z4,d4)
  return cram(
2*(x2-x1),2*(y2-y1),2*(z2-z1),d1^2-d2^2-x1^2-y1^2-z1^2+x2^2+y2^2+z2^2,
2*(x3-x2),2*(y3-y2),2*(z3-z2),d2^2-d3^2-x2^2-y2^2-z2^2+x3^2+y3^2+z3^2,
2*(x4-x3),2*(y4-y3),2*(z4-z3),d3^2-d4^2-x3^2-y3^2-z3^2+x4^2+y4^2+z4^2)
end

local wired_modem
local wireless_modem
for i in component.list("modem")
do
  if component.proxy(i).isWireless()
  then
    wireless_modem=component.proxy(i)
  else
    wired_modem=component.proxy(i)
  end
end

local request_lifetime = 2
local requesttable={}

function wake_relays()
  wired_modem.broadcast(312,"gps awake")
end

function handle_modem(eventname,laddr,raddr,port,distance,message,...)
  print("Caught")
  if(eventname=="modem_message" and message=="gps_response" and distance==0)
  then
    local id,cx,cy,cz,cd,ra=...
    print("Got response from '"..id.."'.")
    if requesttable[ra]
    then
      if not requesttable[ra][id]
      then
        requesttable[ra].hits=requesttable[ra].hits+1
        requesttable[ra][id]=true
        requesttable[ra][requesttable[ra].hits]={cx,cy,cz,cd}
      end
    else
      requesttable[ra]={lifetime=request_lifetime}
      requesttable[ra][id]=true
      requesttable[ra][1]={cx,cy,cz,cd}
      requesttable[ra].hits=1
    end
    if requesttable[ra].hits>=4 then respond(ra) end
  end
end

function respond(ra)
  local rt=requesttable[ra]
  local x,y,z=triangulate(
    rt[1][1],rt[1][2],rt[1][3],rt[1][4],
    rt[2][1],rt[2][2],rt[2][3],rt[2][4],
    rt[3][1],rt[3][2],rt[3][3],rt[3][4],
    rt[4][1],rt[4][2],rt[4][3],rt[4][4])
  requesttable[ra]=nil
  print(ra)
  print(x,y,z)
  if(wireless_modem)
  then
    wireless_modem.setStrength(400)
    wireless_modem.send(ra,312,"gps_response",x,y,z)
  end
end

function handle_requests()
  local remt={}
  for k,v in pairs(requesttable)
  do
    v.lifetime=v.lifetime-1
    if(v.lifetime<=0) then table.insert(remt,k) end
  end
  for _,i in pairs(remt) do requesttable[i]=nil; print("Removing request from "..i.." due to timeout") end
end

--(Собственно, начало проги)
wake_relays()
wired_modem.open(312)
--event.listen("modem_message",handle_modem)
local last_check=computer.uptime()
while true do
  local tbl={event.pull(1,"modem_message")}
  if(#tbl>0) then handle_modem(table.unpack(tbl)) end
  if computer.uptime()-last_check >= 1 then last_check=computer.uptime();  handle_requests() end
end