package bin;

import lib.CLIAppBase;
import kernel.gps.INS;
import lib.Pos3;
import lib.Vec.Vec3;

using tink.CoreApi;

class GPS extends CLIAppBase {
	public function new() {
		registerSyncSubcommand("set", (args) -> {
			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.setManualPosition(pos);

			return true;
		}, "<x> <y> <z>");

		registerSyncSubcommand("status", (args) -> {
			var pos = kernel.gps.GPS.getPosition();
			if (pos != null) {
				handle.writeLine('Position x:${pos.x} y:${pos.y} z:${pos.z}');
			} else {
				handle.writeLine("Position not available");
				return true;
			}

			var acc = kernel.gps.GPS.getAccuracy();
			if (acc == 1) {
				handle.writeLine("Accuracy: Low");
			} else if (acc == 2) {
				handle.writeLine("Accuracy: Medium");
			} else if (acc == 3) {
				handle.writeLine("Accuracy: High");
			}

			var ins = INS.getHeading();
			if (ins != null) {
				handle.writeLine('INS heading: ${ins.x} y:${ins.y} z:${ins.z}');
			} else {
				handle.writeLine("INS heading not available");
			}

			return true;
		});

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

		registerAsyncSubcommand("ins", (args) -> {
			return INS.align().map((_) -> {
				handle.writeLine("INS aligned");
				return true;
			});
		});
	}
}