[OpenComputers] GPS_lib

local component = require('component')
local modem = nil
if component.isAvailable('modem') then
  modem = component.modem
end
local computer = require('computer')
local floor, sqrt, abs = math.floor, math.sqrt, math.abs

local CHANNEL_GPS, gps = 65534, {}

local function round(v, m)
  m = m or 1.0
  return {
    x = floor((v.x+(m*0.5))/m)*m,
    y = floor((v.y+(m*0.5))/m)*m,
    z = floor((v.z+(m*0.5))/m)*m
  }
end

local function len(v)
  return sqrt(v.x^2 + v.y^2 + v.z^2)
end

local function cross(v, b)
  return {x = v.y*b.z-v.z*b.y, y = v.z*b.x-v.x*b.z, z = v.x*b.y-v.y*b.x}
end

local function dot(v, b)
  return v.x*b.x + v.y*b.y + v.z*b.z
end

local function add(v, b)
  return {x = v.x+b.x, y = v.y+b.y, z = v.z+b.z}
end

local function sub(v, b)
  return {x = v.x-b.x, y = v.y-b.y, z = v.z-b.z}
end

local function mul(v, m)
  return {x = v.x*m, y = v.y*m, z = v.z*m}
end

local function norm(v)
  return mul(v, 1/len(v))
end

local function trilaterate(A, B, C)
  local a2b = {x = B.x-A.x, y = B.y-A.y, z = B.z-A.z}
  local a2c = {x = C.x-A.x, y = C.y-A.y, z = C.z-A.z}
  if abs(dot(norm(a2b), norm(a2c))) > 0.999 then
    return nil
  end
  local d = len(a2b)
  local ex = norm(a2b)
  local i = dot(ex, a2c)
  local ey = norm(sub(mul(ex, i), a2c))
  local j = dot(ey, a2c)
  local ez = cross(ex, ey)
  local r1 = A.d
  local r2 = B.d
  local r3 = C.d
  local x = (r1^2 - r2^2 + d^2) / (2*d)
  local y = (r1^2 - r3^2 - x^2 + (x-i)^2 + j^2) / (2*j)
  local result = add(A, add(mul(ex, x), mul(ey, y)))
  local zSquared = r1^2 - x^2 - y^2
  if zSquared > 0 then
    local z = sqrt( zSquared )
    local result1 = add(result, mul(ez, z))
    local result2 = add(result, mul(ez, z))
    local rounded1, rounded2 = round(result1, 0.01), round(result2, 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 round(result, 0.01)
end

local function narrow(p1, p2, fix)
  local dist1 = abs(len(sub(p1, fix)) - fix.d)
  local dist2 = abs(len(sub(p2, fix)) - fix.d)
  if abs(dist1 - dist2) < 0.01 then
    return p1, p2
  elseif dist1 < dist2 then
    return round(p1, 0.01)
  else
    return round(p2, 0.01)
  end
end

function gps.locate(DEBUG)
  if not modem then
    if DEBUG then
      print('No wireless modem attached')
    end
    return nil
  end
  if DEBUG then
    print('Finding position...')
  end
  modem.open(CHANNEL_GPS)
  modem.broadcast(CHANNEL_GPS, 'PING')
  local tFixes = {}
  local pos1, pos2 = nil, nil
  local timeout = computer.uptime()+2
  while true do
    local e = {computer.pullSignal(1)}
    if e[1] == 'modem_message' and e[6] == 'GPS' then
      local tFix = {x = e[7], y = e[8], z = e[9], d = e[5]}
      if DEBUG then
        print(tFix.d .. ' metres from ' .. tFix.x..', '..tFix.y..', '..tFix.z)
      end
      if tFix.d == 0 then
        pos1, pos2 = {tFix.x, tFix.y, tFix.z}, 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
    end
    if computer.uptime() >= timeout then
      break
    end
  end
  modem.close(CHANNEL_GPS)
  if pos1 and pos2 then
    if DEBUG 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 DEBUG then
      print('Position is '..pos1.x..', '..pos1.y..', '..pos1.z)
    end
    return pos1.x, pos1.y, pos1.z
  else
    if DEBUG then
      print('Could not determine position')
    end
    return nil
  end
end

return gps