//=================================================================================================//
// filename: utlspheretree.h                                                                       //
//                                                                                                 //
// description: A binary tree of spheres - put stuff in it to do fast spatial queries              //
//                                                                                                 //
//=================================================================================================//

#ifndef _utlspheretree_H_
#define _utlspheretree_H_

#include "mathlib/vector4d.h"
#include "tier1/utlvector.h"
#include "collisionutils.h"

typedef Vector4D Sphere_t;
typedef Vector4D Plane_t;


// Paste the following commented text into your autoexp.dat to get a nifty debugger visualizer for CUtlSphereTree.
// (insert it below [Visualizer] or ";-------------VALVE AUTOEXP AUTOGENERATED BLOCK START----------------" )
// NOTE: you need to define DEBUGGABLE_NODES (on by default in debug) and rebuild to get the full benefit of this.
// This does bog down VS2005 if you expand many levels of the tree - paste “tree.m_Nodes.m_pElements[node]” to sidestep that.
// Also note that names are CASE-INSENSITIVE, so if you have a member 'index' and a function 'Index', it will break :o/
#ifdef _DEBUG
#define DEBUGGABLE_NODES
#endif
/*
;-------- CUtlSphereTree start ------------------------------------------------------
Vector4D{
	preview
	( #(		"(", $c.x, ", ", $c.y, ", ", $c.z, ", ", $c.w, ")"		) )
}
CUtlSphereTree::Node{
	preview
	( #(
		#if ( $c.deepIndex < 0 )
		( #( "leaf data = ", $c.pData,    ", bounds(", $c.bounds.x, ", ", $c.bounds.y, ", ", $c.bounds.z, ", ", $c.bounds.w, ")" )	)
		#else
		( #( "depth = ",     $c.maxDepth, ", bounds(", $c.bounds.x, ", ", $c.bounds.y, ", ", $c.bounds.z, ", ", $c.bounds.w, ")" )	)
	) )
	children
	( #(
		#if ( $c.deepIndex < 0 )
		( #(
			#(	leaf data      : $c.pData )
		) )
		#else
		( #(
			#(	child[deep]    : $c.pTree->m_Nodes.m_pElements[ $c.deepIndex ] ),
			#(	child[shallow] : $c.pTree->m_Nodes.m_pElements[ $c.shallowIndex ] ),
			#(	subtree depth  : $c.maxDepth )
		) ),
		#(		node bounds    : $c.bounds ),
		#(		z[raw_members] : [$c,!] )
	) )
}
CUtlSphereTree::NodeRef{
	preview
	( #(
		#if ( $c.nodeIndex < 0 )
		( #( "Invalid!" ) )
		#else
		( #(
			#if ( $c.pNodes->m_pElements[ $c.nodeIndex ].deepIndex < 0 )
			( #(	"leaf data = ", $c.pNodes->m_pElements[ $c.nodeIndex ].pData,    ", bounds(", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.x, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.y, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.z, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.w, ")"	) )
			#else
			( #(	"depth = ",     $c.pNodes->m_pElements[ $c.nodeIndex ].maxDepth, ", bounds(", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.x, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.y, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.z, ", ", $c.pNodes->m_pElements[ $c.nodeIndex ].bounds.w, ")"	) )
		) )
	) )
	children
	( #(
		#if ( $c.nodeIndex < 0 )
		( #(	invalid!       : $c.nodeIndex								) )
		#else
		( #(	node           : $c.pNodes->m_pElements[ $c.nodeIndex ]		) ),
		#(		z[raw_members] : [$c,!] )
	) )
}
CUtlSphereTree{
	preview
	( #(	"treeCount = ", $c.m_NumNodesInTree, ", allocCount = ", $c.m_Nodes.m_Size, ", freelistHead = ", $c.m_FreelistHead	) )
	children
	( #(
		deep child     : $c.m_Nodes.m_pElements[ $c.m_Nodes.m_pElements[0].deepIndex ],
		shallow child  : $c.m_Nodes.m_pElements[ $c.m_Nodes.m_pElements[0].shallowIndex ],
        z[raw_members] : [$c,!]
	) )
}
;-------- CUtlSphereTree end ------------------------------------------------------
*/

class CUtlSphereTree
{
	struct Node
	{
		// Every node either has two children or (if it is a leaf) none
		// TODO: shrink the size of Node (pack child node indices and isLeaf flags into an int32, with leaf nodes in a separate vector so we can index 2^16 nodes total)
		// TODO: for the case where the caller's data already contains a bounding Sphere_t, we could omit the 20-byte Sphere_t from leaf nodes
		//       (the void * would need wrapping with a user-provided class that provides a bounding sphere accessor function - or, just provide a pointer to the Sphere_t)
		// TODO: determine whether nodes fully containing other nodes happens a lot in practise - if so, it may be worthwhile allowing non-leaf Nodes to hold a pData pointer
		Sphere_t bounds;
		int deepIndex, shallowIndex;// Not set (-1) for Leaf nodes
		const void *pData;			// Only set for Leaf nodes
		int maxDepth;				// The maximum number of steps to a leaf node from here
									// (zero for leaf nodes), used to rebalance the tree
#ifdef DEBUGGABLE_NODES
		CUtlSphereTree *pTree;		// Allows debug pane tree expansion
#endif
	};

	struct NodeRef
	{
		// Convenience class to allow access to nodes, stored in the m_Nodes vector, by index
		NodeRef( int node, const CUtlVector< Node > &nodes )
		 : nodeIndex( node ), pNodes( (CUtlVector< Node > *)&nodes ) {}

		Node * Ptr( void ) const
		{
			Assert ( nodeIndex >= 0 );
			return ( nodeIndex >= 0 ) ? &(*pNodes)[ nodeIndex ] : (Node *)NULL;
		}
		Node * operator->( void ) const { return  Ptr(); }
		Node & operator*(  void ) const { return *Ptr(); }
		int     Index(     void ) const { return nodeIndex; }
		NodeRef Deep(      void ) const { return NodeRef( Ptr()->deepIndex,    *pNodes ); }
		NodeRef Shallow(   void ) const { return NodeRef( Ptr()->shallowIndex, *pNodes ); }
		bool    IsValid(   void ) const { return ( nodeIndex >= 0 ); }
		bool    IsLeaf(    void ) const { return ( Ptr()->deepIndex < 0 ); }

