diff --git a/src/lib/Block.hx b/src/lib/Block.hx
new file mode 100644
index 0000000..440de99
--- /dev/null
+++ b/src/lib/Block.hx
@@ -0,0 +1,12 @@
+package lib;
+
+@:structInit
+class Block {
+	public final name:PaN;
+	public final tags:Tags;
+
+	public function new(name:String, tags:Tags) {
+		this.name = name;
+		this.tags = tags;
+	}
+}
diff --git a/src/lib/BlockBlockMap.hx b/src/lib/BlockBlockMap.hx
new file mode 100644
index 0000000..3f04b3a
--- /dev/null
+++ b/src/lib/BlockBlockMap.hx
@@ -0,0 +1,114 @@
+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;
+	}
+}
diff --git a/src/lib/BlockMap.hx b/src/lib/BlockMap.hx
new file mode 100644
index 0000000..429e98a
--- /dev/null
+++ b/src/lib/BlockMap.hx
@@ -0,0 +1,48 @@
+package lib;
+
+import haxe.ds.IntMap;
+
+/**
+	Map values to positions in 3D space. For representing blocks from a scan see `BlockBlockMap`.
+
+	The only reason this exsists is that the abstract class for BlockPos does not
+	satisfy the constraint of HashMap. The hashCode function needs to be on a real class and not on 
+	an abstract class. Thanks Obama.
+**/
+@:forward(iterator, clear)
+abstract BlockMap<T>(IntMap<T>) {
+	/**
+		Creates a new HashMap.
+	**/
+	public inline function new() {
+		this = new IntMap();
+	}
+
+	/**
+		See `Map.set`
+	**/
+	@:arrayAccess public inline function set(k:BlockPos, v:T) {
+		this.set(k.hashCode(), v);
+	}
+
+	/**
+		See `Map.get`
+	**/
+	@:arrayAccess public inline function get(k:BlockPos) {
+		return this.get(k.hashCode());
+	}
+
+	/**
+		See `Map.exists`
+	**/
+	public inline function exists(k:BlockPos) {
+		return this.exists(k.hashCode());
+	}
+
+	/**
+		See `Map.remove`
+	**/
+	public inline function remove(k:BlockPos) {
+		return this.remove(k.hashCode());
+	}
+}