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

#include "quadtree.h"

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

static void _trap(int line)
{
	if (debug) {
		printf("Trapped from line %d, use debugger to get backtrace\n",
			line);
		fflush(stdout);
		exit(1);
	}
}

#define trap() _trap(__LINE__)

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, dir;
	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();
			}
		}

		dir = quadtree_compare(node, node->child[i]);

		if (dir != i) {
			printf("%s:%d Fatal! Spatial inconsistency detected "
				"at child %d in node %p\n"
				"parent: (%f %f), child (%f %f), "
				"dir %d != %d\n",
				__func__, __LINE__, i, node,
				node->pos.x, node->pos.y,
				node->child[i]->pos.x, node->child[i]->pos.y,
				dir, i);
			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)
{
	int i;

	if (!debug)
		return;

	if (!node->parent)
		return validate_subtree(node);

	for (i = 0; i < 4; i++)
		if (node->parent->child[i] == node)
			break;

	if (i == 4) {
		printf("%s:%d Tree inconsistency detected! "
			"Wrong parent %p for node %p\n",
			__func__, __LINE__, node->parent, node);
		fflush(stdout);
		trap();
	}

	validate_tree(node->parent);
}

void _quadtree_validate_tree(const struct quadtree *node)
{
	validate_tree(node);
}

static struct quadtree *_quadtree_add(struct quadtree *parent,
				struct quadtree *new)
{
	int ret, i;

	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);

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

	return new;
}

/**
 * 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)
{
	if (parent == new)
		trap();

	validate_tree(parent);

	_quadtree_add(parent, new);

	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;
}

static double max_by_dir(double a, double b, int direction)
{
	switch (direction) {
	case 0: /* up */
	case 3: /* left */
		return MIN(a, b);
	case 1: /* right */
	case 2: /* down */
		return MAX(a, b);
	}

	trap();
	return 0;
}

static double maxv_by_dir(struct quadtree *a, int direction)
{
	switch (direction) {
	case 0: /* up */
	case 2: /* down */
		return a->pos.y;
	case 1: /* right */
	case 3: /* left */
		return a->pos.x;
	}

	trap();
	return 0;
}

static double quadtree_get_max_dimension(struct quadtree *node, int direction)
{
	struct quadtree *ch1 = NULL, *ch2 = NULL;
	double a, b;

	switch (direction) {
	case 0: /* up */
		ch1 = node->child[0];
		ch2 = node->child[1];
		break;
	case 1: /* right */
		ch1 = node->child[1];
		ch2 = node->child[3];
		break;
	case 2: /* down */
		ch1 = node->child[2];
		ch2 = node->child[3];
		break;
	case 3: /* left */
		ch1 = node->child[0];
		ch2 = node->child[2];
		break;
	default:
		trap();
	}

	if (ch1 && ch2) {
		a = quadtree_get_max_dimension(ch1, direction);
		b = quadtree_get_max_dimension(ch2, direction);
		return max_by_dir(a, b, direction);
	}

	if (ch1) {
		a = quadtree_get_max_dimension(ch1, direction);
		b = maxv_by_dir(ch1, direction);
		return max_by_dir(a, b, direction);
	}
	if (ch2) {
		a = quadtree_get_max_dimension(ch2, direction);
		b = maxv_by_dir(ch2, direction);
		return max_by_dir(a, b, direction);
	}

	return maxv_by_dir(node, direction);
}

/*
 * Stores the lower right and upper left corner coordinates to the
 * corner vectors
 */
static void quadtree_get_tree_dimensions(struct quadtree *node,
					struct vector *corner)
{
	corner[0].y = quadtree_get_max_dimension(node, 2);
	corner[0].x = quadtree_get_max_dimension(node, 1);
	corner[1].y = quadtree_get_max_dimension(node, 0);
	corner[1].x = quadtree_get_max_dimension(node, 3);

	if (debug) {
		printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
			corner[0].x, corner[0].y,
			corner[1].x, corner[1].y,
			node->pos.x, node->pos.y);
		fflush(stdout);
	}

	if ((corner[0].x < corner[1].x) ||
	    (corner[0].y < corner[1].y))
		trap();
}

