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