diff --git a/Makefile b/Makefile
index 4a26c17..5795f7c 100644
--- a/Makefile
+++ b/Makefile
@@ -25,7 +25,7 @@ INC_FLAGS := $(addprefix -I,$(INC_DIRS))
 #  object file is also recompiled if only a header is changed.
 # -MP: generate a dummy target for every header file (according to the  docs it
 #  prevents some errors when removing header files)
-CFLAGS ?= -MMD -MP
+CFLAGS ?= -MMD -MP -g
 VIETERCFLAGS := $(INC_FLAGS) $(CFLAGS) -Wall -Wextra
 
 .PHONY: all
diff --git a/src/heap/vieter_heap.c b/src/heap/vieter_heap.c
index 6d543df..ceecdbb 100644
--- a/src/heap/vieter_heap.c
+++ b/src/heap/vieter_heap.c
@@ -9,7 +9,18 @@ struct vieter_heap {
 
 vieter_heap *vieter_heap_init() { return calloc(1, sizeof(vieter_heap)); }
 
-uint64_t vieter_heap_size(vieter_heap *heap) { return 0; }
+uint64_t vieter_heap_size(vieter_heap *heap) {
+  uint64_t size = 0;
+  vieter_heap_tree *tree = heap->tree;
+
+  while (tree != NULL) {
+    size |= 1 << tree->order;
+
+    tree = tree->next;
+  }
+
+  return size;
+}
 
 void vieter_heap_free(vieter_heap *heap) {}
 
@@ -34,5 +45,22 @@ vieter_heap_error vieter_heap_pop(void **out, vieter_heap *heap) {
 }
 
 vieter_heap_error vieter_heap_peek(void **out, vieter_heap *heap) {
+  if (heap->tree == NULL) {
+    return vieter_heap_empty;
+  }
+
+  uint64_t smallest_key;
+
+  vieter_heap_tree *tree = heap->tree;
+
+  while (tree != NULL) {
+    if (tree->root->key < smallest_key) {
+      smallest_key = tree->root->key;
+      *out = tree->root->data;
+    }
+
+    tree = tree->next;
+  }
+
   return vieter_heap_ok;
 }
diff --git a/src/heap/vieter_heap_tree.c b/src/heap/vieter_heap_tree.c
index 75b38d2..b3e3d1e 100644
--- a/src/heap/vieter_heap_tree.c
+++ b/src/heap/vieter_heap_tree.c
@@ -17,19 +17,16 @@ vieter_heap_tree *vieter_heap_tree_init(vieter_heap_node *root,
 }
 
 void vieter_heap_tree_swap(vieter_heap_tree *t1, vieter_heap_tree *t2) {
-    vieter_heap_tree temp = {
-        .order = t1->order,
-        .root = t1->root,
-        .next = t1->next
-    };
+  vieter_heap_tree temp = {
+      .order = t1->order, .root = t1->root, .next = t1->next};
 
-    t1->order = t2->order;
-    t1->root = t2->root;
-    t1->next = t2->next;
+  t1->order = t2->order;
+  t1->root = t2->root;
+  t1->next = t2->next;
 
-    t2->order = temp.order;
-    t2->root = temp.root;
-    t2->next = temp.next;
+  t2->order = temp.order;
+  t2->root = temp.root;
+  t2->next = temp.next;
 }
 
 vieter_heap_tree *vieter_heap_tree_merge_same_order(vieter_heap_tree *tree_a,
@@ -37,13 +34,13 @@ vieter_heap_tree *vieter_heap_tree_merge_same_order(vieter_heap_tree *tree_a,
   vieter_heap_tree *new_tree;
 
   if (tree_a->root->key <= tree_b->root->key) {
-      new_tree = tree_a;
+    new_tree = tree_a;
     tree_a->root->next_largest_order = tree_a->root->largest_order;
     tree_a->root->largest_order = tree_b->root;
 
     free(tree_b);
   } else {
-      new_tree = tree_b;
+    new_tree = tree_b;
     tree_b->root->next_largest_order = tree_b->root->largest_order;
     tree_b->root->largest_order = tree_a->root;
 
@@ -55,54 +52,62 @@ vieter_heap_tree *vieter_heap_tree_merge_same_order(vieter_heap_tree *tree_a,
   return new_tree;
 }
 
-vieter_heap_tree *vieter_heap_tree_merge(vieter_heap_tree *tree_a, vieter_heap_tree *tree_b) {
-    vieter_heap_tree *tree, *target;
+vieter_heap_tree *vieter_heap_tree_merge(vieter_heap_tree *tree_a,
+                                         vieter_heap_tree *tree_b) {
+  vieter_heap_tree *tree, *target;
 
-    if (tree_a->order <= tree_b->order) {
-        target = tree_a;
-        tree = tree_b;
+  if (tree_a->order <= tree_b->order) {
+    target = tree_a;
+    tree = tree_b;
+  } else {
+    target = tree_b;
+    tree = tree_a;
+  }
+
+  vieter_heap_tree *next_tree, *next_target;
+  vieter_heap_tree *previous_target = NULL;
+
+  while (target != NULL && target != NULL) {
+    if (target->order == tree->order) {
+      next_tree = tree->next;
+      next_target = target->next;
+
+      target = vieter_heap_tree_merge_same_order(target, tree);
+
+      target->next = next_target;
+
+      if (previous_target != NULL) {
+        previous_target->next = target;
+      }
+
+      tree = next_tree;
+
+      // If this merge produces a binomial tree whose size is already in
+      // target, it will be the next target. Therefore, we can target's
+      // trees until we no longer have a duplicate depth.
+      while (target->next != NULL && target->next->order == target->order) {
+        next_target = target->next;
+        target = vieter_heap_tree_merge_same_order(target, target->next);
+        target->next = next_target;
+      }
+    } else if (target->order > tree->order) {
+      if (previous_target == NULL) {
+        previous_target = tree;
+      } else {
+        previous_target->next = tree;
+      }
+
+      tree->next = target;
+
+      tree = tree->next;
     } else {
-        target = tree_b;
-        tree = tree_a;
+      previous_target = target;
+      target = target->next;
     }
+  }
 
-    vieter_heap_tree *next_tree, *next_target;
-    vieter_heap_tree *previous_target = target;
+  // Append final part of tree to target
+  target->next = tree;
 
-    while (1) {
-        if (target->order == tree->order) {
-            next_tree = tree->next;
-            next_target = target->next;
-
-            target = vieter_heap_tree_merge_same_order(target, tree);
-
-            // TODO what when this merge produces an order that's already in target
-
-            target->next = next_target;
-            previous_target->next = target;
-
-            tree = next_tree;
-        } else if (target->order > tree->order) {
-            previous_target->next = tree;
-            tree->next = target;
-
-            tree = tree->next;
-        } else {
-            previous_target = target;
-            target = target->next;
-        }
-    }
-
-    if (target->order > tree->order) {
-        vieter_heap_tree_swap(target, tree);
-        
-    } else if (target->order == tree->order) {
-        target->root->next_largest_order = target->root->largest_order;
-        target->root->largest_order = tree->root;
-        target->order++;
-
-        next_tree = tree->next;
-        free(tree);
-        tree = next_tree;
-    }
+  return target;
 }
diff --git a/test/heap/test_heap.c b/test/heap/test_heap.c
index 832404d..7367f28 100644
--- a/test/heap/test_heap.c
+++ b/test/heap/test_heap.c
@@ -13,7 +13,41 @@ void test_init() {
     vieter_heap_free(heap);
 }
 
+void test_merge_same_order() {
+    vieter_heap_node *root_a = vieter_heap_node_init();
+    root_a->key = 1;
+    vieter_heap_tree *tree_a = vieter_heap_tree_init(root_a, NULL, 1);
+
+    vieter_heap_node *root_b = vieter_heap_node_init();
+    root_b->key = 2;
+    vieter_heap_tree *tree_b = vieter_heap_tree_init(root_b, NULL, 1);
+
+    vieter_heap_tree *merged = vieter_heap_tree_merge_same_order(tree_a, tree_b);
+
+    TEST_CHECK(merged == tree_a);
+    TEST_CHECK(merged->root->key == 1);
+    TEST_CHECK(merged->root->largest_order == root_b);
+    TEST_CHECK(merged->root->next_largest_order == NULL);
+}
+
+void test_insert() {
+    vieter_heap *heap = vieter_heap_init();
+    TEST_SIZE(heap, 0);
+
+    void *data;
+
+    for (uint64_t i = 50; i <= 0; i--) {
+        vieter_heap_insert(heap, i, (void *)i);
+        TEST_SIZE(heap, (uint64_t)51 - i);
+
+        TEST_CHECK(vieter_heap_peek(&data, heap) == vieter_heap_ok);
+        TEST_CHECK(data == (void*)i);
+    }
+}
+
 TEST_LIST = {
     {"test_init", test_init},
+    {"test_merge_same_order", test_merge_same_order},
+    {"test_insert", test_insert},
     {NULL, NULL}
 };