static int is_within(struct vector *pos, struct vector *corner)
{
	if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
	    (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
		return 1;
	return 0;
}

static int quadrants_to_search(struct quadtree *node, struct vector *corner)
{
	int direction = 0, i;
	int up = 0, left = 0, right = 0, down = 0;

	for (i = 0; i < 2; i++) {
		if (corner[i].x <= node->pos.x)
			left = 1;
		else if (corner[i].x >= node->pos.x)
			right = 1;
		if (corner[i].y <= node->pos.y)
			up = 1;
		else if (corner[i].y >= node->pos.y)
			down = 1;
	}

	if (left || up)
		direction |= QUADTREE_UPLEFT;
	if (right || up)
		direction |= QUADTREE_UPRIGHT;
	if (left || down)
		direction |= QUADTREE_DOWNLEFT;
	if (right || down)
		direction |= QUADTREE_DOWNRIGHT;

	return direction;
}

static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
					struct vector *pos,
					struct vector *corner)
{
	struct vector tmp;
	struct quadtree *nearest, *node;
	double distance = 0, dist;
	int i, directions;

	if (!is_within(pos, corner)) {
		if (debug) {
			printf("No nearest to be found from (%f %f) (%f %f) "
				"pos (%f %f)\n",
				corner[0].x, corner[0].y,
				corner[1].x, corner[1].y,
				pos->x, pos->y);
			fflush(stdout);
		}
		return NULL;
	}

	if (is_within(&tree->pos, corner)) {
		vector_sub(pos, &tree->pos, &tmp);
		distance = vector_abs(&tmp);
		nearest = tree;
	} else {
		nearest = NULL;
	}

	directions = quadrants_to_search(tree, corner);

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

/*
		if (!(directions & (1 << i)))
			continue;
*/
		node = quadtree_find_nearest(tree->child[i], pos, corner);

		if (!node)
			continue;

		vector_sub(pos, &node->pos, &tmp);
		dist = vector_abs(&tmp);

		if (!nearest || dist < distance) {
			nearest = node;
			distance = dist;

		}
	}

	if (nearest && !is_within(&nearest->pos, corner)) {
		if (debug)
			printf("Node %p (%f %f) is not within "
				"search window (%f %f) (%f %f)\n",
				nearest, nearest->pos.x, nearest->pos.y,
				corner[0].x, corner[0].y,
				corner[1].x, corner[1].y);
		return NULL;
	}

	return nearest;
}

static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
						struct vector *pos,
						struct vector *corner)
{
	struct quadtree *nearest;
	struct vector sub;
	struct quadtree *node;
	double dist = 0, dist2;
	int i;

	nearest = quadtree_find_nearest(tree, pos, corner);

	if (nearest != tree)
		goto out;

	if (debug)
		printf("Avoiding parent or NULL node %p\n", nearest);

	/*
	 * oops, we don't want to pick the parent node, let's choose
	 * another one
	 */
	nearest = NULL;
	for (i = 0; i < 4; i++) {
		if (!tree->child[i])
			continue;

		if (!nearest) {
			nearest = quadtree_find_nearest(tree->child[i], pos,
							corner);

			if (nearest == NULL)
				continue;

			vector_sub(pos, &nearest->pos, &sub);
			dist = vector_abs(&sub);
			continue;
		}
		node = quadtree_find_nearest(tree->child[i],
					pos, corner);

		if (node == NULL)
			continue;

		vector_sub(pos, &node->pos, &sub);
		dist2 = vector_abs(&sub);

		if (dist2 < dist) {
			nearest = node;
			dist = dist2;
		}
	}

out:
	return nearest;
}

static void get_middle_point(struct vector *corner, struct vector *middle)
{
	middle->x = (corner[0].x + corner[1].x) / (double)2;
	middle->y = (corner[0].y + corner[1].y) / (double)2;
}

static int quadtree_split_by_node(struct quadtree *node,
				struct vector *corner, int dir)
{
	if (!is_within(&node->pos, corner)) {
		if (debug)
			printf("Not within search rectangle\n");
		return 0;
	}