	private:
		NodeRef( void ) : nodeIndex( -1 ), pNodes( NULL ) { Assert( 0 ); }

		int						nodeIndex;
		CUtlVector< Node > *	pNodes;	// TODO: find a way to get rid of this, given it's the same for all NodeRefs
	};

public:

	// Cut
	//
	// This member class represents a 'cut' of a sphere tree - which is the result of a depth-first traversal
	// of the tree in which each node is run through a test function that may return "true", "false" or "partial".
	//
	// For example:
	//  - a sphere tree can be 'cut' by a plane
	//  - the cut is initialised to contain the whole tree
	//  - the test function is a sphere-plane test
	//   o nodes which return 'true' (fully in front) are included in the cut and not traversed further
	//   o nodes which return 'false' (fully behind) are discarded from the cut and not traversed further
	//   o for nodes which return 'partial' (intersecting) they are discarded and the test recurses to their children
	//     partial leaf nodes are included in the cut
	//  - the result (the 'cut') is thus the set of nodes fully/partially in front of the plane,
	//    subdivided only where necessary to make the result more accurate
	//
	// In practise, the Cut contains an (unordered) list of sub-tree root node indices.
	class Cut
	{
		friend class CUtlSphereTree;

	public:

		// Flags to guide how a Cut is to be refined by a set of intersection primitives
		enum CutFlags
		{
			PARTIAL_INTERSECTIONS = 1,	// 'Partial' node intersections should be counted
										// (for planes, this means include nodes only partially in front of a plane)
			OR_INTERSECTIONS = 2		// Passing the intersection test for ANY primitive in the current set constitutes
										// a pass ('AND' mode is the default - it requires passing w.r.t ALL primitives)
		};

		enum CutTestResult
		{
			CUT_TEST_FAIL = 0,
			CUT_TEST_PASS = 1,
			CUT_TEST_PARTIAL = 2
		};

		// Initialize a Cut with an entire sphere tree (i.e. the root node)
		Cut( const CUtlSphereTree *pTree );
		~Cut( void );

		// Return the user data pointers of the leaf nodes within the Cut. To limit the number of results returned,
		// pass 'maxResults > 0' (note that the return value may be more than this).
		// This is an efficient way to find all the leaves inside/outside a set of planes.
		int GetLeaves( CUtlVector< void * > &leaves, int maxLeaves = 0 ) const;

	private:

		// Don't use the default constructor
		Cut( void ) : m_pTree( NULL ) { Assert( 0 ); };

		const CUtlSphereTree *m_pTree;	// The associated sphere tree
		CUtlVector< int >m_NodeRefs;	// Sub-tree root node indices
	};


public:

	CUtlSphereTree();
	virtual ~CUtlSphereTree();

	// It is valid to Insert a NULL pData pointer, and it is valid to pass the same pData pointer with
	// multiple different bounding spheres, but it is NOT valid to pass the same pointer+sphere pair twice.
	// TODO: we can build a better tree if given all the spheres at once (would definitely be a win for non-dynamic scenes)
	void Insert( const void *pData, const Sphere_t *bounds );
	// Inverse of Insert (make sure to pass the same parameters!)
	void Remove( const void *pData, const Sphere_t *bounds );
	// Empty the tree (but do not deallocate)
	void RemoveAll( void );
	// Empty the tree (and deallocate)
	void Purge( void );


	// Intersect a ray with the spheretree (or a Cut thereof), returning the user data pointers of the leaf nodes intersecting
	// the ray. To limit the number of results returned, pass 'maxResults > 0' (note that the return value may be more than this).
	// NOTE: these spheres are currently unordered
	// TODO: write a function to return the first intersection and an ordered list of intersections (use a priority queue of nodes,
	//       always processing the top element and emitting leaves - time this and compare it with just quicksorting the results)
	int IntersectWithRay( Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int maxResults = 0, CUtlSphereTree::Cut *cut = NULL ) const;
	// Intersect a sphere with the spheretree (or a Cut thereof), returning the user data pointers of the leaf nodes intersecting
	// the sphere. To limit the number of results returned, pass 'maxResults > 0' (note that the return value may be more than this).
	// If 'bPartial' is FALSE, only leaves fully contained within the sphere are returned (otherwise partial overlaps are returned too)
	// NOTE: these spheres are currently unordered
	// TODO: optionally sort by distance from the sphere centre
	int IntersectWithSphere( const Sphere_t &sphere, bool bPartial, CUtlVector< void * > &result, int maxResults = 0, CUtlSphereTree::Cut *cut = NULL ) const;
	// Cut the tree (or refine an existing cut) using one or more planes. The output Cut will contain all
	// sphere tree nodes which are partially/fully in front of any/all of the planes (see CUtlSphereTree_Cut::CutFlags).
	// If outputCut is NULL, then inputCut will be updated.
	void CutByPlanes( Cut *outputCut, const Cut *inputCut, int cutFlags, Plane_t *planes, int numPlanes = 1 ) const;

	// Use this method to measure the effect of any changes to tree-construction logic. It computes some numbers relating to the efficiency of the tree:
	//  - averageDepth: the tree's depth, as compared to the depth of a 'perfect' tree (idealDepth)
	//  - averageRedundancy: the average 'redundancy' (volume ratio) between a node and its parent - this is in [0,1] and correlates to query cost (lower is better, less than 0.4 is good).
	//  - averageSphereTestCost: estimated average cost of intersecting a sphere (of a given radius) with the tree (uses the redundancy values to compute intersection probabilities)
	void MeasureTreeQuality( float &averageDepth, float &idealDepth, float &averageRedundancy, float &averageSphereTestCost, float sphereTestRadius = 0.0f ) const;

private:

	// To avoid node/index-shuffling, we add Removed Nodes to a freelist:
	void FreelistPush( NodeRef &node );
	NodeRef FreelistPop( void );

	// We can't hold pointers to Nodes within a CUtlVector, so this util func converts from an index to a NodeRef:
	NodeRef Ref( int node ) const { return NodeRef( node, m_Nodes ); }
	// Allocates a new node off m_Nodes and init it as a leaf
	NodeRef NewNode( const void *pData, const Sphere_t *bounds );
	// Allocates a new node off m_Nodes and init it by copying an existing node
	NodeRef CopyNode( const NodeRef &nodeToCopy );
	// Assign two children to a node (recomputes its 'bounds' and 'maxDepth')
	void SetNodeChildren( NodeRef &node, const NodeRef &childA, const NodeRef &childB );

