/*
 * Copyright (c) 2008-2009 Bill Whitacre http://rampancy.g0dsoft.com
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
*/

#include "quad_tree.h"

rat_quad_tree_matrix *rat_quad_tree_matrix_create(vector2 origin,vector2 cellsize,unsigned int numcells_x,unsigned int numcells_y)
{
	register unsigned int i,j,numcells=numcells_x*numcells_y;
	rat_quad_tree_matrix *matrix=(rat_quad_tree_matrix *)rat_alloc(sizeof(rat_quad_tree_matrix));
	memset(matrix,0,sizeof(rat_quad_tree_matrix));

	matrix->origin=origin;
	matrix->cellsize=cellsize;

	matrix->matrix_width=numcells_x;
	matrix->matrix_height=numcells_y;
	matrix->numcells=numcells;

	matrix->quadtrees=(rat_quad_tree **)rat_alloc(sizeof(rat_quad_tree)*numcells);
	for (i=0; i<numcells_x; i++)
	{
		for (j=0; j<numcells_y; j++)
		{
			register vector2 qa={i*cellsize.x,j*cellsize.y};
			register aabb quad;

			quad.a=qa=vector2_add(qa,origin);
			quad.b=vector2_add(qa,cellsize);

			matrix->quadtrees[i+j*numcells_x]=rat_quad_tree_create(quad);
		}
	}

	return matrix;
}

void rat_quad_tree_matrix_destroy(rat_quad_tree_matrix *matrix)
{
	register unsigned int i;

	for (i=0; i<matrix->numcells; i++) rat_quad_tree_destroy(matrix->quadtrees[i]);
	rat_dealloc((void *)matrix->quadtrees);
	rat_dealloc((void *)matrix);
}

static void add_entity_to_node(rat_quad_tree_node *node,rat_body *entity)
{
	register unsigned int i;
	rat_quad_tree_node *parent;

	if (node->numentities==0)
	{
		node->thisnode_entities=(rat_body **)rat_alloc(sizeof(rat_body *));
		node->thisnode_entities[0]=entity;
		node->numentities=1;
	}
	else
	{
		node->thisnode_entities=(rat_body **)
			rat_realloc(node->thisnode_entities,(node->numentities+1)*sizeof(rat_body *));
		node->thisnode_entities[node->numentities]=entity;
		node->numentities++;
	}

	for (parent=node->parent; parent; parent=parent->parent)
	{
		if (parent->numchildents==0)
		{
			parent->numchildents=1;
			parent->childnode_entities=(rat_body **)rat_alloc(sizeof(rat_body *));
			parent->childnode_entities[0]=entity;
		}
		else
		{
			register int is_already=0;
			for (i=0; i<parent->numchildents; i++)
			{
				if (parent->childnode_entities[i]==entity)
				{
					is_already=1;
					break;
				}
			}

			if (!is_already)
			{
				parent->childnode_entities=
					(rat_body **)rat_realloc
					(parent->childnode_entities,sizeof(rat_body *)*
					(parent->numchildents+1));
				parent->childnode_entities[parent->numchildents]=entity;
				parent->numchildents++;
			}
		}
	}
}

void rat_quad_tree_matrix_build(rat_quad_tree_matrix *matrix,rat_body **entities,unsigned int numentities)
{
	// this function just uses bounding box indexing to
	// speed up queries, meaning of course that we will go
	// over the head of rat_quad_tree_build a little bit.
	// that's okay though...
	register int i,j,k;

	for (i=0; i<(int)numentities; i++)
	{
		register int cell_l=(int)((entities[i]->hull->cached_bb.a.x-matrix->origin.x)/matrix->cellsize.x);
		register int cell_r=(int)((entities[i]->hull->cached_bb.b.x-matrix->origin.x)/matrix->cellsize.x);
		register int cell_t=(int)((entities[i]->hull->cached_bb.a.y-matrix->origin.y)/matrix->cellsize.y);
		register int cell_b=(int)((entities[i]->hull->cached_bb.b.y-matrix->origin.y)/matrix->cellsize.y);

		cell_l=cell_l<0?0:cell_l;
		cell_r=cell_r<(int)matrix->matrix_width?cell_r:(int)matrix->matrix_width-1;

		cell_t=cell_t<0?0:cell_t;
		cell_b=cell_b<(int)matrix->matrix_height?cell_b:(int)matrix->matrix_height-1;

		if (cell_l==cell_r&&cell_t==cell_b)
			rat_quad_tree_build(matrix->quadtrees[cell_l+cell_t*(int)matrix->matrix_width],entities+i,1);
		else if (cell_r>cell_l&&cell_t==cell_b)
		{
			for (j=cell_l; j<=cell_r; j++)
			{
				matrix->quadtrees[j+cell_t*matrix->matrix_width]->is_built=1;
				add_entity_to_node(matrix->quadtrees[j+cell_t*matrix->matrix_width]->root,entities[i]);
			}
		}
		else if (cell_b>cell_t&&cell_l==cell_r)
		{
			for (j=cell_t; j<=cell_b; j++)
			{
				matrix->quadtrees[cell_l+j*matrix->matrix_width]->is_built=1;
				add_entity_to_node(matrix->quadtrees[cell_l+j*matrix->matrix_width]->root,entities[i]);
			}
		}
		else
		{
			for (j=cell_l; j<=cell_r; j++)
			{
				for (k=cell_t; k<=cell_b; k++)
				{
					matrix->quadtrees[j+k*matrix->matrix_width]->is_built=1;
					add_entity_to_node(matrix->quadtrees[j+k*matrix->matrix_width]->root,entities[i]);
				}
			}
		}
	}
}

