#include <socks/btree.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>

#define MAX(a, b) ((a) > (b) ? (a) : (b))
#define MIN(a, b) ((a) < (b) ? (a) : (b))

#define IS_LEFT_CHILD(p, c) ((p) && (c) && ((p)->b_left == (c)))
#define IS_RIGHT_CHILD(p, c) ((p) && (c) && ((p)->b_right == (c)))

#define HAS_LEFT_CHILD(x) ((x) && ((x)->b_left))
#define HAS_RIGHT_CHILD(x) ((x) && ((x)->b_right))

#define HAS_NO_CHILDREN(x) ((x) && (!(x)->b_left) && (!(x)->b_right))
#define HAS_ONE_CHILD(x) ((HAS_LEFT_CHILD(x) && !HAS_RIGHT_CHILD(x)) || (!HAS_LEFT_CHILD(x) && HAS_RIGHT_CHILD(x)))
#define HAS_TWO_CHILDREN(x) (HAS_LEFT_CHILD(x) && HAS_RIGHT_CHILD(x))

static void rotate_left(btree_t *tree, btree_node_t *x)
{
	btree_node_t *y = x->b_right;
	btree_node_t *p = x->b_parent;

	if (y->b_left) {
		y->b_left->b_parent = x;
	}

	x->b_right = y->b_left;

	if (!p) {
		tree->b_root = y;
	} else if (x == p->b_left) {
		p->b_left = y;
	} else {
		p->b_right = y;
	}

	x->b_parent = y;
	y->b_left = x;
	y->b_parent = p;

	x->b_bfactor = x->b_bfactor - 1 - MAX(y->b_bfactor, 0);

	p = y;

	while (p) {
	    p->b_bfactor = p->b_bfactor - 1 + MIN(y->b_bfactor, 0);
	    x = p;
	    p = p->b_parent;
	}
}

static void rotate_right(btree_t *tree, btree_node_t *y)
{
	btree_node_t *x = y->b_left;
	btree_node_t *p = y->b_parent;

	if (x->b_right) {
		x->b_right->b_parent = y;
	}

	y->b_left = x->b_right;

	if (!p) {
		tree->b_root = x;
	} else if (x == p->b_left) {
		p->b_left = x;
	} else {
		p->b_right = x;
	}

	y->b_parent = x;
	x->b_right = y;
	x->b_parent = p;

	y->b_bfactor = y->b_bfactor + 1 - MIN(x->b_bfactor, 0);

	p = x;
	while (p) {
	    p->b_bfactor = x->b_bfactor + 1 + MAX(y->b_bfactor, 0);
	    y = p;
	}
}

static void rotate_double_left(btree_t *tree, btree_node_t *z)
{
	btree_node_t *x = z->b_right;
	btree_node_t *y = x->b_left;

	rotate_right(tree, x);
	rotate_left(tree, z);
}

static void rotate_double_right(btree_t *tree, btree_node_t *z)
{
	btree_node_t *x = z->b_left;
	btree_node_t *y = x->b_right;

	rotate_left(tree, x);
	rotate_right(tree, z);
}

static void insert_fixup(btree_t *tree, btree_node_t *w)
{
	btree_node_t *z = NULL, *y = NULL, *x = NULL;

	z = w;
	while (z) {
		if (z->b_bfactor >= -1 && z->b_bfactor <= 1) {
			goto next_ancestor;
		}


		if (IS_LEFT_CHILD(z, y)) {
			if (IS_LEFT_CHILD(y, x)) {
			    rotate_right(tree, z);
			} else {
			    rotate_double_right(tree, z);
			}
		} else {
			if (IS_LEFT_CHILD(y, x)) {
			    rotate_double_left(tree, z);
			} else {
			    rotate_left(tree, z);
			}
		}


next_ancestor:
		x = y;
		y = z;
		z = z->b_parent;
	}
}

void btree_insert(btree_t *tree, btree_node_t *node)
{
	if (!tree->b_root) {
		tree->b_root = node;
		node->b_parent = NULL;
		return;
	}

	btree_node_t *cur = tree->b_root;
	while (1) {
		btree_node_t **nextp = NULL;

		int bfactor_diff = 0;
		if (node->b_key >= cur->b_key) {
			nextp = &cur->b_right;
			bfactor_diff = 1;
		} else {
			nextp = &cur->b_left;
			bfactor_diff = -1;
		}

		cur->b_bfactor = cur->b_bfactor + bfactor_diff;

		if (*nextp) {
			cur = *nextp;
		} else {
			node->b_parent = cur;
			*nextp = node;
			break;
		}
	}

	node->b_bfactor = 0;
	insert_fixup(tree, node);
}

static void remove_node_with_no_children(btree_t *tree, btree_node_t *node)
{
	printf("remove_node_with_no_children()\n");
	btree_node_t *p = node->b_parent;
	node->b_parent = NULL;

	if (!p) {
		tree->b_root = NULL;
	} else if (IS_LEFT_CHILD(p, node)) {
		p->b_left = NULL;
	} else {
		p->b_right = NULL;
	}
}

static void replace_node_with_one_subtree(btree_t *tree, btree_node_t *node)
{
	printf("remove_node_with_one_subtree()\n");
	btree_node_t *p = node->b_parent;
	btree_node_t *z = NULL;

	if (HAS_LEFT_CHILD(node)) {
		z = node->b_left;
	} else {
		z = node->b_right;
	}

	if (IS_LEFT_CHILD(p, node)) {
		p->b_left = z;
	} else if (IS_RIGHT_CHILD(p, node)) {
		p->b_right = z;
	}

	z->b_parent = p;

	node->b_parent = NULL;
	node->b_left = node->b_parent = NULL;
}

static void replace_node_with_two_subtrees(btree_t *tree, btree_node_t *node)
{
	printf("remove_node_with_two_subtrees()\n");
	btree_node_t *cur = node->b_left;

	while (cur->b_right) {
		cur = cur->b_right;
	}

	btree_node_t *p = cur->b_parent;

	if (IS_LEFT_CHILD(p, cur)) {
		p->b_left = NULL;
		p->b_bfactor++;
	} else {
		p->b_right = NULL;
		p->b_bfactor--;
	}

	p = node->b_parent;

	if (p) {
	    if (IS_LEFT_CHILD(p, node)) {
		    p->b_left = cur;
	    } else {
		    p->b_right = cur;
	    }
	}

	cur->b_left = node->b_left;
	cur->b_right = node->b_right;
	cur->b_bfactor = node->b_bfactor;
	cur->b_parent = p;

	if (!p) {
		tree->b_root = cur;
	}
}

void btree_delete(btree_t *tree, btree_node_t *node)
{
	btree_node_t *x = NULL;

	if (HAS_NO_CHILDREN(node)) {
		remove_node_with_no_children(tree, node);
		return;
	}

	if (HAS_ONE_CHILD(node)) {
		replace_node_with_one_subtree(tree, node);
		return;
	}

	if (HAS_TWO_CHILDREN(node)) {
		replace_node_with_two_subtrees(tree, node);
		return;
	}
}