	// Recursive function called by Insert()
	void FindPathToClosestLeaf_R( NodeRef node, const Vector &pos, const float nodeDist, float &minDist, CUtlVector< int > &path ) const;

	// Recursive function called by Remove()
	int Remove_R( NodeRef &node, const void *pData, const Sphere_t *bounds );
	// Called by Remove_R to identify the Node to remove:
	bool NodeHasData( const NodeRef &node, const void *pData, const Sphere_t *bounds );

	// Called by Insert() and Remove() to ensure that the subtrees under a node are balanced
	void RebalanceSubtrees( NodeRef &node, bool bInserting );
	// Is this node's sub-tree balanced? (left and right sub-trees differ in 'maxDepth' by at most 1)
	bool IsSubtreeBalanced( const NodeRef &node ) const;
	// Useful debug function if there's a bug in tree construction
	void ParanoidBalanceCheck_R( const NodeRef &node ) const;

	// The basic metric used during insertion to determine the 'cost' of different node-grouping choices
	float Cost( const Sphere_t &sphere ) const;
	// Computes the composite bounding sphere of two spheres
	Sphere_t AddSpheres( const Sphere_t &pA, const Sphere_t &pB ) const;
	// Called by RebalanceSubtrees() to determine the optimal rebalance permutation
	float ComputePairingCost( const NodeRef &pair1A, const NodeRef &pair1B, const NodeRef &pair2A, const NodeRef *pair2B ) const;

