Radar IFF System 2

local Radar_Data = {}--Distance,Azimuth,Elevation,
local Radar_Targets = {}--X,Y,Z,Identified?, Time last Seen
local Heading = 0
local GPS = {0,0,0}
local Tags = {}-- ID, X, Y, Z, Time last sent
function draw()
GPS = {input.getNumber(1),input.getNumber(2),input.getNumber(3)}
end

function FixHeading()
compass_input = input.getNumber(whatever input channel)
pi2 = math.pi*2
Heading = math.fmod ((compass_input+1.25)*pi2, pi2)
end

function Concert(D,A,E,I)
local ID = Radar_Targets[I][4]
local Y = math.sin(-1*(A/180))*D
local Z = math.sin(E/180)*D
local X = math.sqrt(D^2 - Y^2 - Z^2)
Radar_Targets[I] = {X,Y,Z,ID,0}
end

function Identify(ID)
local Distance
for i = 1, #Tags do
  Distance = math.sqrt(math.pwr((Radar_Targets[ID][1] - Tags[i][1]),2) + math.pwr((Radar_Targets[ID][2] - Tags[i][3]),2) + math.pwr((Radar_Targets[ID][3] - Tags[i][4]),2))
  if Distance < 50 then Radar_Targets[ID][4] = Tags[i][1] end
end
if Radar_Targets[ID][4] == nil then Radar_Targets[ID][4] = -1 end
end