added GPS cli app

src/bin/GPS.hx

@@ -0,0 +1,71 @@
+package bin;
+
+import lib.Pos3;
+import lib.Vec.Vec3;
+import lib.cli.TermHandle;
+import lib.cli.CLIApp;
+
+using tink.CoreApi;
+
+class GPS extends CLIApp {
+	private var handle:TermHandle;
+
+	public function new() {}
+
+	public function invoke(handle: TermHandle):Future<Bool> {
+		this.handle = handle;
+
+		var subcommand = handle.args[0];
+		var subcommand_args = handle.args.slice(1);
+
+		switch (subcommand) {
+			case "set":
+				return Future.sync(setManuelPos(subcommand_args));
+			case "status":
+				return Future.sync(getGPSStatus());
+			case "locate":
+				kernel.gps.GPS.instance.locate();
+				return Future.sync(true);
+			default:
+				handle.writeLn("Unknown subcommand: " + subcommand);
+				return Future.sync(false);
+
+		}
+
+		return Future.sync(true);
+	}
+
+	private function setManuelPos(args: Array<String>): Bool {
+		var x: Float = Std.parseFloat(args[0]);
+		var y: Float = Std.parseFloat(args[1]);
+		var z: Float = Std.parseFloat(args[2]);
+
+		var pos: Pos3 = new Vec3<Float>(x, y, z);
+
+		kernel.gps.GPS.instance.setManualPosition(pos);
+
+		return true;
+	}
+
+	private function getGPSStatus(): Bool {
+
+		var pos = kernel.gps.GPS.instance.getPosition();
+		if (pos != null) {
+			handle.writeLn('Position x:${pos.x} y:${pos.y} z:${pos.z}');
+		} else {
+			handle.writeLn("Position not available");
+			return true;
+		}
+
+		var acc = kernel.gps.GPS.instance.getAccuracy();
+		if (acc == 1){
+			handle.writeLn("Accuracy: Low");
+		} else if (acc == 2){
+			handle.writeLn("Accuracy: Medium");
+		} else if (acc == 3){
+			handle.writeLn("Accuracy: High");
+		}
+
+		return true;
+	}
+}

src/bin/Terminal.hx

@@ -154,6 +154,8 @@ class Terminal extends UIApp {
 				return new Redstone();
 			case "disk":
 				return new Disk();
+			case "gps":
+				return new GPS();
 			default:
 				return null;
 		}

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/KernelEvents.hx

@@ -34,7 +34,7 @@ class KernelEvents {
 		channel:Int,
 		replyChannel:Int,
 		message:Dynamic,
-		distance:Int
+		distance:Null<Float>
 	}>;
 	public final onMonitorResize:Signal<String>;
 	public final onMonitorTouch:Signal<{addr:String, pos:Pos}>;
@@ -71,7 +71,7 @@ class KernelEvents {
 		channel:Int,
 		replyChannel:Int,
 		message:Dynamic,
-		distance:Int
+		distance:Null<Float>
 	}> = Signal.trigger();
 	private final onMonitorResizeTrigger:SignalTrigger<String> = Signal.trigger();
 	private final onMonitorTouchTrigger:SignalTrigger<{addr:String, pos:Pos}> = Signal.trigger();

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/log/Log.hx

