diff --git a/LayoutLink.js b/LayoutLink.js
index ac0ae98..a2437c4 100644
--- a/LayoutLink.js
+++ b/LayoutLink.js
@@ -31,7 +31,6 @@ class LayoutLink {
 
   bfs() {
     // TODO: give more thought to birdirectional search
-    // TODO: give more thought to minheap instead of queue
     // TODO: make diagonals cost more
     // TODO: remove getDirect() once bidirectional + minheap are done
 
@@ -43,17 +42,15 @@ class LayoutLink {
     this.path = direct[1];
 
     // BFS work queue
-    let queue = [
-      {
-        path: [Array.from(this.from_.pos)],
-        cost: 0,
-      },
-    ];
+    let queue = new MinHeap((a) => a.cost);
+    queue.push({
+      path: [Array.from(this.from_.pos)],
+      cost: 0,
+    });
 
     let iter = 0;
-    while (queue.length) {
+    for (let next = queue.pop(); next; next = queue.pop()) {
       ++iter;
-      let next = queue.shift();
       let pos = next.path[next.path.length - 1];
 
       if (cheapestCostToGoal && next.cost >= cheapestCostToGoal) {
@@ -113,7 +110,6 @@ class LayoutLink {
           });
         }
       }
-      queue.sort((a, b) => (a.cost - b.cost));
     }
 
     for (let hop of this.path) {
diff --git a/MinHeap.js b/MinHeap.js
new file mode 100644
index 0000000..de7222e
--- /dev/null
+++ b/MinHeap.js
@@ -0,0 +1,67 @@
+class MinHeap {
+  constructor(valueFunc=(a) => a) {
+    this.valueFunc_ = valueFunc;
+    this.data_ = [];
+  }
+
+  push(...vals) {
+    for (let val of vals) {
+      this.data_.push(val);
+      this.bubbleUp_();
+    }
+  }
+
+  pop() {
+    let min = this.data_[0];
+    if (this.data_.length > 1) {
+      this.data_[0] = this.data_.pop();
+      this.bubbleDown_();
+    } else {
+      this.data_.length = 0;
+    }
+    return min;
+  }
+
+  bubbleUp_() {
+    for (let idx = this.data_.length - 1; idx > 0;) {
+      let parent = Math.floor((idx + 1) / 2) - 1;
+    
+      if (this.valueFunc_(this.data_[parent]) >
+          this.valueFunc_(this.data_[idx])) {
+        [this.data_[parent], this.data_[idx]] =
+            [this.data_[idx], this.data_[parent]];
+      }
+    
+      idx = parent;
+    }
+  }
+
+  bubbleDown_() {
+    for (let idx = 0;;) {
+      let children = [
+        (idx + 1) * 2,
+        (idx + 1) * 2 - 1,
+      ];
+      let toSwap = idx;
+    
+      // Find the minimum value of the current node and its two children
+      for (let child of children) {
+        if (this.data_[child] != undefined && 
+            this.valueFunc_(this.data_[child]) <
+            this.valueFunc_(this.data_[toSwap])) {
+          toSwap = child;
+        }
+      }
+    
+      if (toSwap == idx) {
+        // Current node is smaller than both children; tree is correct
+        break;
+      }
+    
+      [this.data_[toSwap], this.data_[idx]] =
+          [this.data_[idx], this.data_[toSwap]];
+    
+      idx = toSwap;
+    }
+  }
+};
diff --git a/utils.js b/utils.js
index d047682..22de960 100644
--- a/utils.js
+++ b/utils.js
@@ -25,4 +25,5 @@ function intersects(set1, set2) {
   return false;
 }
 
+<!--# include file="MinHeap.js" -->
 <!--# include file="StringMap.js" -->