diff --git a/src/sparsemap.c b/src/sparsemap.c
index 4ccd1c6..f8db713 100644
--- a/src/sparsemap.c
+++ b/src/sparsemap.c
@@ -109,7 +109,6 @@ enum __SM_CHUNK_INFO {
 };
 
 #define SM_CHUNK_GET_FLAGS(from, at) (((from)) & ((sm_bitvec_t)SM_FLAG_MASK << ((at)*2))) >> ((at)*2)
-#define SM_NEXT_CHUNK_ADDR(chunk)  (uint8_t *)((chunk) + sizeof(sm_idx_t) + __sm_chunk_map_get_size((__sm_chunk_t *)&(chunk)));
 
 typedef struct {
   sm_bitvec_t *m_data;
@@ -1216,9 +1215,7 @@ size_t
 sparsemap_get_size(sparsemap_t *map)
 {
   if (map->m_data_used) {
-    __sm_when_diag({
-      __sm_assert(map->m_data_used == __sm_get_size_impl(map));
-    });
+    __sm_when_diag({ __sm_assert(map->m_data_used == __sm_get_size_impl(map)); });
     return map->m_data_used;
   }
   return map->m_data_used = __sm_get_size_impl(map);
@@ -1248,7 +1245,8 @@ sparsemap_scan(sparsemap_t *map, void (*scanner)(sm_idx_t[], size_t), size_t ski
 }
 
 void
-__sm_chunk_map_merge(sparsemap_t *map, sparsemap_idx_t offset, __sm_chunk_t src) {
+__sm_chunk_map_merge(sparsemap_t *map, sparsemap_idx_t offset, __sm_chunk_t src)
+{
   size_t capacity = __sm_chunk_map_get_capacity(&src);
   for (sparsemap_idx_t j = 0; j < capacity; j++, offset++) {
     if (__sm_chunk_map_is_set(&src, j)) {
@@ -1261,41 +1259,47 @@ void
 sparsemap_merge(sparsemap_t *map, sparsemap_t *other)
 {
   uint8_t *src, *dst, *dst_tail;
-  size_t src_count = __sm_get_chunk_map_count(other),
-         dst_count = __sm_get_chunk_map_count(map),
-         max_chunk_count = src_count + dst_count;
+  size_t src_count = __sm_get_chunk_map_count(other), dst_count = __sm_get_chunk_map_count(map), max_chunk_count = src_count + dst_count;
 
   dst = __sm_get_chunk_map_data(map, 0);
   src = __sm_get_chunk_map_data(other, 0);
   for (size_t i = 0; i < max_chunk_count && src_count; i++) {
-    __sm_chunk_t dst_chunk;
     sm_idx_t dst_start = *(sm_idx_t *)dst;
-    __sm_chunk_map_init(&dst_chunk, dst + sizeof(sm_idx_t));
-
-    __sm_chunk_t src_chunk;
     sm_idx_t src_start = *(sm_idx_t *)src;
-    __sm_chunk_map_init(&src_chunk, src + sizeof(sm_idx_t));
-
-    if (dst_count > 0 && src_start == dst_start) {
+    if (src_start == dst_start && dst_count > 0) {
       /* Chunks overlap, merge them. */
+      __sm_chunk_t src_chunk;
+      __sm_chunk_map_init(&src_chunk, src + sizeof(sm_idx_t));
+      __sm_chunk_t dst_chunk;
+      __sm_chunk_map_init(&dst_chunk, dst + sizeof(sm_idx_t));
       __sm_chunk_map_merge(map, src_start, src_chunk);
-      src = SM_NEXT_CHUNK_ADDR(src);
-      dst = SM_NEXT_CHUNK_ADDR(dst);
+      src += sizeof(sm_idx_t) + __sm_chunk_map_get_size(&src_chunk);
+      dst += sizeof(sm_idx_t) + __sm_chunk_map_get_size(&dst_chunk);
       dst_count--;
       src_count--;
       continue;
-    } else {
-      /* Copy the chunk data directly into the buffer. */
-      // TODO capacity check? ENOMEM
-      dst_tail = __sm_get_chunk_map_end(map);
-      *(sm_idx_t *)dst_tail = src_start;
+    }
+    if (src_start < dst_start || dst_count == 0) {
+      __sm_chunk_t src_chunk;
+      __sm_chunk_map_init(&src_chunk, src + sizeof(sm_idx_t));
+      uint8_t buf[sizeof(sm_idx_t) + sizeof(sm_bitvec_t) * 2] = { 0 };
       size_t size = __sm_chunk_map_get_size(&src_chunk);
-      memcpy(dst_tail + sizeof(sm_idx_t), src + sizeof(sm_idx_t), size);
+      memcpy(buf, src + sizeof(sm_idx_t), size);
+      __sm_insert_data(map, src_start, &buf[0], sizeof(buf));
+
+      size_t aligned_idx = __sm_get_fully_aligned_offset(src_start);
+      if (src_start - aligned_idx < SM_CHUNK_MAX_CAPACITY) {
+        __sm_chunk_t dst_chunk;
+        __sm_chunk_map_init(&dst_chunk, dst + sizeof(sm_idx_t));
+        __sm_chunk_map_set_capacity(&dst_chunk, src_start - aligned_idx);
+      }
+      *(sm_idx_t *)src = src_start = aligned_idx;
 
       /* Update the chunk count and data_used. */
-      map->m_data_used = 0;
       __sm_set_chunk_map_count(map, __sm_get_chunk_map_count(map) + 1);
-      src = SM_NEXT_CHUNK_ADDR(src);
+
+      /* Carry on to the next chunk. */
+      src += sizeof(sm_idx_t) + __sm_chunk_map_get_size(&src_chunk);
       src_count--;
     }
   }
diff --git a/tests/test.c b/tests/test.c
index ab4e834..4eeba93 100644
--- a/tests/test.c
+++ b/tests/test.c
@@ -654,7 +654,7 @@ test_api_merge(const MunitParameter params[], void *data)
   sparsemap_set(other, 4097, true);
   sparsemap_set(other, 8193, true);
 
-  sparsemap_merge(map,other);
+  sparsemap_merge(map, other);
 
   assert_true(sparsemap_is_set(map, 1));
   assert_true(sparsemap_is_set(map, 2049));