implemented INS

src/kernel/Init.hx

@@ -1,5 +1,6 @@
 package kernel;
 
+import kernel.gps.INS;
 import kernel.fs.FS;
 import kernel.gps.GPS;
 import kernel.log.Log;
@@ -34,6 +35,7 @@ class Init {
 		Net.instance = new Net();
 
 		GPS.instance = new GPS();
+		INS.instance = new INS();
 
 		// Register default terminate handler
 		KernelEvents.instance.onTerminate.handle(_->{

src/kernel/gps/GPS.hx

@@ -23,6 +23,8 @@ class GPS {
 	private var cachedPosition:Pos3;
 	private var lastPositionResponse: Array<{pos:Pos3,dist:Float}> = [];
 
+	private var futureResolve: (pos:Null<Pos3>) -> Void = null;
+
 	@:allow(kernel.Init)
 	private function new() {
 		this.loadCachedPosition();
@@ -39,6 +41,12 @@ class GPS {
 		}
 	}
 
+	@:allow(kernel.gps.INS)
+	private function setINSPosition(pos:Pos3) {
+		cachedPosition = pos;
+		posAccuracy = 1;
+	}
+
 	public function getPosition():Null<Pos3> {
 		return cachedPosition;
 	}
@@ -52,8 +60,18 @@ class GPS {
 		posAccuracy = 0;
 	}
 
-	public function locate() {
+	public function locate():Future<Null<Pos3>> {
+		// TODO: implenet a timeout
+		// TODO: dont send a request twice if the last one is still pending or we moved
+		return new Future<Null<Pos3>>((resolve)->{
+			this.futureResolve = resolve;
 			sendPositionRequest();
+			return null;
+		});
+	}
+
+	private function resolveFuture(pos:Null<Pos3>) {
+		this.futureResolve(pos);
 	}
 
 	private function persistCachedPositon() {
@@ -124,13 +142,19 @@ class GPS {
 
 				if (lastPositionResponse.length < 4) return; // We need at least 3 responses to calculate the position
 
-				var calculatedPosition = calculatePosition().round();
+				var calculatedPosition = calculatePosition();
+
+				if (calculatedPosition != null){
+					calculatedPosition = calculatedPosition.round();
+				}
 
 				lastPositionResponse = []; // Reset the response array
 
 				if (calculatedPosition == null) return;
 				cachedPosition = calculatedPosition;
 				posAccuracy = 3;
+
+				resolveFuture(calculatedPosition);
 			default:
 		}
 	}

src/kernel/gps/INS.hx

@@ -0,0 +1,211 @@
+package kernel.gps;
+
+import kernel.log.Log;
+import kernel.turtle.Turtle;
+import lib.Pos3;
+
+using tink.CoreApi;
+
+class INS {
+	public static var instance:INS;
+
+	private var heading: Null<Pos3> = null;
+	private var alingment: Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned
+
+	@:allow(kernel.Init)
+	private function new() {
+
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveForward() {
+		if (heading == null) {
+			this.alingment = 0;
+			return;
+		}
+		move(heading);
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveBackward() {
+		if (heading == null) {
+			this.alingment = 0;
+			return;
+		}
+		move(heading.negate());
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveUp() {
+		move({x: 0, y: 1, z: 0});
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function moveDown() {
+		move({x: 0, y: -1, z: 0});
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function turnLeft() {
+		if (heading == null) return;
+		if (heading.x == 0 && heading.z == -1) {
+			heading = {x: -1, y: 0, z: 0};
+		} else if (heading.x == -1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: 1};
+		} else if (heading.x == 0 && heading.z == 1) {
+			heading = {x: 1, y: 0, z: 0};
+		} else if (heading.x == 1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: -1};
+		}
+	}
+
+	@:allow(kernel.turtle.Turtle)
+	private function turnRight() {
+		if (heading == null) return;
+		if (heading.x == 0 && heading.z == -1) {
+			heading = {x: 1, y: 0, z: 0};
+		} else if (heading.x == -1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: -1};
+		} else if (heading.x == 0 && heading.z == 1) {
+			heading = {x: -1, y: 0, z: 0};
+		} else if (heading.x == 1 && heading.z == 0) {
+			heading = {x: 0, y: 0, z: 1};
+		}
+	}
+
+	private function move(dir: Null<Pos3>) {
+		Log.debug('INS move: $dir');
+		var pos = GPS.instance.getPosition();
+		var newPos = pos + dir;
+		GPS.instance.setINSPosition(newPos);
+	}
+
+	public function getHeading():Null<Pos3> {
+		return heading;
+	}
+
+	public function align(): Promise<Noise> {
+		Log.info("Aligning INS");
+		return new Promise<Noise>((resolve,reject)->{
+
+			if (Turtle.instance.getFuelLevel() < 2){
+				Log.warn("Not enough fuel to align");
+				reject(new Error("Not enough fuel to align"));
+				return null;
+			}
+
+			GPS.instance.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.instance.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"));
+						return;
+					}
+
+					var heading = calcHeading(pos1,pos2,moved);
+					if (heading == null) {
+						Log.error("Can't calculate heading");
+						reject(new Error("Can't calculate heading"));
+						return;
+					}
+
+					this.heading = heading;
+					moveBack(moved);
+					GPS.instance.setINSPosition(pos1);
+
+					resolve(Noise);
+				});
+
+			});
+			return null;
+		});
+	}
+
+	// -1 = not moved, 0 = back, 1 = forward, 2 = left, 3 = right
+	private function tryMoving():Int {
+		if (Turtle.instance.back().isSuccess()) {
+			return 0;
+		} else if (Turtle.instance.forward().isSuccess()) {
+			return 1;
+		} else {
+			Turtle.instance.turnLeft(); // TODO: Check if successfull
+			if (Turtle.instance.forward().isSuccess()){
+				return 2;
+			} else if (Turtle.instance.back().isSuccess()) {
+				return 3;
+			} else {
+				// Can't move
+				return -1;
+			}
+		}
+	}
+
+	private function calcHeading(pos1: Pos3,pos2:Pos3,moved:Int): Null<Pos3> {
+		if (moved == 0) {
+			return pos1 - pos2;
+		} else if (moved == 1) {
+			return pos2 - pos1;
+		} else if (moved == 2) {
+			return rotatePos3ToRight(pos2 - pos1);
+		} else if (moved == 3) {
+			return rotatePos3ToLeft(pos2 - pos1);
+		} else {
+			return null;
+		}
+	}
+
+	private function moveBack(moved:Int) {
+		if (moved == 0) {
+			Turtle.instance.forward();
+			// cc.Turtle.forward();
+		} else if (moved == 1) {
+			Turtle.instance.back();
+			// cc.Turtle.back();
+		} else if (moved == 2) {
+			Turtle.instance.back();
+			// cc.Turtle.back();
+			Turtle.instance.turnRight();
+			// cc.Turtle.turnRight();
+		} else if (moved == 3) {
+			Turtle.instance.forward();
+			// cc.Turtle.forward();
+			Turtle.instance.turnRight();
+			// cc.Turtle.turnRight();
+		}
+	}
+
+	private function rotatePos3ToRight(pos:Pos3):Pos3 {
+		if (pos.x == 0 && pos.z == -1) {
+			return {x: 1, y: 0, z: 0};
+		} else if (pos.x == -1 && pos.z == 0) {
+			return {x: 0, y: 0, z: -1};
+		} else if (pos.x == 0 && pos.z == 1) {
+			return {x: -1, y: 0, z: 0};
+		} else if (pos.x == 1 && pos.z == 0) {
+			return {x: 0, y: 0, z: 1};
+		} else {
+			return pos;
+		}
+	}
+
+	private function rotatePos3ToLeft(pos3:Pos3):Pos3 {
+		return rotatePos3ToRight(rotatePos3ToRight(rotatePos3ToRight(pos3)));
+	}
+
+}

src/kernel/turtle/Turtle.hx

@@ -2,6 +2,7 @@ package kernel.turtle;
 
 import kernel.log.Log;
 import kernel.turtle.Types;
+import kernel.gps.INS;
 
 using tink.CoreApi;
 
@@ -34,32 +35,44 @@ class Turtle {
 
 	public function forward():Outcome<Noise, String> {
 		var r = cc.Turtle.forward();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.moveForward();
+		return r2;
 	}
 
 	public function back():Outcome<Noise, String> {
 		var r = cc.Turtle.back();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.moveBackward();
+		return r2;
 	}
 
 	public function up():Outcome<Noise, String> {
 		var r = cc.Turtle.up();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.moveUp();
+		return r2;
 	}
 
 	public function down():Outcome<Noise, String> {
 		var r = cc.Turtle.down();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.moveDown();
+		return r2;
 	}
 
 	public function turnLeft():Outcome<Noise, String> {
 		var r = cc.Turtle.turnLeft();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.turnRight();
+		return r2;
 	}
 
 	public function turnRight():Outcome<Noise, String> {
 		var r = cc.Turtle.turnRight();
-		return conterToOutcome(r);
+		var r2 = conterToOutcome(r);
+		if (r2.isSuccess()) INS.instance.turnRight();
+		return r2;
 	}
 
 	public function dig(dir:InteractDirections, ?toolSide:ToolSide):Outcome<Noise, String> {