From 1d60e1379276a0b16cd64cf5491bc7df391491e4 Mon Sep 17 00:00:00 2001
From: Niklas Kapelle <niklas@kapelle.org>
Date: Fri, 18 Oct 2024 16:48:38 +0200
Subject: [PATCH] idfk

---
 src/kernel/gps/INS.hx | 84 +++++++++++++++++++------------------------
 1 file changed, 36 insertions(+), 48 deletions(-)

diff --git a/src/kernel/gps/INS.hx b/src/kernel/gps/INS.hx
index c492a11..a647b71 100644
--- a/src/kernel/gps/INS.hx
+++ b/src/kernel/gps/INS.hx
@@ -1,7 +1,6 @@
 package kernel.gps;
 
 import lib.SinglePromise;
-import kernel.log.Log;
 import kernel.turtle.Turtle;
 import lib.WorldPos;
 
@@ -71,6 +70,9 @@ class INS {
 	}
 
 	private static function move(dir:Null<WorldPos>) {
+		if (alignPromise.isRunning()) {
+			return;
+		}
 		var pos = GPS.getPosition();
 		if (pos == null || dir == null)
 			return;
@@ -86,53 +88,45 @@ class 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");
-				reject(new Error("Not enough fuel to align"));
-				return null;
-			}
+	public static function startAlign():Void {
+		if (Turtle.getFuelLevel() < 2) {
+			alignPromise.reject(new Error("Not enough fuel to align"));
+			return;
+		}
 
-			GPS.locate().handle((result) -> {
-				switch result {
-					case Failure(err):
-						Log.warn("GPS not available for 1st position");
-						reject(new Error("GPS not available"));
+		GPS.locate().handle((result) -> {
+			switch result {
+				case Failure(err):
+					alignPromise.reject(new Error("Failed to locate 1st positon: " + err));
+					return;
+				case Success(pos1):
+					var moved = tryMoving();
+
+					if (moved == -1) {
+						alignPromise.reject(new Error("Can't move"));
 						return;
-					case Success(pos1):
-						var moved = tryMoving();
+					}
 
-						if (moved == -1) {
-							Log.warn("Can't move");
-							reject(new Error("Can't move"));
-							return;
-						}
-
-						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"));
+					GPS.locate().handle((result2) -> {
+						switch result2 {
+							case Failure(err):
+								alignPromise.reject(new Error("GPS not available for 2nd position: " + err));
+								return;
+							case Success(pos2):
+								var cHeading = calcHeading(pos1, pos2, moved);
+								if (cHeading == null) {
+									alignPromise.reject(new Error("Can't calculate heading"));
 									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;
-									}
+								}
 
-									heading = cHeading;
-									moveBack(moved);
-									GPS.setINSPosition(pos1);
+								heading = cHeading;
+								moveBack(moved);
+								GPS.setINSPosition(pos1);
 
-									resolve(Noise);
-							}
-						});
-				}
-			});
-			return null;
+								alignPromise.resolve(Noise);
+						}
+					});
+			}
 		});
 	}
 
@@ -172,20 +166,14 @@ class INS {
 	private static function moveBack(moved:Int) {
 		if (moved == 0) {
 			Turtle.forward();
-			// cc.Turtle.forward();
 		} else if (moved == 1) {
 			Turtle.back();
-			// cc.Turtle.back();
 		} else if (moved == 2) {
 			Turtle.back();
-			// cc.Turtle.back();
 			Turtle.turnRight();
-			// cc.Turtle.turnRight();
 		} else if (moved == 3) {
 			Turtle.forward();
-			// cc.Turtle.forward();
 			Turtle.turnRight();
-			// cc.Turtle.turnRight();
 		}
 	}