package kernel.gps;

import kernel.log.Log;
import lib.KVStore;
import kernel.net.Net;
import kernel.net.INetworkInterface;
import kernel.net.Package;
import lib.WorldPos;

using tink.CoreApi;

/**
	Determines the position of the computer based on the distance to other computers.
	When receiving a message from another computer via wireless, the distance is also received.
	You need at least 3 computers that know their position to determine the position of the computer.
**/
class GPS {
	private static var shouldRespond = true;
	private static var shouldDoWholeNumberCheck = true;
	private static var posAccuracy = 0; // 0 = unkown, 1 = (ins,best guess), 2 = (stored/manual,should be right), 3 = (gps,confirmed)
	private static var cachedPosition:WorldPos;
	private static var lastPositionResponse:Array<{pos:WorldPos, dist:Float}> = [];

	private static var futureResolve:(pos:Null<WorldPos>) -> Void = null;

	@:allow(kernel.Init)
	private static function init() {
		loadCachedPosition();
	}

	public static function setManualPosition(pos:WorldPos) {
		var kvstore = new KVStore("gps");
		kvstore.set("mpos", pos);
		kvstore.save();

		if (cachedPosition == null) {
			cachedPosition = pos;
			posAccuracy = 2;
		}
	}

	@:allow(kernel.gps.INS)
	private static function setINSPosition(pos:WorldPos) {
		cachedPosition = pos;
		posAccuracy = 1;
	}

	public static function getPosition():Null<WorldPos> {
		return cachedPosition;
	}

	public static function getAccuracy():Int {
		return posAccuracy;
	}

	public static function invalidatePosition() {
		cachedPosition = null;
		posAccuracy = 0;
	}

	public static function locate():Future<Null<WorldPos>> {
		// TODO: implenet a timeout
		// TODO: dont send a request twice if the last one is still pending or we moved
		return new Future<Null<WorldPos>>((resolve) -> {
			futureResolve = resolve;
			sendPositionRequest();
			return null;
		});
	}

	private static function resolveFuture(pos:Null<WorldPos>) {
		futureResolve(pos);
	}

	private static function persistCachedPositon() {
		if (cachedPosition == null)
			return;
		var kvstore = new KVStore("gps");

		kvstore.set("cpos", cachedPosition);
		kvstore.save();
	}

	private static function loadCachedPosition() {
		var kvstore = new KVStore("gps");
		kvstore.load();

		var mPos:Null<WorldPos> = kvstore.get("mpos"); // Manual set position
		var cPos:Null<WorldPos> = kvstore.get("cpos"); // Cached position

		if (mPos != null && cPos != null && mPos == cPos) {
			// Both are the same, so we can use the cached position
			cachedPosition = mPos;
			posAccuracy = 3;
			return;
		}

		if (mPos != null && cPos != null && mPos != cPos) {
			// Both are different, so we can use the manual position
			cachedPosition = mPos;
			posAccuracy = 1;
			return;
		}

		if (mPos == null && cPos != null) {
			// No manual position set, so we can use the cached position
			cachedPosition = cPos;
			posAccuracy = 2;
			return;
		}

		if (mPos != null && cPos == null) {
			// No cached position, so we can use the manual position
			cachedPosition = mPos;
			posAccuracy = 2;
			return;
		}
	}

	private static function sendPositionRequest() {
		Net.brodcastGPSRequest();
	}

	@:allow(kernel.net.Net)
	private static function handlePackage(pack:Package<Noise>, dist:Float, iface:INetworkInterface) {
		switch (pack.type) {
			case GPSRequest:
				if (!shouldRespond)
					return;
				if (posAccuracy < 2)
					return;
				if (cachedPosition == null)
					return;

				var response = new Package(Net.networkID, pack.fromID, pack.msgID, GPSResponse(cachedPosition), null, 0);
				iface.send(pack.fromID, Net.networkID, response);
			case GPSResponse(pos):
				if (lastPositionResponse.contains({pos: pos, dist: dist}))
					return; // Ignore duplicate responses

				lastPositionResponse.push({pos: pos, dist: dist});

				// TODO: wait for a few seconds before calculating the position, so we can get more responses

				if (lastPositionResponse.length < 4)
					return; // We need at least 3 responses to calculate the position

				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:
		}
	}

	private static function calculatePosition():Null<WorldPos> {
		if (lastPositionResponse.length < 3)
			return null;

		// do a simple trilateration with the last 3 responses for now
		var p1 = lastPositionResponse[0].pos;
		var p2 = lastPositionResponse[1].pos;
		var p3 = lastPositionResponse[2].pos;

		var r1 = lastPositionResponse[0].dist;
		var r2 = lastPositionResponse[1].dist;
		var r3 = lastPositionResponse[2].dist;

		var result = trilateration(p1, p2, p3, r1, r2, r3);

		if (result.a.close(result.b))
			return result.a;

		// If we have more than 3 responses, we can use the 4th response to determine which of the two positions is correct
		// TODO: if this is a pocket computer we cant use this since it can move freely
		if (lastPositionResponse.length > 3) {
			var err1 = Math.abs(result.a.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);
			var err2 = Math.abs(result.b.distance(lastPositionResponse[3].pos) - lastPositionResponse[3].dist);

			if (Math.abs(err1 - err2) < 0.0001) {
				Log.warn("Failed to determine GPS position via 4th fix");
				return null; // The two positions are essentially the same, so we cant determine which one is correct
			}

			if (err1 < err2)
				return result.a;
			if (err2 < err1)
				return result.b;
		}

		// last resort, just use the position that is closest to a whole number (whithin a resonable margin of error)
		if (!shouldDoWholeNumberCheck)
			return null;

		// TODO: mark the position as not so accurate if we use this method

		var err1 = (result.a - result.a.round()).length();
		var err2 = (result.b - result.b.round()).length();

		if (err1 < err2 && err1 < 0.0001) {
			return result.a;
		} else if (err2 < 0.0001) {
			return result.b;
		}

		return null;
	}

	/**
		Determines the position(s) of a point given 3 other points and the distance to each of them.
	**/
	private static function trilateration(p1:WorldPos, p2:WorldPos, p3:WorldPos, r1:Float, r2:Float, r3:Float):Pair<WorldPos, WorldPos> {
		var a2b = p2 - p1;
		var a2c = p3 - p1;

		var d = a2b.length();
		var ex = a2b.normalize();
		var i = ex.dot(a2c);
		var ey = (a2c - ex * i).normalize();
		var j = ey.dot(a2c);
		var ez = ex.cross(ey);

		var x = (r1 * r1 - r2 * r2 + d * d) / (2 * d);
		var y = (r1 * r1 - r3 * r3 - x * x + (x - i) * (x - i) + j * j) / (2 * j);

		var result = p1 + ex * x + ey * y;

		var zSquared = r1 * r1 - x * x - y * y;
		if (zSquared > 0) {
			var z = Math.sqrt(zSquared);
			return new Pair(result + (ez * z), result - (ez * z));
		}

		return new Pair(result, result);
	}
}