cc-haxe/src/kernel/gps/INS.hx

package kernel.gps;

import kernel.log.Log;
import kernel.turtle.Turtle;
import lib.Pos3;

using tink.CoreApi;

class INS {
	private static var heading:Null<Pos3> = null;
	private static var alingment:Int = 1; // 0 = degraded, 1 = not aligned, 2 = aligned

	@:allow(kernel.turtle.Turtle)
	private static function moveForward() {
		if (heading == null) {
			alingment = 0;
			return;
		}
		move(heading);
	}

	@:allow(kernel.turtle.Turtle)
	private static function moveBackward() {
		if (heading == null) {
			alingment = 0;
			return;
		}
		move(heading.negate());
	}

	@:allow(kernel.turtle.Turtle)
	private static function moveUp() {
		move({x: 0, y: 1, z: 0});
	}

	@:allow(kernel.turtle.Turtle)
	private static function moveDown() {
		move({x: 0, y: -1, z: 0});
	}

	@:allow(kernel.turtle.Turtle)
	private static 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 static 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 static function move(dir:Null<Pos3>) {
		Log.debug('INS move: $dir');
		var pos = GPS.getPosition();
		var newPos = pos + dir;
		GPS.setINSPosition(newPos);
	}

	public static function getHeading():Null<Pos3> {
		return heading;
	}

	public static 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.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"));
						return;
					}

					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);

					resolve(Noise);
				});
			});
			return null;
		});
	}

	// -1 = not moved, 0 = back, 1 = forward, 2 = left, 3 = right
	private static 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 static 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 static 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 static 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 static function rotatePos3ToLeft(pos3:Pos3):Pos3 {
		return rotatePos3ToRight(rotatePos3ToRight(rotatePos3ToRight(pos3)));
	}
}