// Author : Chris Tolley
// Date   : 7/13/2004
// QuadTree.cpp contains the class definition for a quad tree class to 
// use to keep track of objects in a 3d space

#include <iostream>
#include <vector>
#include <stack>
#include <math.h>

using namespace std;

#include "QuadTree.h"
#include "QuadNode.h"
#include "InteriorQuadNode.h"
#include "LeafQuadNode.h"
#include "Object.h"
#include "logger.h"


// Constructor.  Size must be a power of 2
QuadTree::QuadTree( unsigned short size ) : m_usSize( 0 ), m_iNumInterior( 0 )
{
	// Calculate the number of pointers to allocate space for
	unsigned short total = 2;
	while( size != 1 )
	{
		m_usSize += total;
		total *= 4;
		size /= 2;
	}// End while

	// Allocate enough space for each pointer
	for( int n = 0; n <= m_usSize; ++n )
	{
		m_vNodes.push_back( NULL );
	}// End for n
}// End QuadTree::Constructor


// Destructor
QuadTree::~QuadTree()
{
	// Delete each node
	for( int n = 0; n < m_usSize; ++n )
	{
		if( m_vNodes[n] != NULL )
			delete m_vNodes[n];
	}// End for n
//	delete[] m_vNodes;
}// End QuadTree::Destructor


// Initializes the quad tree
void QuadTree::Initialize( unsigned short maxX, unsigned short minX,
						   unsigned short maxZ, unsigned short minZ,
						   unsigned short arrayPos )
{
	// Calculate the distance between maxX and minX
	unsigned short edge = maxX - minX;

	// Calculate the midpoint of the current node
	unsigned short midX = ( maxX + minX ) / 2;
	unsigned short midZ = ( maxZ + minZ ) / 2;

	// Calculate the radius of this node, used for lookups
	float radius = sqrt( ( maxX - midX ) * ( maxX - midX ) +
						 ( maxZ - midZ ) * ( maxZ - midZ ) );

	// If we have reached the level where we have to create leaf nodes
	if( edge == LEAF_EDGE_LENGTH )
	{
		// Then we create a leaf node and store it
		m_vNodes[ arrayPos ] = new LeafQuadNode( midX, midZ, radius );
	}// End if
	else
	{
		// Create a new interior node
		m_vNodes[ arrayPos ] = new InteriorQuadNode( midX, midZ, radius );
		++m_iNumInterior;

		// Initialize each of the child nodes
		Initialize( maxX, midX, maxZ, midZ, ULChild( arrayPos ) );
		Initialize( midX, minX, maxZ, midZ, URChild( arrayPos ) );
		Initialize( maxX, midX, midZ, minZ, LLChild( arrayPos ) );
		Initialize( midX, minX, midZ, minZ, LRChild( arrayPos ) );
	}// End else
}// End QuadTree::Initialize


// Inserts an object into the quadtree
void QuadTree::Insert( SmartPointer& obj )
{
	// Get the position of the object
	float x, y, z;
	obj->GetPos( x, y, z );

	// Start at the root node
	unsigned short index = 0;

	// Some variables to help out
	InteriorQuadNode* node;
	unsigned short mx = 0;
	unsigned short mz = 0;

	// While we have not reached a leaf node, continue moving down the tree
	while( ! isLeaf( index ) )
	{
		// Get the current node and cast it to an InteriorQuadNode
		node = reinterpret_cast< InteriorQuadNode* >( m_vNodes[index] );

		// Get the midpoint of the current node
		node->GetMidPoint( mx, mz );

		// Translate the position of the object so that the mid point of the
		// node is at 0,0 and the object is moved relative to that point
		float tx = x - float( mx );
		float tz = z - float( mz );

		// Move down the tree according to which quadrant the point lays in
		// If tx is greater than zero
		if( tx > 0 )
		{	// Then we want one of the upper quadrants
			if( tz > 0 )
				index = ULChild( index );
			else
				index = LLChild( index );
		}// End if
		else
		{// Otherwise we want one of the lower quadrants
			if( tz > 0 )
				index = URChild( index );
			else
				index = LRChild( index );
		}// End else
	}// End while

	// Cast the node to a leaf node and add obj to it and return it
	LeafQuadNode* temp = reinterpret_cast< LeafQuadNode* >( m_vNodes[index] );
	temp->AddObject( obj );
//	return temp;
}// End QuadTree::Insert


// Removes an object from the quadtree
void QuadTree::Remove( SmartPointer& obj )
{
	// Get the position of the object
	float x, y, z;
	obj->GetPos( x, y, z );

	// Start at the root node
	unsigned short index = 0;

	// Some variables to help out
	InteriorQuadNode* node;
	unsigned short mx = 0;
	unsigned short mz = 0;

	// While we have not reached a leaf node, continue moving down the tree
	while( ! isLeaf( index ) )
	{
		// Get the current node and cast it to an InteriorQuadNode
		node = reinterpret_cast< InteriorQuadNode* >( m_vNodes[index] );

		// Get the midpoint of the current node
		node->GetMidPoint( mx, mz );

		// Translate the position of the object so that the mid point of the
		// node is at 0,0 and the object is moved relative to that point
		float tx = x - float( mx );
		float tz = z - float( mz );

		// Move down the tree according to which quadrant the point lays in
		// If tx is greater than zero
		if( tx > 0 )
		{	// Then we want one of the upper quadrants
			if( tz > 0 )
				index = ULChild( index );
			else
				index = LLChild( index );
		}// End if
		else
		{// Otherwise we want one of the lower quadrants
			if( tz > 0 )
				index = URChild( index );
			else
				index = LRChild( index );
		}// End else
	}// End while

	// Now that we are at the leaf node level we can remove the object.
	reinterpret_cast< LeafQuadNode* >( m_vNodes[index] )->Remove( obj );
}// End QuadTree::Remove


// Removes all objects in this quadtree
void QuadTree::RemoveAll()
{
	// For each leaf node
	for( int n = m_iNumInterior; n < m_vNodes.size(); ++n )
	{
		// If the pointer is not NULL
		if( m_vNodes[ n ] != NULL )
		{
			// Remove all objects from it
			reinterpret_cast< LeafQuadNode* >( m_vNodes[n] )->RemoveAll();
		}// End if
	}// End for n

}// End QuadTree::RemoveAll


// Stores any objects in a circle within radius around x,z
void QuadTree::GetObjects( vector< SmartPointer >& objects, float radius, 
					  	   float x, float z ) const
{
	// Create a stack to hold the positions of the nodes
	stack< unsigned short > nodes;

	// Put the root on the stack
	nodes.push( 0 );

	// The index of the current node we are looking at
	unsigned short current = 0;

	// While the stack is not empty
	while( ! nodes.empty() )
	{
		// Get the index value of the next node off the stack
		current = nodes.top(); nodes.pop();

		// If the current node is in the sphere
		if( m_vNodes[ current ]->InSphere( x, z, radius ) )
		{
			// If the current node is a leaf node
			if( isLeaf( current ) )
			{
				// Get all the objects from that leaf node that are also inside
				// that sphere and put them into the vector of object
				reinterpret_cast< LeafQuadNode* >( m_vNodes[ current ] )
									->GetAllInSphere( x, z, radius, objects );
			}// End if
			else
			{
				// Otherwise we can put all the nodes children onto the stack
				nodes.push( LLChild( current ) );
				nodes.push( LRChild( current ) );
				nodes.push( ULChild( current ) );
				nodes.push( URChild( current ) );
			}// End else
		}// End if

//		cin.ignore();
	}// End while
}// End QuadTree::GetObjects