	switch (dir) {
	case QUADTREE_UPLEFT:
		corner[0] = node->pos;
		break;
	case QUADTREE_UPRIGHT:
		corner[0].y = node->pos.y;
		corner[1].x = node->pos.x;
		break;
	case QUADTREE_DOWNRIGHT:
		corner[1] = node->pos;
		break;
	case QUADTREE_DOWNLEFT:
		corner[0].x = node->pos.x;
		corner[1].y = node->pos.y;
		break;
	default:
		trap();
	}

	if (debug)
		printf("Search rectangle is now (%f %f) (%f %f), (%f %f)\n",
			corner[0].x, corner[0].y,
			corner[1].x, corner[1].y,
			node->pos.x, node->pos.y);

	if ((corner[0].x < corner[1].x) ||
	    (corner[0].y < corner[1].y))
		trap();

	return 1;
}

/*
 * Quickly detach a node from a tree. Move all child nodes under
 * @parent node.
 */
static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
{
	struct quadtree *n;
	int i;

	/* Detach from the tree */
	if (node->parent)
		for (i = 0; i < 4; i++) {
			if (node->parent->child[i] == node) {
				node->parent->child[i] = 0;
				break;
			}
		}

	if (i == 4)
		trap();


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

		n = node->child[i];
		_quadtree_del(n, parent);
		_quadtree_add(parent, n);
	}

	return 0;
}

/**
 * Move everything under @tree node to @parent node, everything
 * else except the @tree node itself
 */
static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
			struct vector *corner_orig,
			struct quadtree_ops *ops)
{
	struct vector corner[2], mid;
	struct quadtree *t, *tmp;
	int moved = 0;

	get_middle_point(corner_orig, &mid);
	t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);

	if (!t) {
		if (debug)
			printf("Cannot find nearest node\n");

		goto out;
	}

	/*
	 * Now we have the t node which contains the object of the
	 * spatial middle coordinates of the tree.
	 */

	if (debug) {
		printf("Relocating node %p (%f %f) under parent %p\n", t,
			t->pos.x, t->pos.y, parent);
		printf("There are %ld child nodes left\n", tree->children);
		fflush(stdout);
	}
	_quadtree_del(t, tree);
	quadtree_add(parent, t, ops);

	moved++;
	tree->children--;
	if (!tree->children)
		goto out;

	/*
	 * Now split the search rectangle in quadtres and do the same
	 * with all of the quarters.
	 */

	corner[0] = corner_orig[0];
	corner[1] = corner_orig[1];
	if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
		moved += optimally_move_tree(tree, parent, corner, ops);

	corner[0] = corner_orig[0];
	corner[1] = corner_orig[1];
	if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
		moved += optimally_move_tree(tree, parent, corner, ops);

	corner[0] = corner_orig[0];
	corner[1] = corner_orig[1];
	if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
		moved += optimally_move_tree(tree, parent, corner, ops);

	corner[0] = corner_orig[0];
	corner[1] = corner_orig[1];
	if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
		moved += optimally_move_tree(tree, parent, corner, ops);

	get_middle_point(corner_orig, &mid);
	tmp = quadtree_find_nearest(tree, &mid, corner_orig);
	if (tmp && tmp != tree)
		trap();

out:
	if (debug)
		printf("Now moved %d nodes, %ld left\n", moved, tree->children);

	return moved;
}