	// Called by IntersectWithRay
	void IntersectWithRay_R( const NodeRef &node, Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int &count ) const;
	// Called by IntersectWithSphere
	void IntersectWithSphere_R( const NodeRef &node, const Sphere_t &sphere, const bool bPartial, CUtlVector< void * > &result, int &count, const float distSq ) const;
	bool SpheresIntersect( const Sphere_t &A, const Sphere_t &B, float &distSq ) const;
	// Called by CutByPlanes
	void CutByPlanes_R(  Cut *outputCut, const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const;
	Cut::CutTestResult CutByPlanes_Test( const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const;
	// Leaf node list accessor for Cut::GetLeaves:
	void GetLeavesUnderNode_R( const NodeRef &node, CUtlVector< void * > &leaves, int &count ) const;
	// Called by MeasureTreeQuality
	void  MeasureTreeQuality_R( const NodeRef &node, const int steps, float &averageDepth, float &averageRedundancy, const float pathRedundancy, float &averageSphereTestCost, const float pathProbability, const float sphereTestRadius ) const;
	float ComputeRedundancy(    const NodeRef &parent, const NodeRef &child ) const;
	float ComputeIntersectionProbability( const NodeRef &parent, const NodeRef &child, const float sphereTestRadius ) const;

	// We reference Nodes in this vector by index (the root is always at index zero)
	CUtlVector< Node >	m_Nodes;
	// Removed Nodes are added to a freelist so we don't need to do any index-shuffling
	int					m_FreelistHead;
	// How many nodes are in the tree? (i.e. in m_Nodes but not the freelist)
	int					m_NumNodesInTree;
	// The index of the last inserted node (should always be a leaf, -1 if invalid):
	int					m_PrevInsertedNode;
};




//  =====  CUtlSphereTree implementation  =====================================

inline CUtlSphereTree::CUtlSphereTree( void )
{
	Purge();
}

inline CUtlSphereTree::~CUtlSphereTree( void )
{
}

inline void CUtlSphereTree::RemoveAll( void )
{
	// Empty m_Nodes and our freelist
	m_Nodes.RemoveAll();
	m_FreelistHead   = -1;
	m_NumNodesInTree = 0;
	m_PrevInsertedNode = -1;
}

inline void CUtlSphereTree::Purge( void )
{
	// Empty m_Nodes and our freelist (and deallocate memory)
	RemoveAll();
	m_Nodes.Purge();
}

inline void CUtlSphereTree::FreelistPush( NodeRef &node )
{
	// TODO: update the freelist to use a FreeNodeRef struct which can be traversed with a Debug Visualizer
	if ( m_PrevInsertedNode == node.Index() )
		m_PrevInsertedNode = -1;
	*(int *)node.Ptr() = m_FreelistHead;
	m_FreelistHead = node.Index();
	Assert( m_NumNodesInTree > 0 );
	m_NumNodesInTree--;
}

inline CUtlSphereTree::NodeRef CUtlSphereTree::FreelistPop( void )
{
	Assert( m_FreelistHead >= 0 );
	NodeRef node = Ref( m_FreelistHead );
	if ( m_FreelistHead >= 0 )
		m_FreelistHead = *(int *)&m_Nodes[ m_FreelistHead ];
	return node;
}

inline CUtlSphereTree::NodeRef CUtlSphereTree::NewNode( const void *pData, const Sphere_t *bounds )
{
	// Allocate a new node off m_Nodes (from our freelist if available) and init it as a leaf
	Assert( ( m_NumNodesInTree >= 0 ) && ( m_NumNodesInTree <= m_Nodes.Count() ) );
	Assert( ( m_NumNodesInTree < m_Nodes.Count() ) == ( m_FreelistHead >= 0 ) );
	NodeRef node = ( m_FreelistHead >= 0 ) ? FreelistPop() : Ref( m_Nodes.AddToTail() );
	m_NumNodesInTree++;
	node->bounds		= bounds ? *bounds : Sphere_t(0,0,0,0);
	node->deepIndex		= -1;
	node->shallowIndex	= -1;
	node->pData			= pData;
	node->maxDepth		= 0;
#ifdef DEBUGGABLE_NODES
	node->pTree			= this;
#endif
	return node;
}

inline CUtlSphereTree::NodeRef CUtlSphereTree::CopyNode( const NodeRef &nodeToCopy )
{
	// Allocate a new node off m_Nodes and init it by copying an existing node
	NodeRef node = NewNode( NULL, NULL );
	*node = *nodeToCopy;
	return node;
}

inline void CUtlSphereTree::SetNodeChildren( NodeRef &node, const NodeRef &childA, const NodeRef &childB )
{
	bool bChildADeeper = ( childA->maxDepth > childB->maxDepth );
	node->deepIndex		= bChildADeeper ? childA.Index() : childB.Index();
	node->shallowIndex	= bChildADeeper ? childB.Index() : childA.Index();
	node->bounds		= AddSpheres( node.Deep()->bounds, node.Shallow()->bounds );
	node->maxDepth		= node.Deep()->maxDepth + 1;
	node->pData			= NULL; // If it was a leaf, it's not any more
}

inline void CUtlSphereTree::Insert( const void *pData, const Sphere_t *bounds )
{
	if ( !m_NumNodesInTree )
	{
		// Special case: first node inserted
		NodeRef root = NewNode( pData, bounds );
		Assert( root.Index() == 0 ); // The root should always be at index zero
		return;
	}

	// Find the closest leaf to the inserted sphere, and record the path to it
	float minDist = FLT_MAX;
	const Vector &pos = bounds->AsVector3D();
	if ( m_PrevInsertedNode >= 0 )
	{
		// Use the last inserted node as an upper bound on minDist (~halves the cost of FindPathToClosestLeaf_R)
		NodeRef node = Ref( m_PrevInsertedNode );
		Assert( node.IsLeaf() );
		if ( node.IsLeaf() )
		{
			float dist = ( node->bounds.AsVector3D() - pos ).Length();
			minDist = MAX( dist*1.1f, dist + 0.001f ); // Add slop for paranoia's sake
		}
	}
	CUtlVector< int > path;
	float nodeDist = ( Ref( 0 )->bounds.AsVector3D() - pos ).Length();
	FindPathToClosestLeaf_R( Ref( 0 ), pos, nodeDist, minDist, path );
	Assert( path.Count() && path[ path.Count() - 1 ] == 0 ); // All paths lead to the root!
	int bestSibling = 0;

/* TODO: see if this could improve the tree noticeably, without increasing tree-building time too much (reuse the old ChooseInsertionStrategy code)
	// Now determine where best to insert the sphere along the recorded path, minimizing summed node volume growth
	// (if the sphere is much bigger than the leaf it was paired with, we'll insert it higher up the tree).
	float minCost = FLT_MAX;
	for ( int i = 0; i < path.Count(); i++ )
	{
		NodeRef &sibling = Ref( path[ i ] );
		// NOTE: Can't rebalance the tree if you pair with a depth 3+ sibling
		if ( ( sibling->maxDepth <= 2 ) && ( ComputePairingCost2( sibling, bounds, path ) < minCost ) )
			bestSibling = i;
	} */

	// Insert alongside the chosen sibling - duplicate the sibling and add this copy plus the new node to the original sibling:
	NodeRef sibling    = Ref( path[ bestSibling ] );
	NodeRef newSibling = CopyNode( sibling );
	NodeRef newNode    = NewNode( pData, bounds );
	SetNodeChildren( sibling, newNode, newSibling );
	RebalanceSubtrees( sibling, true );

	// Remember the last node we inserted
	m_PrevInsertedNode = newNode.Index();

	for ( int i = ( bestSibling + 1 ); i < path.Count(); i++ )
	{
		// Update each node's status whose descendants were modified, and rebalance it:
		NodeRef &sibling = Ref( path[ i ] );
		SetNodeChildren( sibling, sibling.Deep(), sibling.Shallow() );
		RebalanceSubtrees( sibling, true );
	}

	//ParanoidBalanceCheck_R( Ref( 0 ) ); // Use this to help debug Insert/Remove issues
}

inline void CUtlSphereTree::FindPathToClosestLeaf_R( NodeRef node, const Vector &pos, const float nodeDist, float &minDist, CUtlVector< int > &path ) const
{
	float prevMinDist = minDist;

	if ( node.IsLeaf() )
	{
// TODO: should we use radius here? pos could be more 'central' in a larger sphere, while being closer to the centre of a smaller sphere...
		if ( nodeDist >= minDist )
			return;
		// New closest leaf! Start the path again:
		path.RemoveAll();
		minDist = nodeDist;
	}
	else
	{
		float deepDist    = ( node.Deep()->bounds.AsVector3D()    - pos ).Length();
		float shallowDist = ( node.Shallow()->bounds.AsVector3D() - pos ).Length();
		if ( ( deepDist     - node.Deep()->bounds.w    ) < minDist )
			FindPathToClosestLeaf_R( node.Deep(),    pos, deepDist,    minDist, path );
		if ( ( shallowDist  - node.Shallow()->bounds.w ) < minDist )
			FindPathToClosestLeaf_R( node.Shallow(), pos, shallowDist, minDist, path );
	}

	if ( minDist < prevMinDist )
	{
		// If minDist improved, we're on the path to the closest leaf, so add ourself to the path:
		path.AddToTail( node.Index() );
	}
}

inline void CUtlSphereTree::Remove( const void *pData, const Sphere_t *bounds )
{
	Assert( m_NumNodesInTree );
	if ( !m_NumNodesInTree )
		return; // Empty tree

	NodeRef root = Ref( 0 );
	if ( m_NumNodesInTree == 1 )
	{
		// Special case: the last node to remove is the root, making the tree empty
		Assert( NodeHasData( root, pData, bounds ) );
		if ( NodeHasData( root, pData, bounds ) )
			FreelistPush( root );
		return;
	}

	// Recurse to find the target node and remove it
	int deletedNode;
	deletedNode = Remove_R( root, pData, bounds );
	Assert( deletedNode != 0 );
	//ParanoidBalanceCheck_R( root ); // Use this to help debug Insert/Remove issues
}

inline int CUtlSphereTree::Remove_R( NodeRef &node, const void *pData, const Sphere_t *bounds )
{
	// NOTE: it's ok to cache deepChild+shallowChild refs here because no items are removed from m_Nodes during this recursion
	NodeRef deepChild = node.Deep(), shallowChild = node.Shallow();

	// Is the data owned by one of our direct children?
	for ( int i = 0; i < 2; i++ )
	{
		NodeRef &child = ( i ? shallowChild : deepChild ), otherChild = ( i ? deepChild : shallowChild );
		if ( NodeHasData( child, pData, bounds ) )
		{
			// Found our target - replace node with the other child
			*node = *otherChild;
			FreelistPush( child );
			FreelistPush( otherChild );
			return child.Index();
		}
	}

	// Recurse further to find the owner
	int deletedNodeIndex = -1;
	if ( !deepChild.IsLeaf() )
		deletedNodeIndex = Remove_R(   deepChild, pData, bounds );
	if ( ( deletedNodeIndex < 0 ) && !shallowChild.IsLeaf() )
		deletedNodeIndex = Remove_R( shallowChild, pData, bounds );

	if ( deletedNodeIndex >= 0 )
	{
		// One of this node's sub-trees has changed, so update its status and rebalance the tree
		SetNodeChildren( node, deepChild, shallowChild );
		RebalanceSubtrees( node, false );
	}

	return deletedNodeIndex;
}

inline bool CUtlSphereTree::NodeHasData( const NodeRef &node, const void *pData, const Sphere_t *bounds )
{
	// NOTE: this allows for pData being NULL or the same pData value being used with multiple spheres
	//       (though results are undefined if you insert the exact same pData/bounds pair twice)
	return ( ( node->pData == pData ) && ( node->bounds == *bounds ) );
}

inline void CUtlSphereTree::RebalanceSubtrees( NodeRef &node, bool bInserting )
{
	if ( IsSubtreeBalanced( node ) )
		return;

	// ShallowChild needs to be paired with a node from the deepChild subtree in order to rebalance the tree.
	// We can either pair it with a 'nephew' (one of the children of deepChild) or a great-nephew (one of the
	// children of the nephews). Not all choices result in a balanced tree, so we first determine which
	// choices are valid and then we choose the one with the lowest 'cost'.
	NodeRef shallowChild = node.Shallow(), deepChild = node.Deep();
	Assert( IsSubtreeBalanced( deepChild ) );

	// Put the shallower nephew first
	NodeRef nephews[2] = { deepChild.Shallow(), deepChild.Deep() };
	Assert( nephews[1]->maxDepth == ( shallowChild->maxDepth + 1 ) );
	Assert( IsSubtreeBalanced( nephews[0] ) );
	Assert( IsSubtreeBalanced( nephews[1] ) );

	// Put the shallower great-nephew pair first, and within each pair put the shallower great-nephew first
	NodeRef greatNephews[4] = {	nephews[0].Shallow(), nephews[0].Deep(),
								nephews[1].Shallow(), nephews[1].Deep() };
	int nephewMask = 0x3, greatNephewMask = 0xF;
	if ( nephews[0]->maxDepth == nephews[1]->maxDepth )
	{
		Assert( !bInserting );
		Assert( nephews[0]->maxDepth == ( shallowChild->maxDepth + 1 ) );

		// The nephews are both fair game for pairing with shallowChild. Determine which great-nephews are viable:
		for ( int i = 0; i < 2; i++ )
		{
			// If a nephew is uneven, only the shallower great-nephew may be paired with shallowChild
			if ( greatNephews[ i*2 ]->maxDepth < greatNephews[ i*2 + 1 ]->maxDepth )
				greatNephewMask ^= 1 << ( i*2 + 1 );
		}
	}
	else
	{
		Assert( nephews[0]->maxDepth == shallowChild->maxDepth );

		// Only the shallower nephew and the deeper nephew's children may be paired with shallowChild
		nephewMask      = ( 1 << 0 );
		greatNephewMask = ( 1 << 2 ) | ( 1 << 3 );
	}

	// Now use a cost metric to determine the best choice
	float minCost    = FLT_MAX;
	int   bestIndex  = -1;
	bool  nephewBest = true;
	for ( int i = 0; i < 2; i++ )
	{
		if ( nephewMask & (1<<i) )
		{
			float cost = ComputePairingCost( shallowChild, nephews[i], nephews[i^1], NULL );
			if ( cost < minCost )
			{
				minCost   = cost;
				bestIndex = i;
			}
		}
	}
	for ( int i = 0; i < 4; i++ )
	{
		if ( greatNephewMask & (1<<i) )
		{
			int otherNephew      = ( i >> 1 ) ^ 1;
			float cost = ComputePairingCost( shallowChild, greatNephews[i], nephews[otherNephew], &greatNephews[i^1] );
			if ( cost < minCost )
			{
				minCost    = cost;
				bestIndex  = i;
				nephewBest = false;
			}
		}
	}

	if ( nephewBest )
	{
		// Pair shallowChild with one of its nephews (reuse deepChild as their parent), and pull the other nephew up the tree
		SetNodeChildren( deepChild, shallowChild, nephews[ bestIndex ] );
		SetNodeChildren( node,      deepChild,    nephews[ bestIndex ^ 1 ] );
	}
	else
	{
		// Pair shallowChild with one of its great-nephews, and pair the remaining great-nephew with the other nephew
		NodeRef &nephew      = nephews[   bestIndex >> 1 ];
		NodeRef &otherNephew = nephews[ ( bestIndex >> 1 ) ^ 1 ];
		SetNodeChildren( deepChild, otherNephew,  greatNephews[ bestIndex ^ 1 ] );
		SetNodeChildren( nephew,    shallowChild, greatNephews[ bestIndex ] );
		SetNodeChildren( node,      deepChild,    nephew );
	}

	// The tree under 'node' should now be balanced
	Assert( IsSubtreeBalanced( node ) );
	Assert( IsSubtreeBalanced( node.Deep() ) );
	Assert( IsSubtreeBalanced( node.Shallow() ) );
}

inline bool CUtlSphereTree::IsSubtreeBalanced( const NodeRef &node ) const
{
	if ( !node.IsLeaf() )
	{
		Assert( node.Deep()->maxDepth >= node.Shallow()->maxDepth );
		if ( node.Deep()->maxDepth > ( node.Shallow()->maxDepth + 1 ) )
			return false;
	}
	return true;
}

inline float CUtlSphereTree::Cost( const Sphere_t &sphere ) const
{
	// We optimize the tree based on total resulting bounding sphere volume:
	//  - this minimizes average cost in the space occupied by the tree, i.e. the number of tests you have
	//    to perform to satisfy a given query; on entering a sphere, you need to do 2 tests to pick a child,
	//    so total cost is 'two tests' times the total volume of (non-leaf) nodes
	//  - NOTE: this metric works for input spheres which overlap or fully contain each other

	// TODO: Volume is the best criterion for a point test ("which leaves does a point intersect?"),
	//       but area seems better for ray tests and radius for plane tests - are there pathological
	//       cases where these 3 metrics would differ enough to make Insert choose a different pairing?
	//       Should be easy to change this and test perf differences on real data...
	return sphere.w*sphere.w*sphere.w;
}

inline Sphere_t CUtlSphereTree::AddSpheres( const Sphere_t &pA, const Sphere_t &pB ) const
{
	const Vector &aPos = pA.AsVector3D(), &bPos = pB.AsVector3D();
	float         aRad = pA.w,             bRad = pB.w;

	Vector delta = aPos - bPos;
	float  dist  = delta.NormalizeInPlace();

	if ( dist < fabsf( aRad - bRad ) )
	{
		// One sphere entirely contains the other
		return ( ( aRad > bRad ) ? pA : pB );
	}

	// Figure out the new sphere's centre+radius, using 'mathematics'
	float  newRad = ( aRad + bRad + dist ) / 2;
	Vector newPos = bPos + delta*( newRad - bRad );

	return Sphere_t( newPos.x, newPos.y, newPos.z, newRad );
}

inline float CUtlSphereTree::ComputePairingCost( const NodeRef &pair1A, const NodeRef &pair1B, const NodeRef &pair2A, const NodeRef *pair2B ) const
{
	Sphere_t pair1Sphere = AddSpheres( pair1A->bounds, pair1B->bounds );
	Sphere_t pair2Sphere = pair2B ? AddSpheres( pair2A->bounds, (*pair2B)->bounds ) : pair2A->bounds;
	Sphere_t outerSphere = AddSpheres( pair1Sphere, pair2Sphere );
	return ( Cost( pair1Sphere ) + Cost( pair2Sphere ) + Cost( outerSphere ) );
}

inline void CUtlSphereTree::ParanoidBalanceCheck_R( const NodeRef &node ) const
{
	if ( node.IsLeaf() )
		return;
	Assert( IsSubtreeBalanced( node ) );
	ParanoidBalanceCheck_R( node.Deep() );
	ParanoidBalanceCheck_R( node.Shallow() );
}

inline void CUtlSphereTree::GetLeavesUnderNode_R( const NodeRef &node, CUtlVector< void * > &leaves, int &count ) const
{
	if ( node.IsLeaf() )
	{
		if ( count > 0 )
			leaves.AddToTail( (void *)node->pData );
		count--;
	}
	else
	{
		GetLeavesUnderNode_R( node.Deep(),    leaves, count );
		GetLeavesUnderNode_R( node.Shallow(), leaves, count );
	}
}

inline int CUtlSphereTree::IntersectWithRay( Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int maxResults, CUtlSphereTree::Cut *cut ) const
{
	// Returns a list of the indices of the (leaf) nodes intersecting the given ray
	// TODO: optionally return the spheres in intersection order, from rayStart to (rayStart+rayDelta)

	result.RemoveAll();
	if ( !m_NumNodesInTree )
		return 0; // Empty tree

	// Default to searching the whole tree
	Cut completeCut( this );
	if ( cut == NULL )
		cut = &completeCut;

	Assert( cut->m_pTree == this );
	if ( cut->m_pTree != this )
		return 0;

	maxResults = maxResults ? maxResults : 0x7FFFFFFF;
	int count  = maxResults;
	for ( int i = 0; i < cut->m_NodeRefs.Count(); i++ )
	{
		IntersectWithRay_R( Ref( cut->m_NodeRefs[ i ] ), rayStart, rayDelta, result, count );
	}
	return ( maxResults - count );
}

// extern int gNumCalls; // Useful for accurately measuring the cost of intersection tests (average num calls should be a low multiple of average tree depth)

inline void CUtlSphereTree::IntersectWithRay_R( const NodeRef &node, Vector &rayStart, Vector &rayDelta, CUtlVector< void * > &result, int & count ) const
{
// TODO: pull the intersection test outside the recursive call, as is done in IntersectWithSphere_R
	//gNumCalls++;

	float hit1, hit2;
	Sphere_t &sphere = node->bounds;
	// TODO: In cases where a node overlaps heavily with its children, might it be a win to skip this test
	//       and assume it passes? (i.e. recurse to the children and test their (similar) bounds instead)
	//       Might be a clearer win in a SIMD-ified version of the tree (where we can simply omit this node)
	if ( !IntersectRayWithSphere( rayStart, rayDelta, sphere.AsVector3D(), sphere.w, &hit1, &hit2 ) )
	{
		// No intersection
		return;
	}

	if ( node.IsLeaf() )
	{
		// Leaf node, add it to the result
		if ( count > 0 )
			result.AddToTail( (void *)node->pData );
		count--;
	}
	else
	{
		// Non-leaf, recurse to child nodes
		IntersectWithRay_R( node.Deep(),    rayStart, rayDelta, result, count );
		IntersectWithRay_R( node.Shallow(), rayStart, rayDelta, result, count );
	}
}

inline int CUtlSphereTree::IntersectWithSphere( const Sphere_t &sphere, bool bPartial, CUtlVector< void * > &result, int maxResults, CUtlSphereTree::Cut *cut ) const
{
	// Returns a list of the indices of the (leaf) nodes intersecting the given sphere
	// TODO: optionally return the spheres in order of distance from the sphere centre

	result.RemoveAll();
	if ( !m_NumNodesInTree )
		return 0; // Empty tree

	// Default to searching the whole tree
	Cut completeCut( this );
	if ( cut == NULL )
		cut = &completeCut;

	Assert( cut->m_pTree == this );
	if ( cut->m_pTree != this )
		return 0;

	maxResults = maxResults ? maxResults : 0x7FFFFFFF;
	int count  = maxResults;
	float distSq;
	for ( int i = 0; i < cut->m_NodeRefs.Count(); i++ )
	{
		if ( SpheresIntersect( sphere, Ref( cut->m_NodeRefs[ i ] )->bounds, distSq ) )
			IntersectWithSphere_R( Ref( cut->m_NodeRefs[ i ] ), sphere, bPartial, result, count, distSq );
	}
	return ( maxResults - count );
}

inline void CUtlSphereTree::IntersectWithSphere_R( const NodeRef &node, const Sphere_t &sphere, const bool bPartial, CUtlVector< void * > &result, int &count, const float distSq ) const
{
	//gNumCalls++;
	if ( node.IsLeaf() )
	{
		// Leaf node
		if ( !bPartial )
		{
			// sphere must fully contain bounds
			Sphere_t &bounds = node->bounds;
			if ( sphere.w < bounds.w )
				return;
			float maxDist = sphere.w - bounds.w;
			if ( distSq > ( maxDist*maxDist ) )
				return;
		}
		// Add it to the result
		if ( count > 0 )
			result.AddToTail( (void *)node->pData );
		count--;
	}
	else
	{
		// Non-leaf, recurse to child nodes (do the sphere test here to minimize function calls)
		// TODO: In cases where a node overlaps heavily with its children, might it be a win to skip this test
		//       and assume it passes? (i.e. recurse to the children and test their (similar) bounds instead)
		//       Might be a clearer win in a SIMD-ified version of the tree (where we can simply omit the parent node)
		float deepDistSq, shallowDistSq;
		if ( SpheresIntersect( sphere, node.Deep()->bounds, deepDistSq ) )
			IntersectWithSphere_R( node.Deep(), sphere, bPartial, result, count, deepDistSq );
		if ( SpheresIntersect( sphere, node.Shallow()->bounds, shallowDistSq ) )
			IntersectWithSphere_R( node.Shallow(), sphere, bPartial, result, count, shallowDistSq );
	}
}

inline bool CUtlSphereTree::SpheresIntersect( const Sphere_t &A, const Sphere_t &B, float &distSq ) const
{
	Vector delta     = A.AsVector3D() - B.AsVector3D();
	float  radiusSum = A.w + B.w;
	distSq           = delta.LengthSqr();
	return ( distSq <= ( radiusSum*radiusSum ) );
}

inline void CUtlSphereTree::CutByPlanes( Cut *outputCut, const Cut *inputCut, int cutFlags, Plane_t *planes, int numPlanes ) const
{
	// TODO: might be faster if this code called one of 4 variants of CutByPlanes_R (i.e. templatize to make 'cutFlags' a compile-time constant)

	if ( !m_NumNodesInTree )
		return; // Empty tree

	Assert( outputCut );
	if ( !outputCut )
		return;

	// Default to searching the whole tree
	Cut completeCut( this );
	if ( inputCut == NULL )
		inputCut = &completeCut;

	if ( inputCut == outputCut )
	{
		// Copy the input/output Cut's contents to a temporary Cut so we don't have to special-case much code
		// TODO: it would be quicker to modify the input Cut in-place (given that node order is not important,
		//       you can process nodes in order, addtotail new items and replace removed items with the last entry)
		completeCut.m_NodeRefs.RemoveAll();
		completeCut.m_NodeRefs.AddMultipleToTail( inputCut->m_NodeRefs.Count(), inputCut->m_NodeRefs.Base() );
		inputCut = &completeCut;
	}

	// Clear the output before we start
	outputCut->m_NodeRefs.RemoveAll();

	for ( int i = 0; i < inputCut->m_NodeRefs.Count(); i++ )
	{
		CutByPlanes_R( outputCut, Ref( inputCut->m_NodeRefs[ i ] ), cutFlags, planes, numPlanes );
	}
}

inline void CUtlSphereTree::CutByPlanes_R( Cut *outputCut, const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const
{
	// When refining a Cut w.r.t a set of intersection primitives (here planes), there are two flags to control how this is done:
	//   PARTIAL_INTERSECTIONS  -  nodes partially passing the intersection test (partially in front of the plane) are included
	//                             these are only leaf nodes; non-leaf nodes are subdivided if found to be 'partial' (since we do
	//                             not want both a node and its parent to be present in the result)
	//   OR_INTERSECTIONS       -  in 'OR' mode, a given node needs to pass the intersection test for ANY intersection primitive
	//                          -  in 'AND' mode (the default), it must pass the intersection test for ALL intersection primitives
	//                             (for planes, AND is useful to find 'nodes INSIDE a region' and OR to find 'nodes OUTSIDE a region')
	//
	// When recursing, we follow these rules for each node:
	//   AND:
	//    - fail any plane  -> FAIL, end recursion
	//    - pass all planes -> PASS, add node
	//    - otherwise (partial for any planes)
	//     o non-leaf -> recurse to children
	//     o leaf -> add node if allow partial
	//  OR:
	//    - fail all planes -> FAIL, end recursion
	//    - pass any planes -> PASS, add node
	//    - partial for any planes (fail for others)
	//     o non-leaf -> recurse to children
	//     o leaf -> add node if allow partial

	Cut::CutTestResult result = CutByPlanes_Test( node, cutFlags, planes, numPlanes );
	
	switch( result )
	{
	case Cut::CUT_TEST_PASS:
		// The node fully passes the test, add it to the output Cut
		outputCut->m_NodeRefs.AddToTail( node.Index() );
		return;

	case Cut::CUT_TEST_FAIL:
		// The node fully fails the test, stop recursion
		return;

	case Cut::CUT_TEST_PARTIAL:
		// The node partially passes the test
		if ( node.IsLeaf() )
		{
			// Add to the result if partial intersections are desired
			if ( cutFlags & Cut::PARTIAL_INTERSECTIONS )
				outputCut->m_NodeRefs.AddToTail( node.Index() );
		}
		else
		{
			// Recurse to the child nodes (since we can't know yet whether they are pass, fail or partial)
			CutByPlanes_R( outputCut, node.Deep(),    cutFlags, planes, numPlanes );
			CutByPlanes_R( outputCut, node.Shallow(), cutFlags, planes, numPlanes );
		}
		return;
	}
}

inline CUtlSphereTree::Cut::CutTestResult CUtlSphereTree::CutByPlanes_Test( const NodeRef &node, int cutFlags, Plane_t *planes, int numPlanes ) const
{
	Vector &centre = node->bounds.AsVector3D();
	float   radius = node->bounds.w;

	int numPass = 0, numFail = 0, numPartial = 0;
	for ( int i = 0; i < numPlanes; i++ )
	{
		float dist = centre.Dot( planes[ i ].AsVector3D() ) - planes[ i ].w;
		if ( dist >= radius )
		{
			// Node is fully in front
			if ( cutFlags & Cut::OR_INTERSECTIONS )
				return Cut::CUT_TEST_PASS; // A single pass is enough for 'OR' mode
			numPass++;
		}
		else if ( dist < -radius )
		{
			// Node is fully behind
			if ( !( cutFlags & Cut::OR_INTERSECTIONS ) )
				return Cut::CUT_TEST_FAIL; // A single fail is enough for 'AND' mode
			numFail++;
		}
		else
		{
			// Node is partially in front
			numPartial++;
		}
	}

	if ( cutFlags & Cut::OR_INTERSECTIONS ) // 'OR' mode
	{
		Assert( numPass == 0 ); // Should have earlied-out above, otherwise
		return ( numPartial == 0 ) ? Cut::CUT_TEST_FAIL : Cut::CUT_TEST_PARTIAL;
	}
	else // 'AND' mode
	{
		Assert( numFail == 0 ); // Should have earlied-out above, otherwise
		return ( numPass == numPlanes ) ? Cut::CUT_TEST_PASS : Cut::CUT_TEST_PARTIAL;
	}
}

inline void CUtlSphereTree::MeasureTreeQuality( float &averageDepth, float &idealDepth, float &averageRedundancy, float &averageSphereTestCost, float sphereTestRadius ) const
{
	int numLeaves = ( m_NumNodesInTree + 1 ) / 2;
	float pathRedundancy = 0;
	averageRedundancy = 0;
	averageDepth = 0;
	float pathProbability = 1;
	averageSphereTestCost = 0;
	int steps = 0;

	// Measure depth and redundancy for all paths through the tree:
	MeasureTreeQuality_R( Ref( 0 ), steps, averageDepth, averageRedundancy, pathRedundancy, averageSphereTestCost, pathProbability, sphereTestRadius );

	// The more efficient a perfectly tree, the closer its averageRedundancy to 0.0 (1.0 is the worst case, 0.5 is normal)
	// NOTE: parent-child redundancy correlates well with sibling overlap
	averageRedundancy /= MAX( 1, numLeaves );

	// For a well-balanced tree, averageDepth ~= idealDepth
	averageDepth /= MAX( 1, numLeaves );
	// Compute idealDepth for comparison:
	idealDepth				= ( log( m_NumNodesInTree + 1.0f ) / log( 2.0f ) ) - 1;
	int baseDepth			= (int)floor( idealDepth );
	int numFullTreeNodes	= ( 1 << baseDepth )*2 - 1;
	int numHighDepthLeaves	= m_NumNodesInTree - numFullTreeNodes;
	int numLowDepthLeaves	= numLeaves - numHighDepthLeaves;
	idealDepth				= ( numLowDepthLeaves*baseDepth + numHighDepthLeaves*( baseDepth + 1 ) ) / (float)numLeaves;
}

inline void CUtlSphereTree::MeasureTreeQuality_R( const NodeRef &node, const int steps, float &averageDepth, float &averageRedundancy, const float pathRedundancy, float &averageSphereTestCost, const float pathProbability, const float sphereTestRadius ) const
{
	averageSphereTestCost += pathProbability;
	if ( !node.IsLeaf() )
	{
		float deepHitProbability = ComputeIntersectionProbability( node, node.Deep(), sphereTestRadius ), shallowHitProbability = ComputeIntersectionProbability( node, node.Shallow(), sphereTestRadius );
		MeasureTreeQuality_R( node.Deep(),    steps+1, averageDepth, averageRedundancy, ( pathRedundancy + ComputeRedundancy( node, node.Deep() )    ), averageSphereTestCost, pathProbability*deepHitProbability,    sphereTestRadius );
		MeasureTreeQuality_R( node.Shallow(), steps+1, averageDepth, averageRedundancy, ( pathRedundancy + ComputeRedundancy( node, node.Shallow() ) ), averageSphereTestCost, pathProbability*shallowHitProbability, sphereTestRadius );
	}
	else
	{
		// Leaf
		averageDepth      += steps;
		averageRedundancy += pathRedundancy / steps;
	}
}

inline float CUtlSphereTree::ComputeRedundancy( const NodeRef &parent, const NodeRef &child ) const
{
	const Sphere_t &parentSphere = parent->bounds, &childSphere = child->bounds;
	if ( parentSphere.w == 0 )
		return 1;
	return ( childSphere.w*childSphere.w*childSphere.w ) / ( parentSphere.w*parentSphere.w*parentSphere.w );
}

inline float CUtlSphereTree::ComputeIntersectionProbability( const NodeRef &parent, const NodeRef &child, const float sphereTestRadius ) const
{
	// The probability of a test sphere intersecting a parent's child sphere, GIVEN that it has intersected the parent sphere:
	// [NOTE: this math assumes that the child sphere touches the parent sphere's boundary, which is always true for CUtlSphereTree]
	float totalRadius = ( parent->bounds.w + sphereTestRadius ), hitRadius = ( child->bounds.w + sphereTestRadius );
	if ( totalRadius == 0 )
		return 1;
	return ( hitRadius*hitRadius*hitRadius ) / ( totalRadius*totalRadius*totalRadius );
}



//  =====  CUtlSphereTree_Cut implementation  =================================

inline CUtlSphereTree::Cut::Cut( const CUtlSphereTree *pTree )
{
	// Initialize a Cut with an entire sphere tree (i.e. the root node)
	Assert( pTree );
	m_pTree = pTree;
	if ( pTree->m_NumNodesInTree )
		m_NodeRefs.AddToTail( 0 );
}

inline CUtlSphereTree::Cut::~Cut( void )
{
}

inline int CUtlSphereTree::Cut::GetLeaves( CUtlVector< void * > &leaves, int maxResults ) const
{
	leaves.RemoveAll();
	maxResults = maxResults ? maxResults : 0x7FFFFFFF;
	int count  = maxResults;
	for ( int i = 0; i < m_NodeRefs.Count(); i++ )
	{
		m_pTree->GetLeavesUnderNode_R( m_pTree->Ref( m_NodeRefs[ i ] ), leaves, count );
	}
	return ( maxResults - count );
}

#endif // _utlspheretree_H_