added GPS

src/kernel/Init.hx

@@ -1,5 +1,7 @@
 package kernel;
 
+import kernel.fs.FS;
+import kernel.gps.GPS;
 import kernel.log.Log;
 import kernel.turtle.Turtle;
 import haxe.MainLoop;
@@ -27,6 +29,8 @@ class Init {
 		Routing.instance = new Routing();
 		Net.instance = new Net();
 
+		GPS.instance = new GPS();
+
 		// Register default terminate handler
 		KernelEvents.instance.onTerminate.handle(_->{
 			OS.reboot();
@@ -36,6 +40,10 @@ class Init {
 
 		Routing.instance.init();
 
+		if (!FS.exists("/var/ns")) {
+			FS.makeDir("/var/ns");
+		}
+
 		MainLoop.add(()->{
 			KernelEvents.instance.startEventLoop();
 		});

src/kernel/gps/GPS.hx

@@ -0,0 +1,219 @@
+package kernel.gps;
+
+import lib.KVStore;
+import kernel.net.Net;
+import kernel.net.INetworkInterface;
+import kernel.net.Package;
+import lib.Pos3;
+
+/**
+	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.
+	You need at least 3 computers that know their position to determine the position of the computer.
+**/
+class GPS {
+	public static var instance:GPS;
+
+	private var shouldRespond = 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}> = [];
+
+	@:allow(kernel.Init)
+	private function new() {
+		this.loadCachedPosition();
+	}
+
+	public function setManualPosition(pos:Pos3) {
+		var kvstore = new KVStore("gps");
+		kvstore.set("mpos",pos);
+		kvstore.save();
+
+		if (cachedPosition == null) {
+			cachedPosition = pos;
+			posAccuracy = 2;
+		}
+	}
+
+	public function getPosition():Null<Pos3> {
+		return cachedPosition;
+	}
+
+	public function getAccuracy():Int {
+		return posAccuracy;
+	}
+
+	public function invalidatePosition() {
+		cachedPosition = null;
+		posAccuracy = 0;
+	}
+
+	public function locate() {
+		sendPositionRequest();
+	}
+
+	private function persistCachedPositon() {
+		if (cachedPosition == null) return;
+		var kvstore = new KVStore("gps");
+
+		kvstore.set("cpos",cachedPosition);
+		kvstore.save();
+	}
+
+	private function loadCachedPosition() {
+		var kvstore = new KVStore("gps");
+		kvstore.load();
+
+		var mPos:Null<Pos3> = kvstore.get("mpos"); // Manual set position
+		var cPos:Null<Pos3> = kvstore.get("cpos"); // Cached position
+
+		if (mPos != null && cPos != null && mPos == cPos) {
+			// Both are the same, so we can use the cached position
+			cachedPosition = mPos;
+			posAccuracy = 3;
+			return;
+		}
+
+		if (mPos != null && cPos != null && mPos != cPos){
+			// Both are different, so we can use the manual position
+			cachedPosition = mPos;
+			posAccuracy = 1;
+			return;
+		}
+
+		if (mPos == null && cPos != null) {
+			// No manual position set, so we can use the cached position
+			cachedPosition = cPos;
+			posAccuracy = 2;
+			return;
+		}
+
+		if (mPos != null && cPos == null) {
+			// No cached position, so we can use the manual position
+			cachedPosition = mPos;
+			posAccuracy = 2;
+			return;
+		}
+
+	}
+
+	private function sendPositionRequest() {
+		Net.instance.brodcastGPSRequest();
+	}
+
+	@:allow(kernel.net.Net)
+	private function handlePackage(pack:Package, dist: Float,iface: INetworkInterface) {
+		switch (pack.type) {
+			case GPSRequest:
+				if (!shouldRespond) return;
+				if (posAccuracy < 2) return;
+				if (cachedPosition == null) return;
+
+				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;
+				lastPositionResponse.push({pos:pos,dist:dist});
+				if (lastPositionResponse.length > 5) lastPositionResponse.shift();
+				if (lastPositionResponse.length < 3) return;
+
+				var calculatedPosition = calculatePosition();
+				if (calculatedPosition == null) return;
+				cachedPosition = calculatedPosition;
+				posAccuracy = 3;
+			default:
+		}
+	}
+
+	private function calculatePosition():Null<Pos3> {
+		if (lastPositionResponse.length < 3) return null;
+
+		// do a simple trilateration with the last 3 responses for now
+		var p1 = lastPositionResponse[0].pos;
+		var p2 = lastPositionResponse[1].pos;
+		var p3 = lastPositionResponse[2].pos;
+
+		var r1 = lastPositionResponse[0].dist;
+		var r2 = lastPositionResponse[1].dist;
+		var r3 = lastPositionResponse[2].dist;
+
+		return trilateration(p1,p2,p3,r1,r2,r3);
+
+		// var calculatedPositions: Array<Pos3> = [];
+
+		// // Loop through all possible permutations of the last 3 responses
+		// for (i in 0...lastPositionResponse.length) {
+		// 	for (j in 0...lastPositionResponse.length) {
+		// 		if (i == j) continue;
+		// 		for (k in 0...lastPositionResponse.length) {
+		// 			if (i == k || j == k) continue;
+
+		// 			var p1 = lastPositionResponse[i].pos;
+		// 			var p2 = lastPositionResponse[j].pos;
+		// 			var p3 = lastPositionResponse[k].pos;
+
+		// 			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 {
+		var a2b = p2 - p1;
+		var a2c = p3 - p1;
+
+		var d = a2b.length();
+		var ex = a2b.normalize();
+		var i = ex.dot(a2c);
+		var ey = (a2c - ex * i).normalize();
+		var j = ey.dot(a2c);
+		var ez = ex.cross(ey);
+
+		var x = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
+		var y = (r1 * r1 - r3 * r3 - x * x + (x - i) * (x - i) + j * j) / (2 * j);
+
+		var result = p1 + ex * x + ey * y;
+
+		var zSquared = r1 * r1 - x * x - y * y;
+		if (zSquared > 0) {
+			var z = Math.sqrt(zSquared);
+			result += ez * z;
+		}
+
+		return result;
+	}
+}

src/kernel/net/Net.hx

@@ -1,5 +1,6 @@
 package kernel.net;
 
+import kernel.gps.GPS;
 import haxe.ds.ReadOnlyArray;
 import kernel.net.Package.NetworkID;
 import kernel.peripherals.Peripherals.Peripheral;
@@ -9,7 +10,6 @@ import cc.OS;
 
 using tink.CoreApi;
 using Lambda;
-using lib.Extender.LambdaExtender;
 
 /**
 	Class responsible for everything network related.
@@ -82,6 +82,13 @@ class Net {
 				case RouteDiscover(_) | RouteDiscoverResponse(_) | RouteDiscoverUpdate(_):
 					// Delegate to Routing
 					Routing.instance.handleRoutePackage(pack,interf);
+				case GPSRequest | GPSResponse(_):
+					if (dist == null) {
+						Log.silly("Got a GPS package but no distance was provided");
+						return;
+					}
+					// Delegate to GPS
+					GPS.instance.handlePackage(pack,dist,interf);
 			}
 		}else{
 			// New message received but its not ment for us. Forward if possible.
@@ -260,4 +267,21 @@ class Net {
 
 		return arr;
 	}
+
+	@:allow(kernel.gps.GPS)
+	private function brodcastGPSRequest() {
+		var pack: Package = {
+			fromID: networkID,
+			toID: Net.BRODCAST_PORT,
+			ttl: 0, // Prevent forwarding
+			msgID: generateMessageID(),
+			type: GPSRequest,
+			data: null,
+		};
+
+		for (modem in Peripheral.instance.getModems()) {
+			if (!modem.isWireless()) continue;
+			modem.send(Net.BRODCAST_PORT, Net.instance.networkID, pack);
+		}
+	}
 }