package lib;

import kernel.peripherals.GeoScanner.ScanBlock;
import lib.BlockMap;

@:forward
abstract BlockBlockMap(BlockMap<Block>) {
	public inline function new() {
		this = new BlockMap<Block>();
	}

	public static function fromScan(scan:Array<ScanBlock>):BlockBlockMap {
		var map:BlockBlockMap = new BlockBlockMap();

		for (block in scan) {
			map.set(block.pos, new Block(block.name, block.tags));
		}

		return map;
	}

	/**
		Navigate from one point into another.
		Returns the path to take. 
		Based on A*
	**/
	public function nav(from:BlockPos, to:BlockPos):Array<BlockPos> {
		if (!canPass(to)) {
			trace("End is blocked");
			return [];
		}

		var openSet = new PriorityQueue();
		openSet.insert(from, 0);

		var cameFrom:BlockMap<BlockPos> = new BlockMap();

		var gCost:BlockMap<Int> = new BlockMap(); // G cost is the distance from start
		gCost.set(from, 0);

		var fCost:BlockMap<Float> = new BlockMap(); // F cost is the combines cost of moving to this pos (G cost) + the distnace to the end
		fCost.set(from, from.distance(to));

		while (!openSet.isEmpty()) {
			var current = openSet.extractMin(); // Check the pos with the lowest F cost

			if (current.equals(to)) {
				var totalPath = [];
				var currentPathPos = to;

				while (cameFrom.exists(currentPathPos)) {
					totalPath.unshift(currentPathPos);
					currentPathPos = cameFrom.get(currentPathPos);
				}
				totalPath.unshift(from);

				return totalPath;
			}

			for (neighbor in current.neighbors()) {
				if (!canPass(neighbor)) {
					continue;
				}

				var newGCost = gCost.get(current) + 1; // Movment cost to neighbor is always 1 as we can't move diagonally

				if (newGCost < (gCost.get(neighbor) ?? 9999)) {
					cameFrom.set(neighbor, current);
					gCost.set(neighbor, newGCost);
					fCost.set(neighbor, newGCost + neighbor.distance(to));

					if (!openSet.containsElement(neighbor)) {
						openSet.insert(neighbor, fCost.get(neighbor));
					}
				}
			}
		}

		return [];
	}

	/**
		Some block can be passed by the turtle.
		Some blocks get destroyed by it.
	**/
	public function canPass(k:BlockPos):Bool {
		var block = this.get(k);
		if (block == null) {
			return true;
		}

		switch block.name {
			case "minecraft:water":
				return true;
			case "minecaft:lava":
			case "mimecraft:grass":
			case "minecraft:hanging_roots":
			case "minecraft:warped_roots":
			case "minecraft:dead_bush":
			case "minecraft:fern":
			case "minecraft:vine":
			case "minecraft:glow_lichen":
			case "minecraft:seagrass":
			case "minecraft:crimson_roots":
			case "minecraft:nether_sprouts":
			case "minecraft:snow":
			case "minecraft:rose_bush":
			case "minecraft:fire":
				return true;
		}

		return false;
	}
}