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 e65fb56..b3e3d1e 100644
--- a/src/heap/vieter_heap_tree.c
+++ b/src/heap/vieter_heap_tree.c
@@ -4,24 +4,6 @@ vieter_heap_node *vieter_heap_node_init() {
   return calloc(1, sizeof(vieter_heap_node));
 }
 
-vieter_heap_node *vieter_heap_node_merge_same_order(vieter_heap_node *root_a,
-                                                    vieter_heap_node *root_b) {
-  vieter_heap_node *new_root, *new_child;
-
-  if (root_a->key <= root_b->key) {
-    new_root = root_a;
-    new_child = root_b;
-  } else {
-    new_root = root_b;
-    new_child = root_a;
-  }
-
-  new_root->next_largest_order = new_root->largest_order;
-  new_root->largest_order = new_child;
-
-  return new_root;
-}
-
 vieter_heap_tree *vieter_heap_tree_init(vieter_heap_node *root,
                                         vieter_heap_tree *next,
                                         uint64_t order) {
@@ -34,5 +16,98 @@ vieter_heap_tree *vieter_heap_tree_init(vieter_heap_node *root,
   return tree;
 }
 
+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};
+
+  t1->order = t2->order;
+  t1->root = t2->root;
+  t1->next = t2->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,
+                                                    vieter_heap_tree *tree_b) {
+  vieter_heap_tree *new_tree;
+
+  if (tree_a->root->key <= tree_b->root->key) {
+    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;
+    tree_b->root->next_largest_order = tree_b->root->largest_order;
+    tree_b->root->largest_order = tree_a->root;
+
+    free(tree_a);
+  }
+
+  new_tree->order++;
+
+  return new_tree;
+}
+
 vieter_heap_tree *vieter_heap_tree_merge(vieter_heap_tree *tree_a,
-                                         vieter_heap_tree *tree_b) {}
+                                         vieter_heap_tree *tree_b) {
+  vieter_heap_tree *tree, *target;
+
+  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 {
+      previous_target = target;
+      target = target->next;
+    }
+  }
+
+  // Append final part of tree to target
+  target->next = tree;
+
+  return target;
+}
diff --git a/src/heap/vieter_heap_tree.h b/src/heap/vieter_heap_tree.h
index 18808e1..03b61a4 100644
--- a/src/heap/vieter_heap_tree.h
+++ b/src/heap/vieter_heap_tree.h
@@ -15,8 +15,6 @@ vieter_heap_node *vieter_heap_node_init();
 
 void vieter_heap_node_free(vieter_heap_node *node);
 
-vieter_heap_node *vieter_heap_tree_merge_same_order(vieter_heap_node *root_a, vieter_heap_node *root_b);
-
 typedef struct vieter_heap_tree {
     uint64_t order;
     vieter_heap_node *root;
@@ -27,4 +25,7 @@ vieter_heap_tree *vieter_heap_tree_init(vieter_heap_node *root, vieter_heap_tree
 
 vieter_heap_tree *vieter_heap_tree_merge(vieter_heap_tree *tree_a, vieter_heap_tree *tree_b);
 
+vieter_heap_tree *vieter_heap_tree_merge_same_order(vieter_heap_tree *tree_a,
+                                                    vieter_heap_tree *tree_b);
+
 #endif
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}
 };