#include <stdio.h>
#include <stdlib.h>

#include "quadtree.h"

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

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

static int quadtree_compare_coord(const struct vector *a,
				const struct vector *b)
{
	int up, left;

	up = b->y < a->y;
	left = b->x < a->x;

	if (up && left)
		return 0;
	if (up && !left)
		return 1;
	if (left)
		return 2;
	return 3;
}

static int quadtree_compare(const struct quadtree *a, const struct quadtree *b)
{
	return quadtree_compare_coord(&a->pos, &b->pos);
}

static void validate_subtree(const struct quadtree *node)
{
	int i;
	long int children;
	children = 0;

	if (!node) {
		printf("Attempted to validate a null pointer\n");
		fflush(stdout);
		trap();
	}

	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 %p in node %p, incorrent parent %p\n",
			       __func__, __LINE__, i, node->child[i], node,
			       node->child[i]->parent);
			fflush(stdout);
			trap();
		}

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

		children += node->child[i]->children + 1;

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

	if (node->depth < 0) {
		printf("%s:%d Tree statistics inconsistency detected! "
		       "Negative depth: %ld\n",
		       __func__, __LINE__, node->depth);
	}

	if (node->children != children) {
		printf("%s:%d Tree statistics inconsistency detected! "
		       "child count mismatch. Expected %ld, got %ld\n",
		       __func__, __LINE__, children, node->children);
		fflush(stdout);
		trap();
	}

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

		if (node->depth == node->child[i]->depth + 1)
			break;

		if (node->child[i]->depth > node->depth) {
			printf("%s:%d Tree statistics inconsistency detected! "
			       "child depth mismatch %ld > %ld\n",
			       __func__, __LINE__, node->child[i]->depth,
			       node->depth);
		}
	}

	if (i == 4) {
		printf("%s:%d Tree statistics inconsistency detected! "
		       "child depth mismatch.",
		       __func__, __LINE__);
		fflush(stdout);
		trap();
	}
}

static void validate_tree(const struct quadtree *node)
{
	if (debug)
		validate_subtree(quadtree_find_parent(node));
}

/**
 * quadtree_add - add a node to a quadtree
 * @parent: parent node
 * @new: the new node to be added
 *
 * Add a node to a quadtree. The tree is kept in order, the new node
 * is placed in the end of appropriate branch.
 *
 * The case of nodes sharing identical coordinates is not taken into
 * account at all.
 */

struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
			      struct quadtree_ops *ops)
{
	int ret, i;
	if (parent == new)
		trap();

	validate_tree(parent);

	ret = quadtree_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, ops);

	parent->child[ret] = new;
	new->parent = parent;
	for (i = 0; i < 4; i++)
		new->child[i] = 0;

	if (debug) {
		printf("adding node %p to parent %p\n", new, parent);
		fflush(stdout);
	}

	quadtree_recalculate_parent_stats(new, ops);

	validate_tree(new);

	return new;
}

/*
 * _qt_optimal_del - Delete nodes from a detached subtree in optimal order
 *
 * All nodes are moved under the new parent node
 */
static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
		       struct quadtree_ops *ops, int parents)
{
	struct quadtree *child[4] = {0};
	int i, children = 0, max_c, max_i;

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

		child[children] = node->child[i];
		node->child[i] = NULL;
		children++;
	}

	/*
	 * Try to remove the nodes in such order that the node which
	 * has child number one quarter of the parent's child numer
	 * gets deleted first.
	 */

	if (node->children <= parents / 4) {
		if (debug) {
			printf("%d: Moving node %p away from parent %p\n",
			       __LINE__, node, parent);
			fflush(stdout);
		}
		validate_tree(parent);
		quadtree_add(parent, node, ops);
		node = NULL;
		parents /= 4;
	}

	while(children) {
		max_c = -1;
		max_i = -1;
		for (i = 0; i < 4; i++) {
			if (!child[i])
				continue;

			if (max_c < child[i]->children) {
				max_c = child[i]->children;
				max_i = i;
			}
		}

		_qt_optimal_del(parent, child[max_i], ops, parents / 4);
		child[max_i] = 0;
		children--;
	}

	if (node) {
		validate_tree(parent);
		if (debug) {
			printf("%d: Moving node %p away from parent %p\n",
			       __LINE__, node, parent);
			fflush(stdout);
		}
		quadtree_add(parent, node, ops);
	}
}

/*
 * 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,
			      struct quadtree_ops *ops)
{
	struct quadtree *parent = 0;
	int i;

	if (debug) {
		printf("Deleting node %p under parent %p\n",
			       node, node->parent);
		fflush(stdout);
	}
	/*
	 * 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];
				parent->parent = 0;
				continue;
			}
			_qt_optimal_del(parent, node->child[i], ops,
					node->children);
			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)
		trap();

	parent = node->parent;

	/*
	 * The sub branch is now detached from the main tree. Fix the
	 * stats.
	 */
	quadtree_recalculate_parent_stats(node->parent, ops);

	parent = quadtree_find_parent(parent);

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

			_qt_optimal_del(parent, node->child[i], ops,
					node->children);
		}
	}

	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, (struct quadtree_iterator *)it);

	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, (struct quadtree_iterator *)it);
		count++;
	}

	return count;
}

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