fixed GPS

src/kernel/gps/GPS.hx

@@ -1,11 +1,14 @@
 package kernel.gps;
 
+import kernel.log.Log;
 import lib.KVStore;
 import kernel.net.Net;
 import kernel.net.INetworkInterface;
 import kernel.net.Package;
 import lib.Pos3;
 
+using tink.CoreApi;
+
 /**
 	Determines the position of the computer based on the distance to other computers.
 	When receiving a message from another computer via wireless, the distance is also received.
@@ -15,6 +18,7 @@ class GPS {
 	public static var instance:GPS;
 
 	private var shouldRespond = true;
+	private var shouldDoWholeNumberCheck = true;
 	private var posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed)
 	private var cachedPosition:Pos3;
 	private var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = [];
@@ -112,12 +116,18 @@ class GPS {
 				var response = new Package(Net.instance.networkID,pack.fromID, pack.msgID, GPSResponse(cachedPosition),null,0);
 				iface.send(pack.fromID,Net.instance.networkID,response);
 			case GPSResponse(pos):
-				if (lastPositionResponse.contains({pos:pos,dist:dist})) return;
+				if (lastPositionResponse.contains({pos:pos,dist:dist})) return; // Ignore duplicate responses
-				lastPositionResponse.push({pos:pos,dist:dist});
+
-				if (lastPositionResponse.length > 5) lastPositionResponse.shift();
+				lastPositionResponse.push({pos:pos,dist:dist});
-				if (lastPositionResponse.length < 3) return;
+
+				// TODO: wait for a few seconds before calculating the position, so we can get more responses
+
+				if (lastPositionResponse.length < 4) return; // We need at least 3 responses to calculate the position
+
+				var calculatedPosition = calculatePosition().round();
+
+				lastPositionResponse = []; // Reset the response array
 
-				var calculatedPosition = calculatePosition();
 				if (calculatedPosition == null) return;
 				cachedPosition = calculatedPosition;
 				posAccuracy = 3;
@@ -137,62 +147,46 @@ class GPS {
 		var r2 = lastPositionResponse[1].dist;
 		var r3 = lastPositionResponse[2].dist;
 
-		return trilateration(p1,p2,p3,r1,r2,r3);
+		var result = trilateration(p1,p2,p3,r1,r2,r3);
 
-		// var calculatedPositions: Array<Pos3> = [];
+		if (result.a.close(result.b)) return result.a;
 
-		// // Loop through all possible permutations of the last 3 responses
+		// If we have more than 3 responses, we can use the 4th response to determine which of the two positions is correct
-		// for (i in 0...lastPositionResponse.length) {
+		// TODO: if this is a pocket computer we cant use this since it can move freely
-		// 	for (j in 0...lastPositionResponse.length) {
+		if (lastPositionResponse.length > 3){
-		// 		if (i == j) continue;
+			var err1 = Math.abs(result.a.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);
-		// 		for (k in 0...lastPositionResponse.length) {
+			var err2 = Math.abs(result.b.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);
-		// 			if (i == k || j == k) continue;
 
-		// 			var p1 = lastPositionResponse[i].pos;
+			if (Math.abs(err1 - err2) < 0.0001) {
-		// 			var p2 = lastPositionResponse[j].pos;
+				Log.warn("Failed to determine GPS position via 4th fix");
-		// 			var p3 = lastPositionResponse[k].pos;
+				return null; // The two positions are essentially the same, so we cant determine which one is correct
-
-		// 			var r1 = lastPositionResponse[i].dist;
-		// 			var r2 = lastPositionResponse[j].dist;
-		// 			var r3 = lastPositionResponse[k].dist;
-
-		// 			calculatedPositions.push(trilateration(p1,p2,p3,r1,r2,r3));
-		// 		}
-		// 	}
-		// }
-
-		// var tainted = false;
-		// // Check if all calculated positions are the same
-		// for (i in 0...calculatedPositions.length) {
-		// 	for (j in 0...calculatedPositions.length) {
-		// 		if (i == j) continue;
-		// 		if (calculatedPositions[i] != calculatedPositions[j]){
-		// 			Log.warn("GPS not all calculated positions are the same");
-		// 			tainted = true;
-		// 		}
-		// 	}
-		// }
-
-		// if (!tainted) return calculatedPositions[0];
-
-		// // Get the most common position
-		// var mostCommon:Pos3 = null;
-		// var mostCommonCount = 0;
-		// for (i in 0...calculatedPositions.length) {
-		// 	var count = 0;
-		// 	for (j in 0...calculatedPositions.length) {
-		// 		if (calculatedPositions[i] == calculatedPositions[j]) count++;
-		// 	}
-		// 	if (count > mostCommonCount) {
-		// 		mostCommon = calculatedPositions[i];
-		// 		mostCommonCount = count;
-		// 	}
-		// }
-
-		// return mostCommon;
 			}
 
-	private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pos3 {
+			if (err1 < err2) return result.a;
+			if (err2 < err1) return result.b;
+		}
+
+		// last resort, just use the position that is closest to a whole number (whithin a resonable margin of error)
+		if (!shouldDoWholeNumberCheck) return null;
+
+		// TODO: mark the position as not so accurate if we use this method
+
+		var err1 = (result.a - result.a.round()).length();
+		var err2 = (result.b - result.b.round()).length();
+
+		if (err1 < err2 && err1 < 0.0001) {
+			return result.a;
+		} else if (err2 < 0.0001) {
+			return result.b;
+		}
+
+		return null;
+	}
+
+	/**
+		Determines the position(s) of a point given 3 other points and the distance to each of them.
+	**/
+	private function trilateration(p1:Pos3,p2:Pos3,p3: Pos3,r1:Float,r2:Float,r3:Float):Pair<Pos3,Pos3> {
 		var a2b = p2 - p1;
 		var a2c = p3 - p1;
 
@@ -211,9 +205,9 @@ class GPS {
 		var zSquared = r1 * r1 - x * x - y * y;
 		if (zSquared > 0) {
 			var z = Math.sqrt(zSquared);
-			result += ez * z;
+			return new Pair(result + (ez * z),result - (ez * z));
 		}
 
-		return result;
+		return new Pair(result,result);
 	}
 }