From c1430f6dac79cf687a10c15af786fa9204a0b3b6 Mon Sep 17 00:00:00 2001
From: xAnavrins <xanavrins@gmail.com>
Date: Sat, 27 Jul 2019 00:13:24 -0400
Subject: [PATCH] GPS ambiguous position fix

---
 sys/modules/opus/gps.lua  | 24 +++++------
 sys/modules/opus/util.lua | 83 ++++++++++++++++++++++++---------------
 2 files changed, 64 insertions(+), 43 deletions(-)

diff --git a/sys/modules/opus/gps.lua b/sys/modules/opus/gps.lua
index 1b3e223..0ca7a66 100644
--- a/sys/modules/opus/gps.lua
+++ b/sys/modules/opus/gps.lua
@@ -1,3 +1,5 @@
+local Util = require('opus.util')
+
 local GPS = { }
 
 local device = _G.device
@@ -89,19 +91,17 @@ end
 -- end stock gps api
 
 function GPS.trilaterate(tFixes)
-	local pos1, pos2 = trilaterate(tFixes[1], tFixes[2], tFixes[3])
-
-	if pos2 then
-		pos1, pos2 = narrow(pos1, pos2, tFixes[4])
+	local attemps = 0
+	for tFixes in Util.permutation(tFixes) do
+		attemps = attemps + 1
+		local pos1, pos2 = trilaterate(tFixes[4], tFixes[3], tFixes[2])
+		if pos2 then
+			pos1, pos2 = narrow(pos1, pos2, tFixes[1])
+		end
+		if not pos2 then
+			return pos1, attemps
+		end
 	end
-
-	if pos1 and pos2 then
-		print("Ambiguous position")
-		print("Could be "..pos1.x..","..pos1.y..","..pos1.z.." or "..pos2.x..","..pos2.y..","..pos2.z )
-		return
-	end
-
-	return pos1
 end
 
 return GPS
\ No newline at end of file
diff --git a/sys/modules/opus/util.lua b/sys/modules/opus/util.lua
index eee7f70..4604199 100644
--- a/sys/modules/opus/util.lua
+++ b/sys/modules/opus/util.lua
@@ -46,10 +46,10 @@ function Util.tryTimes(attempts, f, ...)
 end
 
 function Util.timer()
-  local ct = os.clock()
-  return function()
-    return os.clock() - ct
-  end
+	local ct = os.clock()
+	return function()
+		return os.clock() - ct
+	end
 end
 
 Util.Timer = Util.timer -- deprecate
@@ -678,33 +678,33 @@ end
 
 -- https://github.com/MightyPirates/OpenComputers
 function Util.parse(...)
-  local params = table.pack(...)
-  local args = {}
-  local options = {}
-  local doneWithOptions = false
-  for i = 1, params.n do
-    local param = params[i]
-    if not doneWithOptions and type(param) == "string" then
-      if param == "--" then
-        doneWithOptions = true -- stop processing options at `--`
-      elseif param:sub(1, 2) == "--" then
-        local key, value = param:match("%-%-(.-)=(.*)")
-        if not key then
-          key, value = param:sub(3), true
-        end
-        options[key] = value
-      elseif param:sub(1, 1) == "-" and param ~= "-" then
-        for j = 2, string.len(param) do
-          options[string.sub(param, j, j)] = true
-        end
-      else
-        table.insert(args, param)
-      end
-    else
-      table.insert(args, param)
-    end
-  end
-  return args, options
+	local params = table.pack(...)
+	local args = {}
+	local options = {}
+	local doneWithOptions = false
+	for i = 1, params.n do
+		local param = params[i]
+		if not doneWithOptions and type(param) == "string" then
+			if param == "--" then
+				doneWithOptions = true -- stop processing options at `--`
+			elseif param:sub(1, 2) == "--" then
+				local key, value = param:match("%-%-(.-)=(.*)")
+				if not key then
+					key, value = param:sub(3), true
+				end
+				options[key] = value
+			elseif param:sub(1, 1) == "-" and param ~= "-" then
+				for j = 2, string.len(param) do
+					options[string.sub(param, j, j)] = true
+				end
+			else
+				table.insert(args, param)
+			end
+		else
+			table.insert(args, param)
+		end
+	end
+	return args, options
 end
 
 function Util.args(arg)
@@ -799,4 +799,25 @@ function Util.getOptions(options, args, ignoreInvalid)
 	return true, Util.size(rawOptions)
 end
 
+-- https://www.lua.org/pil/9.3.html
+function Util.permutation(tbl)
+	local function permgen(a, n)
+		if n == 0 then
+			coroutine.yield(a)
+		else
+			for i=1,n do
+				a[n], a[i] = a[i], a[n]
+				permgen(a, n - 1)
+				a[n], a[i] = a[i], a[n]
+			end
+		end
+	end
+
+	local co = coroutine.create(function() permgen(tbl, #tbl) end)
+	return function()
+		local _, res = coroutine.resume(co)
+		return res
+	end
+end
+
 return Util