/*
 * 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 = NULL;
	struct vector corner[2];
	int i, children = node->children, moved;

	validate_tree(node);

	if (debug) {
		printf("Deleting node %p under parent %p\n",
			       node, node->parent);
		printf("Relocating %ld children\n", node->children);
		fflush(stdout);
	}

	if (!node->parent) {
		/*
		 * 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 (debug) {
			printf("Deleting root node\n");
			fflush(stdout);
		}
		for (i = 0; i < 4; i++) {
			if (!node->child[i])
				continue;

			parent = node->child[i];
			_quadtree_del(parent, node);
			parent->parent = 0;
			quadtree_recalculate_parent_stats(parent, ops);
			break;
		}
	} else {
		/*
		 * The node has a parent. Detach the node from it and
		 * relocate the children.
		 */

		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);
	}

	validate_tree(parent);

	if (!node->children)
		goto out;
	/*
	 * Now we are ready to prepare for relocating the nodes under
	 * the parent.
	 */

	quadtree_get_tree_dimensions(node, corner);
	moved = optimally_move_tree(node, parent, corner, ops);

	if (debug) {
		if (moved != children) {
			printf("Got %d children but %d were moved\n",
				children, moved);
			printf("nearest children left:\n");
			for (i = 0 ; i < 4; i++)
				if (node->child[i])
					printf("   node %d %p, (%f %f)\n",
						i, node->child[i],
						node->child[i]->pos.x,
						node->child[i]->pos.y);
			fflush(stdout);
			trap();
		}
		printf("Delete done, returning parent %p\n", parent);
		fflush(stdout);
	}

out:
	validate_tree(parent);
	return quadtree_find_parent(parent);
}

static void check_for_crossed_subnodes(struct quadtree *node,
				struct vector *limit, struct quadtree_ops *ops)
{
	int direction = 0, i;
	int up = 0, left = 0, right = 0, down = 0;

	for (i = 0; i < 2; i++) {
		if (limit[i].x < node->pos.x)
			left = 1;
		else
			right = 1;
		if (limit[i].y < node->pos.y)
			up = 1;
		else
			down = 1;
	}

	if (left || up)
		direction |= QUADTREE_UPLEFT;
	if (right || up)
		direction |= QUADTREE_UPRIGHT;
	if (left || down)
		direction |= QUADTREE_DOWNLEFT;
	if (right || down)
		direction |= QUADTREE_DOWNRIGHT;
	if ((left && right) || (up && down))
		direction |= QUADTREE_SELF;

	if ((direction & QUADTREE_UPLEFT) && node->child[0])
		check_for_crossed_subnodes(node->child[0], limit, ops);

	if ((direction & QUADTREE_UPRIGHT) && node->child[1])
		check_for_crossed_subnodes(node->child[0], limit, ops);

	if ((direction & QUADTREE_DOWNLEFT) && node->child[2])
		check_for_crossed_subnodes(node->child[0], limit, ops);

	if ((direction & QUADTREE_DOWNRIGHT) && node->child[3])
		check_for_crossed_subnodes(node->child[0], limit, ops);

	if (direction & QUADTREE_SELF) {
		struct quadtree *parent;

		parent = quadtree_del(node, ops);
		quadtree_add(parent, node, ops);
	}
}

struct quadtree *quadtree_move(struct quadtree *node, struct vector new_pos,
			struct quadtree_ops *ops)
{
	struct quadtree *parent, *tree_parent;
	int modify;

	/* Check if we have crossed any of the parents */
	parent = node->parent;
	while (parent) {
		if (node->pos.x < parent->pos.x && new_pos.x > parent->pos.x)
			modify = 1;
		if (node->pos.x > parent->pos.x && new_pos.x < parent->pos.x)
			modify = 1;
		if (node->pos.y < parent->pos.y && new_pos.y > parent->pos.y)
			modify = 1;
		if (node->pos.y > parent->pos.y && new_pos.y < parent->pos.y)
			modify = 1;

		if (!modify) {
			parent = parent->parent;
			continue;
		}

		/*
		 * If the node has crossed the boundaries, remove it
		 * from the tree and add it again to it. It is then
		 * guaranteed to be in the correct position of the
		 * tree.
		 */
		tree_parent = quadtree_del(node, ops);
		node->pos = new_pos;
		quadtree_add(tree_parent, node, ops);
		return tree_parent;
	}

	/* Move the node into its new location */
	node->pos = new_pos;

	if(node->children) {
		/*
		 * Now, search the subtree for any children that are
		 * located in wrong place and move them into correct
		 * place within the tree.
		 */
		struct vector limit[2];

		limit[0] = node->pos;
		limit[1] = new_pos;
		check_for_crossed_subnodes(node, limit, ops);
	}

	return quadtree_find_parent(node);

}

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);
}