#include "vieter_heap_tree.h"

vieter_heap_node *vieter_heap_node_init() {
  return calloc(1, sizeof(vieter_heap_node));
}

void vieter_heap_node_free(vieter_heap_node *node) { free(node); }

void vieter_heap_tree_free(vieter_heap_node *root) {
  if (root->order == 0) {
    goto end;
  }

  uint64_t size = 1;
  vieter_heap_node **stack =
      malloc(((uint64_t)1 << root->order) * sizeof(vieter_heap_node *));
  stack[0] = root->largest_order;

  vieter_heap_node *node;

  while (size > 0) {
    node = stack[size - 1];
    size--;

    if (node->largest_order != NULL) {
      stack[size] = node->largest_order;
      size++;
    }

    if (node->ptr.next_largest_order != NULL) {
      stack[size] = node->ptr.next_largest_order;
      size++;
    }

    vieter_heap_node_free(node);
  }

  free(stack);

end:
  vieter_heap_node_free(root);
}

vieter_heap_node *vieter_heap_tree_merge_same_order(vieter_heap_node *root_a,
                                                    vieter_heap_node *root_b) {
  vieter_heap_node *new_root;

  if (root_a->key <= root_b->key) {
    new_root = root_a;
    root_b->ptr.next_largest_order = root_a->largest_order;
    root_a->largest_order = root_b;
  } else {
    new_root = root_b;
    root_a->ptr.next_largest_order = root_b->largest_order;
    root_b->largest_order = root_a;
  }

  new_root->order++;

  return new_root;
}

vieter_heap_node *vieter_heap_tree_merge(vieter_heap_node *root_a,
                                         vieter_heap_node *root_b) {
  vieter_heap_node *root, *target, *out;

  if (root_a->order <= root_b->order) {
    target = root_a;
    root = root_b;
  } else {
    target = root_b;
    root = root_a;
  }

  vieter_heap_node *next_tree, *next_target;
  vieter_heap_node *previous_target = NULL;

  while (target != NULL && root != NULL) {
    if (target->order == root->order) {
      next_tree = root->ptr.next_tree;
      next_target = target->ptr.next_tree;

      target = vieter_heap_tree_merge_same_order(target, root);

      target->ptr.next_tree = next_target;

      // If this merge produces a binomial tree whose size is already in
      // target, it will be the next target. Therefore, we can  merge target's
      // trees until we no longer have a duplicate depth.
      while (target->ptr.next_tree != NULL &&
             target->ptr.next_tree->order == target->order) {
        next_target = target->ptr.next_tree->ptr.next_tree;
        target =
            vieter_heap_tree_merge_same_order(target, target->ptr.next_tree);
        target->ptr.next_tree = next_target;
      }

      if (previous_target != NULL) {
        previous_target->ptr.next_tree = target;
      } else {
        out = target;
      }

      root = next_tree;
    } else if (target->order > root->order) {
      next_tree = root->ptr.next_tree;

      if (previous_target == NULL) {
        previous_target = root;
        out = root;
      } else {
        previous_target->ptr.next_tree = root;
      }

      root->ptr.next_tree = target;
      root = next_tree;
    } else {
      if (previous_target == NULL) {
        out = target;
      }

      previous_target = target;
      target = target->ptr.next_tree;
    }
  }

  // Append final part of tree to target
  if (target == NULL) {
    previous_target->ptr.next_tree = root;
  }

  return out;
}

vieter_heap_node *vieter_heap_tree_pop(void **out, vieter_heap_node *tree) {
  vieter_heap_node *tree_before_smallest = NULL;
  vieter_heap_node *previous_tree = NULL;
  vieter_heap_node *original_root = tree;

  uint64_t smallest_key = tree->key;

  while (tree->ptr.next_tree != NULL) {
    previous_tree = tree;
    tree = tree->ptr.next_tree;

    if (tree->key < smallest_key) {
      smallest_key = tree->key;
      tree_before_smallest = previous_tree;
    }
  }

  vieter_heap_node *tree_to_pop;

  if (tree_before_smallest != NULL) {
    tree_to_pop = tree_before_smallest->ptr.next_tree;
    tree_before_smallest->ptr.next_tree = tree_to_pop->ptr.next_tree;
  } else {
    tree_to_pop = original_root;
    original_root = original_root->ptr.next_tree;
  }

  *out = tree_to_pop->data;

  if (tree_to_pop->order == 0) {
    vieter_heap_tree_free(tree_to_pop);

    return original_root;
  }

  // Each child has a pointer to its sibling with the next largest order. If we
  // want to convert this list of children into their own tree, these pointers
  // have to be reversed.
  previous_tree = tree_to_pop->largest_order;
  vieter_heap_node_free(tree_to_pop);

  tree = previous_tree->ptr.next_largest_order;
  previous_tree->ptr.next_tree = NULL;

  vieter_heap_node *next_tree;

  while (tree != NULL) {
    next_tree = tree->ptr.next_largest_order;
    tree->ptr.next_tree = previous_tree;

    previous_tree = tree;
    tree = next_tree;
  }

  // original_root is zero if the heap only contained a single tree.
  if (original_root != NULL) {
    return vieter_heap_tree_merge(original_root, previous_tree);
  } else {
    return previous_tree;
  }
}