void rat_quad_tree_matrix_eachin(rat_quad_tree_matrix *matrix,rat_quad_tree_each_method each_method,void *data)
{
	register unsigned int i;
	for (i=0; i<matrix->numcells; i++)
	{
		if (matrix->quadtrees[i]->is_built)
			rat_quad_tree_eachin(matrix->quadtrees[i],each_method,data);
	}
}

void rat_quad_tree_matrix_clear(rat_quad_tree_matrix *matrix)
{
	register unsigned int i;
	for (i=0; i<matrix->numcells; i++) rat_quad_tree_clear(matrix->quadtrees[i]);
}

rat_quad_tree_node *rat_quad_tree_node_create(aabb quad,unsigned int depth,rat_quad_tree *tree,rat_quad_tree_node *parent)
{
	rat_quad_tree_node *node=(rat_quad_tree_node *)rat_alloc(sizeof(rat_quad_tree_node));
	memset(node,0,sizeof(rat_quad_tree_node));

	node->tree=tree;
	node->parent=parent;
	node->quad=quad;

	node->depth=depth;
	node->tree->numnodes++;
	node->tree->maxdepth=depth>node->tree->maxdepth?depth:node->tree->maxdepth;

	return node;
}

rat_quad_tree *rat_quad_tree_create(aabb quad)
{
	rat_quad_tree *tree=(rat_quad_tree *)rat_alloc(sizeof(rat_quad_tree));
	memset(tree,0,sizeof(rat_quad_tree));

	tree->is_built=0;
	tree->root=rat_quad_tree_node_create(quad,0,tree,NULL);

	return tree;
}

void rat_quad_tree_node_destroy(rat_quad_tree_node *node)
{
	register unsigned int i;

	if (node->are_children)
	{
		for (i=0; i<4; i++)
			rat_quad_tree_node_destroy(node->children[i]);
		rat_dealloc((void *)node->children);
	}

	if (node->numentities) rat_dealloc((void *)node->thisnode_entities);
	if (node->numchildents) rat_dealloc((void *)node->childnode_entities);

	node->tree->numnodes--;
	rat_dealloc((void *)node);
}

void rat_quad_tree_destroy(rat_quad_tree *tree)
{
	rat_quad_tree_node_destroy(tree->root);
	rat_dealloc((void *)tree);
}

int rat_quad_tree_node_query_entity(rat_quad_tree_node *node,rat_body *entity)
{
	if (node->are_children)
	{
		if (aabb_is_aabb_inside(entity->hull->cached_bb,node->quad))
		{
			register int result1=rat_quad_tree_node_query_entity(node->children[0],entity);
			register int result2=rat_quad_tree_node_query_entity(node->children[1],entity);
			register int result3=rat_quad_tree_node_query_entity(node->children[2],entity);
			register int result4=rat_quad_tree_node_query_entity(node->children[3],entity);

			if (!result1&&!result2&&!result3&&!result4) add_entity_to_node(node,entity);
			return 1;
		}
		else if (node->depth==0)
		{
			if (aabb_is_aabb_overlapping(entity->hull->cached_bb,node->quad))
			{
				add_entity_to_node(node,entity);
				return 1;
			}
		}
	}
	else if (aabb_is_aabb_inside(entity->hull->cached_bb,node->quad))
	{
		register unsigned int i;
		add_entity_to_node(node,entity);
		if (node->numentities>2)
		{
			register unsigned int i;
			register unsigned int numents=node->numentities;

			rat_body **theseentities=(rat_body **)rat_alloc(sizeof(rat_body *)*numents);
			for (i=0; i<numents; i++) theseentities[i]=node->thisnode_entities[i];
			//memcpy(theseentities,node->thisnode_entities,sizeof(rat_body *)*numents);

			rat_quad_tree_node_split(node);
			rat_quad_tree_node_clear(node);

			for (i=0; i<numents; i++)
				rat_quad_tree_node_query_entity(node,theseentities[i]);

			rat_dealloc((void *)theseentities);
		}
		return 1;
	}
	else if (node->depth==0)
	{
		if (aabb_is_aabb_overlapping(entity->hull->cached_bb,node->quad))
		{
			add_entity_to_node(node,entity);
			return 1;
		}
	}

	return 0;
}