@@ -25,28 +25,29 @@ class Log {
 	}
 
 	public static function info(msg:Dynamic, ?pos:haxe.PosInfos) {
-		instance.log({level: Info, message: Std.string(msg),time: 0});
+		instance.log({level: Info, message: Std.string(msg),time: 0},pos);
 	}
 
 	public static function warn(msg:Dynamic, ?pos:haxe.PosInfos) {
-		instance.log({level: Warn, message: Std.string(msg),time: 0});
+		instance.log({level: Warn, message: Std.string(msg),time: 0},pos);
 	}
 
 	public static function error(msg:Dynamic, ?pos:haxe.PosInfos) {
-		instance.log({level: Error, message: Std.string(msg),time: 0});
+		instance.log({level: Error, message: Std.string(msg),time: 0},pos);
 	}
 
 	public static function debug(msg:Dynamic, ?pos:haxe.PosInfos) {
 		#if debug
-		instance.log({level: Debug, message: Std.string(msg),time: 0});
+		instance.log({level: Debug, message: Std.string(msg),time: 0},pos);
 		#end
 	}
 
 	public static function silly(msg:Dynamic, ?pos:haxe.PosInfos) {
-		instance.log({level: Silly, message: Std.string(msg),time: 0});
+		instance.log({level: Silly, message: Std.string(msg),time: 0},pos);
 	}
 
 	private function log(line: LogLine, ?pos:haxe.PosInfos) {
+		line.origin = pos.className;
 		logLines.push(line);
 
 		if (logLines.length > MAX_LINES) {
@@ -54,7 +55,7 @@ class Log {
 		}
 
 		#if webconsole
-		Debug.printWeb('[${Std.string(line.level)}] ${line.message}');
+		Debug.printWeb('[${Std.string(line.level)}][${line.origin}] ${line.message}');
 		#end
 
 		onLogTrigger.trigger(line);

src/kernel/log/LogLine.hx

@@ -4,4 +4,5 @@ typedef LogLine = {
 	level: LogLevel,
 	message: String,
 	time: Int,
+	?origin: String,
 }

src/kernel/net/INetworkInterface.hx

@@ -13,5 +13,5 @@ interface INetworkInterface {
 	public function send(chan: Int,replyChan: Int,payload: Any):Void;
 	public function name():String;
 	public function getBaseRoutingCost():Int;
-	public var onMessage (default, null): Signal<Package>;
+	public var onMessage (default, null): Signal<{pack:Package,?dist:Float}>;
 }

src/kernel/net/Loopback.hx

@@ -9,9 +9,9 @@ using tink.CoreApi;
 class Loopback implements INetworkInterface {
 	public static final instance:Loopback = new Loopback();
 
-	public var onMessage(default, null):Signal<Package>;
+	public var onMessage(default, null):Signal<{pack:Package,dist:Null<Float>}>;
 
-	private final onMessageTrigger: SignalTrigger<Package> = Signal.trigger();
+	private final onMessageTrigger: SignalTrigger<{pack:Package,dist:Null<Float>}> = Signal.trigger();
 	private var openChans: Array<Int> = [];
 
 	private function new() {
@@ -38,7 +38,7 @@ class Loopback implements INetworkInterface {
 
 	public function send(chan:Int, replyChan:Int, payload:Any) {
 		if (this.openChans.contains(chan)){
-			this.onMessageTrigger.trigger(payload);
+			this.onMessageTrigger.trigger({pack:payload,dist:null});
 		}else{
 			Log.silly("Loopback got package on non open channel");
 		}

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.
@@ -60,7 +60,7 @@ class Net {
 	}
 
 	private function setupInterf(interf: INetworkInterface) {
-		interf.onMessage.handle(pack -> handle(pack,interf));
+		interf.onMessage.handle(e -> handle(e.pack,interf,e.dist));
 		interf.listen(networkID);
 		interf.listen(BRODCAST_PORT);
 	}
@@ -68,7 +68,7 @@ class Net {
 	/**
 		Called when a new package comes in.
 	**/
-	private function handle(pack:Package,interf: INetworkInterface) {
+	private function handle(pack:Package,interf: INetworkInterface, ?dist: Float) {
 		if (pack.toID == this.networkID || pack.toID == Net.BRODCAST_PORT){
 			switch pack.type {
 				case Data(_) | DataNoResponse(_):
@@ -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);
+		}
+	}
 }

src/kernel/net/Package.hx

@@ -1,5 +1,7 @@
 package kernel.net;
 
+import lib.Pos3;
+
 typedef NetworkID = Int;
 
 enum PackageTypes {
@@ -9,6 +11,8 @@ enum PackageTypes {
 	RouteDiscover(reachableIDs: Array<{id:NetworkID,cost:Int}>);
 	RouteDiscoverResponse(reachableIDs: Array<{id:NetworkID,cost:Int}>);
 	RouteDiscoverUpdate(reachableIDs: Array<{id:NetworkID,cost:Int}>);
+	GPSResponse(pos:Pos3);
+	GPSRequest();
 }
 
 /**
@@ -22,6 +26,15 @@ enum PackageTypes {
 	public final data:Dynamic;
 	public var ttl: Int;
 
+	public function new(fromID:NetworkID, toID:NetworkID, msgID:Int, type:PackageTypes, data:Dynamic, ttl:Int) {
+		this.fromID = fromID;
+		this.toID = toID;
+		this.msgID = msgID;
+		this.type = type;
+		this.data = data;
+		this.ttl = ttl;
+	}
+
 	/**
 		Create package that can be used as a response.
 	**/

src/kernel/peripherals/Modem.hx

@@ -1,5 +1,6 @@
 package kernel.peripherals;
 
+import kernel.log.Log;
 import kernel.net.Package;
 import kernel.net.INetworkInterface;
 
@@ -7,9 +8,9 @@ using tink.CoreApi;
 
 class Modem implements INetworkInterface implements IPeripheral {
 	public final addr:String;
-	public var onMessage(default, null):Signal<Package>;
+	public var onMessage(default, null):Signal<{pack:Package,dist:Null<Float>}>;
 
-	private final onMessageTrigger:SignalTrigger<Package> = Signal.trigger();
+	private final onMessageTrigger:SignalTrigger<{pack:Package,dist:Null<Float>}> = Signal.trigger();
 	private final native:cc.periphs.Modem.Modem;
 
 	@:allow(kernel.peripherals)
@@ -19,6 +20,7 @@ class Modem implements INetworkInterface implements IPeripheral {
 		this.addr = addr;
 
 		KernelEvents.instance.onModemMessage.handle(params ->{
+			try{
 				if (params.addr == this.addr){
 					var pack:Package = {
 						fromID: params.message.fromID,
@@ -29,11 +31,18 @@ class Modem implements INetworkInterface implements IPeripheral {
 						ttl: params.message.ttl,
 					};
 
-				this.onMessageTrigger.trigger(pack);
+					this.onMessageTrigger.trigger({pack: pack, dist: params.distance});
+				}
+			}catch(e:Dynamic){
+				Log.error("Error while parsing modem message");
 			}
 		});
 	}
 
+	public function isWireless():Bool {
+		return native.isWireless();
+	}
+
 	public function listen(chan:Int) {
 		native.open(chan);
 	}

src/lib/KVStore.hx

@@ -17,12 +17,13 @@ class KVStore {
 	}
 
 	private static function getNamespaceFile(namespace: String): String {
-		return '/$namespace';
+		return '/var/ns/$namespace';
 	}
 
 	public function load() {
-		if (FS.exists(getNamespaceFile(namespace))){
+		var nsFile = getNamespaceFile(this.namespace);
-			var handle = FS.openRead("/" + namespace);
+		if (FS.exists(nsFile)){
+			var handle = FS.openRead(nsFile);
 			parseFile(handle.readAll());
 		}
 	}

src/lib/Pos3.hx

@@ -3,18 +3,18 @@ package lib;
 import lib.Vec.Vec3;
 
 /**
-	Reporesents a Point in a 3D `Int` System.
+	Reporesents a Point in a 3D `Float` System.
-	Basicly a wrapper for Vec3<Int> with some extra functions.
+	Basicly a wrapper for Vec3<Float> with some extra functions.
 	`Y` represents the height of the point.
 **/
 @:forward(x,y,z)
-abstract Pos3(Vec3<Int>) from Vec3<Int> to Vec3<Int>{
+abstract Pos3(Vec3<Float>) from Vec3<Float> to Vec3<Float>{
-	inline public function new(i:Vec3<Int>) {
+	inline public function new(i:Vec3<Float>) {
 		this = i;
 	}
 
 	@:op(A + B)
-	public function add(rhs: Vec3<Int>):Pos3 {
+	public function add(rhs: Vec3<Float>):Pos3 {
 		return new Pos3({
 			y: this.y + rhs.y,
 			x: this.x + rhs.x,
@@ -23,7 +23,7 @@ abstract Pos3(Vec3<Int>) from Vec3<Int> to Vec3<Int>{
 	}
 
 	@:op(A - B)
-	public function sub(rhs: Vec3<Int>):Pos3 {
+	public function sub(rhs: Vec3<Float>):Pos3 {
 		return new Pos3({
 			y: this.y - rhs.y,
 			x: this.x - rhs.x,
@@ -32,11 +32,20 @@ abstract Pos3(Vec3<Int>) from Vec3<Int> to Vec3<Int>{
 	}
 
 	@:op(A * B)
-	public function multiply(rhs: Vec3<Int>): Pos3 {
+	public function multiplyScalar(rhs: Float): Pos3 {
 		return new Pos3({
-			y: this.y * rhs.y,
+			y: this.y * rhs,
-			x: this.x * rhs.x,
+			x: this.x * rhs,
-			z: this.z * rhs.z
+			z: this.z * rhs
+		});
+	}
+
+	@:op(A / B)
+	public function divideScalar(rhs: Float): Pos3 {
+		return new Pos3({
+			y: this.y / rhs,
+			x: this.x / rhs,
+			z: this.z / rhs
 		});
 	}
 
@@ -48,4 +57,43 @@ abstract Pos3(Vec3<Int>) from Vec3<Int> to Vec3<Int>{
 			z: -this.z
 		});
 	}
+
+	public function dot(rhs: Vec3<Float>): Float {
+		return this.x * rhs.x + this.y * rhs.y + this.z * rhs.z;
+	}
+
+	public function cross(rhs: Vec3<Float>):Pos3 {
+		return new Pos3({
+			x: this.y * rhs.z - this.z * rhs.y,
+			y: this.z * rhs.x - this.x * rhs.z,
+			z: this.x * rhs.y - this.y * rhs.x
+		});
+	}
+
+	public function normalize():Pos3 {
+		var l = length();
+		return multiplyScalar(1 / l);
+	}
+
+	public function length():Float {
+		return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
+	}
+
+	@:op(A == B)
+	public function equals(rhs:Pos3):Bool {
+		return close(rhs);
+	}
+
+	@:op(A != B)
+	public function notEquals(rhs:Pos3):Bool {
+		return !close(rhs);
+	}
+
+	public function close(rhs:Pos3, epsilon:Float = 0.001):Bool {
+		return Math.abs(this.x - rhs.x) < epsilon && Math.abs(this.y - rhs.y) < epsilon && Math.abs(this.z - rhs.z) < epsilon;
+	}
+
+	public function toString():String {
+		return 'Pos3(${this.x}, ${this.y}, ${this.z})';
+	}
 }

src/lib/Vec.hx

@@ -3,10 +3,21 @@ package lib;
 @:structInit class Vec2<T> {
 	public var x:T;
 	public var y:T;
+
+	public function new(x:T, y:T) {
+		this.x = x;
+		this.y = y;
+	}
 }
 
 @:structInit class Vec3<T> {
 	public var x:T;
 	public var y:T;
 	public var z:T;
+
+	public function new(x:T, y:T, z:T) {
+		this.x = x;
+		this.y = y;
+		this.z = z;
+	}
 }