added a timeout and improvements to GPS and INS

closes #6

src/bin/GPS.hx

@@ -51,11 +51,12 @@ class GPS extends CLIAppBase {
 		});
 
 		registerAsyncSubcommand("locate", (args) -> {
-			return kernel.gps.GPS.locate().map((pos) -> {
-				if (pos != null) {
-					handle.writeLine('Position x:${pos.x} y:${pos.y} z:${pos.z}');
-				} else {
-					handle.writeLine("Position not available");
+			return kernel.gps.GPS.locate().map((result) -> {
+				switch result {
+					case Success(pos):
+						handle.writeLine('Position x:${pos.x} y:${pos.y} z:${pos.z}');
+					case Failure(err):
+						handle.writeLine("Position not available: " + err);
 				}
 				return true;
 			});

src/kernel/gps/GPS.hx

@@ -1,5 +1,6 @@
 package kernel.gps;
 
+import lib.SingleTimeoutPromise;
 import kernel.log.Log;
 import lib.KVStore;
 import kernel.net.Net;
@@ -15,13 +16,15 @@ using tink.CoreApi;
 	You need at least 3 computers that know their position to determine the position of the computer.
 **/
 class GPS {
+	private static inline final TIMEOUT:Int = 1;
+
 	private static var shouldRespond = true;
 	private static var shouldDoWholeNumberCheck = true;
 	private static var posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed)
 	private static var cachedPosition:WorldPos;
 	private static var lastPositionResponse:Array<{pos:WorldPos, dist:Float}> = [];
 
-	private static var futureResolve:(pos:Null<WorldPos>) -> Void = null;
+	private static final locatePromise:SingleTimeoutPromise<WorldPos> = new SingleTimeoutPromise(TIMEOUT, brodcastPositionRequest);
 
 	@:allow(kernel.Init)
 	private static function init() {
@@ -58,18 +61,8 @@ class GPS {
 		posAccuracy = 0;
 	}
 
-	public static function locate():Future<Null<WorldPos>> {
-		// TODO: implenet a timeout
-		// TODO: dont send a request twice if the last one is still pending or we moved
-		return new Future<Null<WorldPos>>((resolve) -> {
-			futureResolve = resolve;
-			sendPositionRequest();
-			return null;
-		});
-	}
-
-	private static function resolveFuture(pos:Null<WorldPos>) {
-		futureResolve(pos);
+	public static function locate():Promise<WorldPos> {
+		return locatePromise.request();
 	}
 
 	private static function persistCachedPositon() {
@@ -117,7 +110,7 @@ class GPS {
 		}
 	}
 
-	private static function sendPositionRequest() {
+	private static function brodcastPositionRequest() {
 		Net.brodcastGPSRequest();
 	}
 
@@ -132,13 +125,13 @@ class GPS {
 				if (cachedPosition == null)
 					return;
 
-				var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition), null, 0);
+				var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition.x, cachedPosition.y, cachedPosition.z,), null, 0);
 				iface.send(pack.fromID, Net.networkID, response);
-			case GPSResponse(pos):
-				if (lastPositionResponse.contains({pos: pos, dist: dist}))
+			case GPSResponse(x, y, z):
+				if (lastPositionResponse.contains({pos: {x: x, y: y, z: z}, dist: dist}))
 					return; // Ignore duplicate responses
 
-				lastPositionResponse.push({pos: pos, dist: dist});
+				lastPositionResponse.push({pos: {x: x, y: y, z: z}, dist: dist});
 
 				// TODO: wait for a few seconds before calculating the position, so we can get more responses
 
@@ -158,7 +151,7 @@ class GPS {
 				cachedPosition = calculatedPosition;
 				posAccuracy = 3;
 
-				resolveFuture(calculatedPosition);
+				locatePromise.resolve(calculatedPosition);
 			default:
 		}
 	}
@@ -228,7 +221,7 @@ class GPS {
 		var i = ex.dot(a2c);
 		var ey = (a2c - ex * i).normalize();
 		var j = ey.dot(a2c);
-		var ez = ex.cross(ey);
+		var ez:WorldPos = 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);

src/kernel/gps/INS.hx

@@ -1,5 +1,6 @@
 package kernel.gps;
 
+import lib.SinglePromise;
 import kernel.log.Log;
 import kernel.turtle.Turtle;
 import lib.WorldPos;
@@ -9,6 +10,7 @@ using tink.CoreApi;
 class INS {
 	private static var heading:Null<WorldPos> = null;
 	private static var alingment:Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
+	private static final alignPromise:SinglePromise<Noise> = new SinglePromise(startAlign);
 
 	@:allow(kernel.turtle.Turtle)
 	private static function moveForward() {
@@ -81,7 +83,10 @@ class INS {
 	}
 
 	public static function align():Promise<Noise> {
-		Log.info("Aligning INS");
+		return alignPromise.request();
+	}
+
+	public static function startAlign():Promise<Noise> {
 		return new Promise<Noise>((resolve, reject) -> {
 			if (Turtle.getFuelLevel() < 2) {
 				Log.warn("Not enough fuel to align");
@@ -89,43 +94,43 @@ class INS {
 				return null;
 			}
 
-			GPS.locate().handle((pos1) -> {
-				Log.debug('pos1: $pos1');
-				if (pos1 == null) {
-					Log.warn("GPS not available for 1st position");
-					reject(new Error("GPS not available"));
-					return;
-				}
-
-				var moved = tryMoving();
-
-				if (moved == -1) {
-					Log.warn("Can't move");
-					reject(new Error("Can't move"));
-					return;
-				}
-
-				GPS.locate().handle((pos2) -> {
-					Log.debug('pos2: $pos2');
-					if (pos2 == null) {
-						Log.warn("GPS not available for 2nd position");
-						reject(new Error("GPS not available for 2nd position"));
+			GPS.locate().handle((result) -> {
+				switch result {
+					case Failure(err):
+						Log.warn("GPS not available for 1st position");
+						reject(new Error("GPS not available"));
 						return;
-					}
+					case Success(pos1):
+						var moved = tryMoving();
 
-					var cHeading = calcHeading(pos1, pos2, moved);
-					if (cHeading == null) {
-						Log.error("Can't calculate heading");
-						reject(new Error("Can't calculate heading"));
-						return;
-					}
+						if (moved == -1) {
+							Log.warn("Can't move");
+							reject(new Error("Can't move"));
+							return;
+						}
 
-					heading = cHeading;
-					moveBack(moved);
-					GPS.setINSPosition(pos1);
+						GPS.locate().handle((result) -> {
+							switch result {
+								case Failure(err):
+									Log.warn("GPS not available for 2nd position");
+									reject(new Error("GPS not available for 2nd position"));
+									return;
+								case Success(pos2):
+									var cHeading = calcHeading(pos1, pos2, moved);
+									if (cHeading == null) {
+										Log.error("Can't calculate heading");
+										reject(new Error("Can't calculate heading"));
+										return;
+									}
 
-					resolve(Noise);
-				});
+									heading = cHeading;
+									moveBack(moved);
+									GPS.setINSPosition(pos1);
+
+									resolve(Noise);
+							}
+						});
+				}
 			});
 			return null;
 		});

src/kernel/net/Package.hx

@@ -1,7 +1,5 @@
 package kernel.net;
 
-import lib.WorldPos;
-
 typedef NetworkID = Int;
 typedef GenericPackage = Package<Dynamic>;
 
@@ -12,7 +10,7 @@ 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:WorldPos);
+	GPSResponse(x:Float, y:Float, z:Float);
 	GPSRequest();
 }
 

src/lib/BlockPos.hx

@@ -7,46 +7,30 @@ import lib.Vec.Vec3;
 	Basicly a wrapper for Vec3<Int> with some extra functions.
 	`Y` represents the height of the point.
 **/
-@:forward(x, y, z)
+@:forward
 abstract BlockPos(Vec3<Int>) from Vec3<Int> to Vec3<Int> {
 	public inline function new(x:Int, y:Int, z:Int) {
 		this = new Vec3(x, y, z);
 	}
 
 	@:op(A + B)
-	public function add(rhs:BlockPos):BlockPos {
-		return {
-			y: this.y + rhs.y,
-			x: this.x + rhs.x,
-			z: this.z + rhs.z
-		};
+	public inline function add(rhs:BlockPos):BlockPos {
+		return this.add(rhs);
 	}
 
 	@:op(A - B)
-	public function sub(rhs:BlockPos):BlockPos {
-		return {
-			y: this.y - rhs.y,
-			x: this.x - rhs.x,
-			z: this.z - rhs.z
-		};
+	public inline function sub(rhs:BlockPos):BlockPos {
+		return this.sub(rhs);
 	}
 
 	@:op(A * B)
-	public function multiplyScalar(rhs:Int):BlockPos {
-		return {
-			y: this.y * rhs,
-			x: this.x * rhs,
-			z: this.z * rhs
-		};
+	public inline function multiplyScalar(rhs:Int):BlockPos {
+		return this.multiplyScalar(rhs);
 	}
 
 	@:op(-A)
-	public function negate():BlockPos {
-		return {
-			y: -this.y,
-			x: -this.x,
-			z: -this.z
-		};
+	public inline function negate():BlockPos {
+		return this.negate();
 	}
 
 	@:op(A == B)
@@ -59,22 +43,6 @@ abstract BlockPos(Vec3<Int>) from Vec3<Int> to Vec3<Int> {
 		return !equals(rhs);
 	}
 
-	public function dot(rhs:BlockPos):Float {
-		return this.x * rhs.x + this.y * rhs.y + this.z * rhs.z;
-	}
-
-	public function cross(rhs:BlockPos):BlockPos {
-		return {
-			x: this.x * rhs.y - this.y * rhs.x,
-			y: this.y * rhs.z - this.z * rhs.y,
-			z: this.z * rhs.x - this.x * rhs.z,
-		};
-	}
-
-	public function length():Float {
-		return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
-	}
-
 	public function hashCode():Int {
 		return (this.x * 73856093) ^ (this.y * 19349663) ^ (this.z * 83492791);
 	}
@@ -83,10 +51,6 @@ abstract BlockPos(Vec3<Int>) from Vec3<Int> to Vec3<Int> {
 		return 'BlockPos(${this.x}, ${this.y}, ${this.z})';
 	}
 
-	public function distance(rhs:BlockPos):Float {
-		return Math.sqrt(Math.pow(this.x - rhs.x, 2) + Math.pow(this.y - rhs.y, 2) + Math.pow(this.z - rhs.z, 2));
-	}
-
 	/**
 		Returns a list of positions neighboring the block. No Diagonal.
 	**/

src/lib/ScreenPos.hx

@@ -13,34 +13,22 @@ abstract ScreenPos(Vec2<Int>) from Vec2<Int> to Vec2<Int> {
 	}
 
 	@:op(A + B)
-	public function add(rhs:Vec2<Int>):ScreenPos {
-		return new ScreenPos({
-			y: this.y + rhs.y,
-			x: this.x + rhs.x,
-		});
+	public inline function add(rhs:Vec2<Int>):ScreenPos {
+		return this.add(rhs);
 	}
 
 	@:op(A - B)
-	public function sub(rhs:Vec2<Int>):ScreenPos {
-		return new ScreenPos({
-			y: this.y - rhs.y,
-			x: this.x - rhs.x,
-		});
+	public inline function sub(rhs:Vec2<Int>):ScreenPos {
+		return this.sub(rhs);
 	}
 
 	@:op(A * B)
-	public function multiply(rhs:Vec2<Int>):ScreenPos {
-		return new ScreenPos({
-			y: this.y * rhs.y,
-			x: this.x * rhs.x,
-		});
+	public inline function multiply(rhs:Vec2<Int>):ScreenPos {
+		return this.multiply(rhs);
 	}
 
 	@:op(-A)
-	public function negate():ScreenPos {
-		return new ScreenPos({
-			y: -this.y,
-			x: -this.x,
-		});
+	public inline function negate():ScreenPos {
+		return this.negate();
 	}
 }

src/lib/SinglePromise.hx

@@ -0,0 +1,48 @@
+package lib;
+
+using tink.CoreApi;
+
+class SinglePromise<T> {
+	private var inProgress:Bool = false;
+	private var interalPromise:Promise<T> = null;
+	private var interalResolve:T->Void = null;
+	private var interalReject:(Error->Void) = null;
+	private final activate:Void->Void;
+
+	public function new(activate:Void->Void) {
+		this.activate = activate;
+	}
+
+	public function request():Promise<T> {
+		if (this.inProgress) {
+			return this.interalPromise;
+		}
+
+		this.inProgress = true;
+
+		this.interalPromise = new Promise((resolve, reject) -> {
+			this.interalResolve = resolve;
+			this.interalReject = reject;
+
+			this.activate();
+
+			return null;
+		});
+
+		return this.interalPromise;
+	}
+
+	public function resolve(val:T) {
+		if (this.inProgress) {
+			this.interalResolve(val);
+			this.inProgress = false;
+		}
+	}
+
+	public function reject(err:Error) {
+		if (this.inProgress) {
+			this.interalReject(err);
+			this.inProgress = false;
+		}
+	}
+}

src/lib/SingleTimeoutPromise.hx

@@ -0,0 +1,60 @@
+package lib;
+
+import kernel.Timer;
+
+using tink.CoreApi;
+
+class SingleTimeoutPromise<T> {
+	private var inProgress:Bool = false;
+	private var interalPromise:Promise<T> = null;
+	private var interalResolve:T->Void = null;
+	private var interalReject:(Error->Void) = null;
+	private var timer:Timer = null;
+
+	private final activate:Void->Void;
+	private final timeout:Int;
+
+	public function new(timeout:Int, activate:Void->Void) {
+		this.activate = activate;
+		this.timeout = timeout;
+	}
+
+	public function request():Promise<T> {
+		if (this.inProgress) {
+			return this.interalPromise;
+		}
+
+		this.inProgress = true;
+
+		this.interalPromise = new Promise((resolve, reject) -> {
+			this.interalResolve = resolve;
+			this.interalReject = reject;
+
+			this.activate();
+
+			this.timer = new Timer(this.timeout, () -> {
+				this.reject(new Error("Timeout"));
+			});
+
+			return null;
+		});
+
+		return this.interalPromise;
+	}
+
+	public function resolve(val:T) {
+		if (this.inProgress) {
+			this.interalResolve(val);
+			this.inProgress = false;
+			this.timer.cancle();
+		}
+	}
+
+	public function reject(err:Error) {
+		if (this.inProgress) {
+			this.interalReject(err);
+			this.inProgress = false;
+			this.timer.cancle();
+		}
+	}
+}

src/lib/Vec.hx

@@ -8,16 +8,96 @@ package lib;
 		this.x = x;
 		this.y = y;
 	}
+
+	public function add(rhs:Vec2<T>):Vec2<T> {
+		return {
+			y: this.y + rhs.y,
+			x: this.x + rhs.x,
+		};
+	}
+
+	public function sub(rhs:Vec2<T>):Vec2<T> {
+		return {
+			y: this.y - rhs.y,
+			x: this.x - rhs.x,
+		};
+	}
+
+	public function multiply(rhs:Vec2<T>):Vec2<T> {
+		return {
+			y: this.y * rhs.y,
+			x: this.x * rhs.x,
+		};
+	}
+
+	public function negate():Vec2<T> {
+		return {
+			y: -this.y,
+			x: -this.x,
+		};
+	}
 }
 
 @:structInit class Vec3<T:Float> {
-	public var x:T;
-	public var y:T;
-	public var z:T;
+	public final x:T;
+	public final y:T;
+	public final z:T;
 
 	public function new(x:T, y:T, z:T) {
 		this.x = x;
 		this.y = y;
 		this.z = z;
 	}
+
+	public function add(rhs:Vec3<T>):Vec3<T> {
+		return {
+			x: this.x + rhs.x,
+			y: this.y + rhs.y,
+			z: this.z + rhs.z
+		};
+	}
+
+	public function sub(rhs:Vec3<T>):Vec3<T> {
+		return {
+			x: this.x - rhs.x,
+			y: this.y - rhs.y,
+			z: this.z - rhs.z
+		};
+	}
+
+	public function multiplyScalar(rhs:T):Vec3<T> {
+		return {
+			x: this.x * rhs,
+			y: this.y * rhs,
+			z: this.z * rhs
+		};
+	}
+
+	public function negate():Vec3<T> {
+		return {
+			x: -this.x,
+			y: -this.y,
+			z: -this.z
+		};
+	}
+
+	public function dot(rhs:Vec3<T>):Float {
+		return this.x * rhs.x + this.y * rhs.y + this.z * rhs.z;
+	}
+
+	public function cross(rhs:Vec3<T>):Vec3<T> {
+		return {
+			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 length():Float {
+		return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
+	}
+
+	public function distance(rhs:Vec3<T>):Float {
+		return Math.sqrt(Math.pow(this.x - rhs.x, 2) + Math.pow(this.y - rhs.y, 2) + Math.pow(this.z - rhs.z, 2));
+	}
 }

src/lib/WorldPos.hx

@@ -7,78 +7,46 @@ import lib.Vec.Vec3;
 	Basicly a wrapper for Vec3<Float> with some extra functions.
 	`Y` represents the height of the point.
 **/
-@:forward(x, y, z)
+@:forward
 abstract WorldPos(Vec3<Float>) from Vec3<Float> to Vec3<Float> {
-	inline public function new(i:Vec3<Float>) {
-		this = i;
+	public inline function new(v:Vec3<Float>) {
+		this = v;
 	}
 
 	@:op(A + B)
-	public function add(rhs:Vec3<Float>):WorldPos {
-		return new WorldPos({
-			y: this.y + rhs.y,
-			x: this.x + rhs.x,
-			z: this.z + rhs.z
-		});
+	public inline function add(rhs:WorldPos):WorldPos {
+		return this.add(rhs);
 	}
 
 	@:op(A - B)
-	public function sub(rhs:Vec3<Float>):WorldPos {
-		return new WorldPos({
-			y: this.y - rhs.y,
-			x: this.x - rhs.x,
-			z: this.z - rhs.z
-		});
+	public inline function sub(rhs:Vec3<Float>):WorldPos {
+		return this.sub(rhs);
 	}
 
 	@:op(A * B)
-	public function multiplyScalar(rhs:Float):WorldPos {
-		return new WorldPos({
-			y: this.y * rhs,
-			x: this.x * rhs,
-			z: this.z * rhs
-		});
+	public inline function multiplyScalar(rhs:Float):WorldPos {
+		return this.multiplyScalar(rhs);
 	}
 
 	@:op(A / B)
 	public function divideScalar(rhs:Float):WorldPos {
-		return new WorldPos({
+		return {
 			y: this.y / rhs,
 			x: this.x / rhs,
 			z: this.z / rhs
-		});
+		};
 	}
 
 	@:op(-A)
 	public function negate():WorldPos {
-		return new WorldPos({
-			y: -this.y,
-			x: -this.x,
-			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>):WorldPos {
-		return new WorldPos({
-			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
-		});
+		return this.negate();
 	}
 
 	public function normalize():WorldPos {
-		var l = length();
+		var l = this.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:WorldPos):Bool {
 		return close(rhs);
@@ -104,8 +72,4 @@ abstract WorldPos(Vec3<Float>) from Vec3<Float> to Vec3<Float> {
 			z: Math.fround(this.z)
 		});
 	}
-
-	public function distance(rhs:WorldPos):Float {
-		return Math.sqrt(Math.pow(this.x - rhs.x, 2) + Math.pow(this.y - rhs.y, 2) + Math.pow(this.z - rhs.z, 2));
-	}
 }