void rat_quad_tree_node_split(rat_quad_tree_node *node)
{
	register unsigned int i;
	register vector2 avg,tl,br;
	register aabb ul,ur,ll,lr;

	if (node->are_children) return;

	tl=node->quad.a; br=node->quad.b;
	avg=vector2_multf(vector2_add(node->quad.a,node->quad.b),0.5);

	ul.a=tl; ul.b=avg;
	lr.a=avg; lr.b=br;

	ur.a.x=avg.x; ur.a.y=tl.y;
	ur.b.x=br.x; ur.b.y=avg.y;

	ll.a.x=tl.x; ll.a.y=avg.y;
	ll.b.x=avg.x; ll.b.y=br.y;

	node->are_children=1;

	node->children=(rat_quad_tree_node **)rat_alloc(sizeof(rat_quad_tree_node *)*4);
	node->children[0]=rat_quad_tree_node_create(ul,node->depth+1,node->tree,node);
	node->children[1]=rat_quad_tree_node_create(ur,node->depth+1,node->tree,node);
	node->children[2]=rat_quad_tree_node_create(ll,node->depth+1,node->tree,node);
	node->children[3]=rat_quad_tree_node_create(lr,node->depth+1,node->tree,node);
}

void rat_quad_tree_build(rat_quad_tree *tree,rat_body **entities,unsigned int numentities)
{
	register unsigned int i;
	tree->is_built=1;
	for (i=0; i<numentities; i++)
		rat_quad_tree_node_query_entity(tree->root,entities[i]);
}

/*
static void quad_node_draw(rat_quad_tree_node *node)
{
	drawutil_draw_rect(node->quad.a.x,node->quad.a.y,node->quad.b.x,node->quad.b.y);
	if (node->are_children)
	{
		quad_node_draw(node->children[0]);
		quad_node_draw(node->children[1]);
		quad_node_draw(node->children[2]);
		quad_node_draw(node->children[3]);
	}
}

void rat_quad_tree_draw(rat_quad_tree *tree)
{
	drawutil_set_color(0,0,0,1.0);
	quad_node_draw(tree->root);
}
*/

void rat_quad_tree_node_eachin(rat_quad_tree_node *node,rat_quad_tree_each_method each_method,void *data)
{
	register unsigned int i,j;

	if (node->numentities>0)
	{
		if (node->numentities>1)
		{
			for (i=0; i<node->numentities; i++)
			{
				for (j=i+1; j<node->numentities; j++)
					each_method(node->thisnode_entities[i],node->thisnode_entities[j],data);

				for (j=0; j<node->numchildents; j++)
					each_method(node->thisnode_entities[i],node->childnode_entities[j],data);
			}
		}
		else
		{
			for (j=0; j<node->numchildents; j++)
				each_method(node->thisnode_entities[0],node->childnode_entities[j],data);
		}
	}

	if (node->are_children&&node->numchildents>0)
	{
		rat_quad_tree_node_eachin(node->children[0],each_method,data);
		rat_quad_tree_node_eachin(node->children[1],each_method,data);
		rat_quad_tree_node_eachin(node->children[2],each_method,data);
		rat_quad_tree_node_eachin(node->children[3],each_method,data);
	}
}

void rat_quad_tree_eachin(rat_quad_tree *tree,rat_quad_tree_each_method each_method,void *data)
{
	rat_quad_tree_node_eachin(tree->root,each_method,data);
}

void rat_quad_tree_node_clear(rat_quad_tree_node *node)
{
	if (node->numentities) rat_dealloc((void *)node->thisnode_entities);
	if (node->numchildents) rat_dealloc((void *)node->childnode_entities);
	node->numentities=0;
	node->numchildents=0;
}

void rat_quad_tree_clear(rat_quad_tree *tree)
{
	aabb quad=tree->root->quad;

	rat_quad_tree_node_destroy(tree->root);
	tree->root=rat_quad_tree_node_create(quad,0,tree,NULL);

	tree->is_built=0;
	tree->maxdepth=0;
}