#include <stdio.h>

#include "quadtree.h"

#ifdef DEBUG
#define debug 1
#else
#define debug 0
#endif

static void trap(void )
{
	if (debug)
		exit(1);
}

static void validate_subtree(const struct quadtree *node)
{
	int i;

	for (i = 0; i < 4; i++) {
		if (!node->child[i])
			continue;

		if (node->child[i]->parent != node) {
			printf("%s:%d Fatal! Tree inconsistency detected "
			       "at child %d in node %p\n",
			       __FUNCTION__, __LINE__, i, node);
			trap();
		}
		if (node->parent) {
			if (node->child[i] == node->parent) {
			printf("%s:%d Fatal! Tree loop detected "
			       "at child %d in node %p\n",
			       __FUNCTION__, __LINE__, i, node);
			trap();
			}
		}

		validate_subtree(node->child[i]);
	}
}

static void validate_tree(const struct quadtree *node)
{
	const struct quadtree *parent = quadtree_find_parent(node);

	if (debug)
		validate_subtree(parent);
}

/**
 * quadtree_add - add a node to a quadtree
 * @parent: parent node
 * @new: the new node to be added
 * @compare: pointer to a comparison function
 *
 * Add a node to a quadtree. The tree is kept in order, the new node
 * is placed in the end of appropriate branch. The compare function is
 * used to compare the new node against a branch in the tree. The
 * comparison function must have following return value depending on
 * the position of node a compared to the position of node b:
 *
 * 0: upper left
 * 1: upper right
 * 2: lower left
 * 3: lower right
 *
 * The case of nodes sharing identical coordinates is not taken into
 * account at all.
 */

struct quadtree *quadtree_add(struct quadtree *parent,
			      struct quadtree *new,
			      int (*compare)(struct quadtree *a,
					     struct quadtree *b))
{
	int ret;
	if (parent == new)
		trap();

	validate_tree(parent);

	ret = compare(parent, new);

	if (ret < 0 || ret >= 4) {
		printf("Invalid comparison result of %d\n", ret);
		return 0;
	}

	if (parent->child[ret])
		return quadtree_add(parent->child[ret], new, compare);

	parent->child[ret] = new;
	new->parent = parent;

	validate_tree(new);

	return new;
}

/*
 * _quadtree_reposition_reqursively - Move everything under and
 * including a given node under the new root node
 */

static void
_quadtree_reposition_reqursively(struct quadtree *root,
				 struct quadtree *node,
				 int (*compare)(struct quadtree *a,
						struct quadtree *b))
{
	int i;

	validate_tree(node);

	/* First remove all children, if any */

	for (i = 0; i < 4; i++) {
		if (!node->child[i])
			continue;

		if (node->child[i] == node ||
		    node->child[i] == node->parent)
			trap();

		_quadtree_reposition_reqursively(root, node->child[i], compare);
		node->child[i] = 0;
	}

	/* Then remove this node under the new root. */
	quadtree_add(root, node, compare);
}

/*
 * quadtree_del - Detach a node from the tree
 *
 * Return value: The new root node of the tree. If we are detaching
 * anything but the root node of the entire tree, the returned root
 * value will be the original root of the tree.
 */
struct quadtree *quadtree_del(struct quadtree *node,
			      int (*compare)(struct quadtree *a,
					     struct quadtree *b))
{
	struct quadtree *parent = 0;
	int i;

	/*
	 * We are deleting the root node. This means we have to select
	 * a new root node and reconstruct the entire tree under it
	 * again.
	 */
	if (!node->parent) {
		for (i = 0; i < 4; i++) {
			if (!node->child[i])
				continue;

			if (!parent) {
				parent = node->child[i];
				continue;
			}
			_quadtree_reposition_reqursively(parent,
							 node->child[i],
							 compare);
			node->child[i] = 0;
		}

		return parent;
	}

	/*
	 * We are not deleting the parent. Detach the node from the
	 * parent abd relocate the children. The node will be then
	 * detached from the tree.
	 */

	for (i = 0; i < 4; i++) {
		if (node->parent->child[i] == node) {
			node->parent->child[i] = 0;
			break;
		}
	}
	if (i == 4) {
		printf("%s:%d Fatal! Tree inconsistency detected\n",
		       __FUNCTION__, __LINE__);
		trap();
	}

	/*
	 * The sub branch is now detached from the main tree. Continue
	 * relocating the detached branch.
	 */

	for (i = 0; i < 4; i++) {
		if (!node->child[i])
			continue;

		_quadtree_reposition_reqursively(node->parent, node->child[i],
						 compare);
		node->child[i] = 0;
	}

	parent = quadtree_find_parent(node);
	node->parent = 0;

	validate_tree(parent);
	return parent;
}


static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
{
	int direction, count = 0;

	direction = it->direction(head, it->ptr);

	if ((direction & QUADTREE_UPLEFT) && head->child[0])
		count += _walk_tree(head->child[0], it);

	if ((direction & QUADTREE_UPRIGHT) && head->child[1])
		count += _walk_tree(head->child[1], it);

	if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
		count += _walk_tree(head->child[2], it);

	if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
		count += _walk_tree(head->child[3], it);

	if ((direction & QUADTREE_SELF) && it->callback) {
		it->callback(head, it->ptr);
		count++;
	}

	return count;
}

int walk_tree(const struct quadtree_iterator *it)
{
	return _walk_tree(it->head, it);
}