/**
  @file KDTree.h
  
  @maintainer Morgan McGuire, http://graphics.cs.williams.edu
 
  @created 2004-01-11
  @edited  2009-12-28

  Copyright 2000-2009, Morgan McGuire.
  All rights reserved.  
  */

#ifndef G3D_KDTree_h
#define G3D_KDTree_h

#include "G3D/platform.h"
#include "G3D/Array.h"
#include "G3D/Table.h"
#include "G3D/Vector2.h"
#include "G3D/Vector3.h"
#include "G3D/Vector4.h"
#include "G3D/AABox.h"
#include "G3D/Sphere.h"
#include "G3D/Box.h"
#include "G3D/Triangle.h"
#include "G3D/Ray.h"
#include "G3D/Frustum.h"
#include "G3D/BinaryInput.h"
#include "G3D/BinaryOutput.h"
#include "G3D/CollisionDetection.h"
#include "G3D/BoundsTrait.h"
#include <algorithm>

// If defined, in debug mode the tree is checked for consistency
// as a way of detecting corruption due to implementation bugs
// #define VERIFY_TREE

template<> struct BoundsTrait<class G3D::Vector2> {
    static void getBounds(const G3D::Vector2& v, G3D::AABox& out) { out = G3D::AABox(G3D::Vector3(v, 0)); }
};

template<> struct BoundsTrait<class G3D::Vector3> {
    static void getBounds(const G3D::Vector3& v, G3D::AABox& out) { out = G3D::AABox(v); }
};

template<> struct BoundsTrait<class G3D::Vector4> {
    static void getBounds(const G3D::Vector4& v, G3D::AABox& out) { out = G3D::AABox(v.xyz()); }
};

template<> struct BoundsTrait<class G3D::AABox> {
    static void getBounds(const G3D::AABox& v, G3D::AABox& out) { out = v; }
};

template<> struct BoundsTrait<class G3D::Sphere> {
    static void getBounds(const G3D::Sphere& s, G3D::AABox& out) { s.getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Box> {
    static void getBounds(const G3D::Box& b, G3D::AABox& out) { b.getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Vector2*> {
    static void getBounds(const G3D::Vector2*& v, G3D::AABox& out) { out = G3D::AABox(G3D::Vector3(*v, 0)); }
};

template<> struct BoundsTrait<class G3D::Vector3*> {
    static void getBounds(const G3D::Vector3*& v, G3D::AABox& out) { out = G3D::AABox(*v); }
};

template<> struct BoundsTrait<class G3D::Vector4*> {
    static void getBounds(const G3D::Vector4*& v, G3D::AABox& out) { out = G3D::AABox(v->xyz()); }
};

template<> struct BoundsTrait<class G3D::AABox*> {
    static void getBounds(const G3D::AABox*& v, G3D::AABox& out) { out = *v; }
};

template<> struct BoundsTrait<class G3D::Sphere*> {
    static void getBounds(const G3D::Sphere*& s, G3D::AABox& out) { s->getBounds(out); }
};

template<> struct BoundsTrait<class G3D::Box*> {
    static void getBounds(const G3D::Box*& b, G3D::AABox& out) { b->getBounds(out); }
};


template<> struct BoundsTrait<class G3D::Triangle*> {
    static void getBounds(const G3D::Triangle*& t, G3D::AABox& out) { t->getBounds(out); }
};

namespace G3D {
    namespace _internal {

    /**
     Wraps a pointer value so that it can be treated as the instance itself;
     convenient for inserting pointers into a Table but using the 
     object equality instead of pointer equality.
    */
    template<class Type>
    class Indirector {
    public:
        Type*         handle;

        inline Indirector(Type* h) : handle(h) {}

        inline Indirector() : handle(NULL) {}

        /** Returns true iff the values referenced by the handles are equivalent. */
        inline bool operator==(const Indirector& m) const {
            return *handle == *(m.handle);
        }

        inline bool operator==(const Type& m) const {
            return *handle == m;
        }

        inline size_t hashCode() const {
            return handle->hashCode();
        }
    };
 } // namespace internal
} // namespace G3D

template <class Handle> struct HashTrait<typename G3D::_internal::Indirector<Handle> > {
    static size_t hashCode(const G3D::_internal::Indirector<Handle>& key) { return key.hashCode(); }
};

namespace G3D {

/**
 A set that supports spatial queries using a KD tree (axis-aligned
 BSP tree) for speed.

 KDTree allows you to quickly find objects in 3D that lie within
 a box or along a ray. For large sets of objects it is much faster
 than testing each object for a collision.

 KDTree is as powerful as but more general than a Quad Tree, Oct
 Tree, or regular KD tree that cycles through axes, but less general than an unconstrained BSP tree
 (which is much slower to create).
 
 Internally, objects
 are arranged into a tree according to their 
 axis-aligned bounds.  This increases the cost of insertion to
 O(log n) but allows fast overlap queries.

 <B>Template Parameters</B>
 <DT>The template parameter <I>T</I> must be one for which
 the following functions are all overloaded:

 <pre>
  T::T(); // public constructor of no arguments
  template <> struct HashTrait<T> { static size_t hashCode(int key); };
  template<> struct BoundsTrait<T> { static void getBounds(const T& obj, G3D::AABox& out); };
 </pre>

 G3D provides these for common classes like G3D::Vector3 and G3D::Sphere.
 If you use a custom class, or a pointer to a custom class, you will need
 to define those functions.

 <B>Moving %Set Members</B>
 <DT>It is important that objects do not move without updating the
 KDTree.  If the axis-aligned bounds of an object are about
 to change, KDTree::remove it before they change and 
 KDTree::insert it again afterward.  For objects
 where the hashCode and == operator are invariant with respect 
 to the 3D position,
 you can use the KDTree::update method as a shortcut to
 insert/remove an object in one step after it has moved.
 

 Note: Do not mutate any value once it has been inserted into KDTree. Values
 are copied interally. All KDTree iterators convert to pointers to constant
 values to reinforce this.

 If you want to mutate the objects you intend to store in a KDTree
 simply insert <I>pointers</I> to your objects instead of the objects
 themselves, and ensure that the above operations are defined. (And
 actually, because values are copied, if your values are large you may
 want to insert pointers anyway, to save space and make the balance
 operation faster.)

 <B>Dimensions</B>
 Although designed as a 3D-data structure, you can use the KDTree
 for data distributed along 2 or 1 axes by simply returning bounds
 that are always zero along one or more dimensions.

*/
template< class T, 
          class BoundsFunc = BoundsTrait<T>, 
          class HashFunc   = HashTrait<T>, 
          class EqualsFunc = EqualsTrait<T> > 
class KDTree {
protected:
#define TreeType KDTree<T, BoundsFunc, HashFunc, EqualsFunc>

    /** Wrapper for a value that includes a cache of its bounds. 
        Except for the test value used in a set-query operation, there
        is only ever one instance of the handle associated with any 
        value and the memberTable and Nodes maintain pointers to that
        heap-allocated value.
     */
    class Handle {
    public:
        /** The bounds of each object are constrained to AABox::large */
        AABox               bounds;

        /** Center of bounds.  We cache this value to avoid recomputing it
            during the median sort, and because MSVC 6 std::sort goes into
            an infinite loop if we compute the midpoint on the fly (possibly
            a floating point roundoff issue, where B<A and A<B both are true).*/
        Vector3             center;

        T                   value;

        Handle() {}

        inline Handle(const T& v) : value(v) {
            BoundsFunc::getBounds(v, bounds);
            bounds = bounds.intersect(AABox::large());
            center = bounds.center();
        }

        inline bool operator==(const Handle& other) const {
            return EqualsFunc::equals(value, other.value);
        }

        inline size_t hashCode() const {
            return HashFunc::hashCode(value);
        }
    };

    /** Returns the bounds of the sub array. Used by makeNode. */
    static AABox computeBounds(
        const Array<Handle*>& point, 
        int                   beginIndex,
        int                   endIndex) {
    
        Vector3 lo = Vector3::inf();
        Vector3 hi = -lo;

        debugAssertM(beginIndex <= endIndex, "No points");
        for (int p = beginIndex; p <= endIndex; ++p) {
            // This code is written with the vector min and max expanded
            // because otherwise it compiles incorrectly with -O3 on
            // gcc 3.4 

            const Vector3& pLo = point[p]->bounds.low();
            const Vector3& pHi = point[p]->bounds.high();
            for (int a = 0; a < 3; ++a) {
                lo[a] = G3D::min(lo[a], pLo[a]);
                hi[a] = G3D::max(hi[a], pHi[a]);
            }
        }

        return AABox(lo, hi);
    }

    /** Compares centers */
    class CenterComparator {
    public:
        Vector3::Axis sortAxis;

        CenterComparator(Vector3::Axis a) : sortAxis(a) {}

        inline int operator()(Handle* A, const Handle* B) const {
            float a = A->center[sortAxis];
            float b = B->center[sortAxis];

            if (a < b) {
                return 1;
            } else if (a > b) {
                return -1;
            } else {
                return 0;
            }
        }
    };


    /** Compares bounds for strict >, <, or overlap*/
    class BoundsComparator {
    public:
        Vector3::Axis sortAxis;

        BoundsComparator(Vector3::Axis a) : sortAxis(a) {}

        inline int operator()(Handle* A, const Handle* B) const {
            const AABox& a = A->bounds;
            const AABox& b = B->bounds;

            if (a.high()[sortAxis] < b.low()[sortAxis]) {
                return 1;
            } else if (a.low()[sortAxis] > b.high()[sortAxis]) {
                return -1;
            } else {
                return 0;
            }
        }
    };


    /** Compares bounds to the sort location */
    class Comparator {
    public:
        Vector3::Axis sortAxis;
        float sortLocation;

        Comparator(Vector3::Axis a, float l) : sortAxis(a), sortLocation(l) {}

        inline int operator()(Handle* ignore, const Handle* handle) const {
            (void)ignore;
            const AABox& box = handle->bounds;
            debugAssert(ignore == NULL);

            if (box.high()[sortAxis] < sortLocation) {
                // Box is strictly below the sort location
                return -1;
            } else if (box.low()[sortAxis] > sortLocation) {
                // Box is strictly above the sort location
                return 1;
            } else {
                // Box overlaps the sort location
                return 0;
            }
        }
    };

    // Using System::malloc with this class provided no speed improvement.
    class Node {
    public:

        /** Spatial bounds on all values at this node and its children, based purely on
            the parent's splitting planes.  May be infinite. */
        AABox               splitBounds;

        Vector3::Axis       splitAxis;

        /** Location along the specified axis */
        float               splitLocation;
 
        /** child[0] contains all values strictly 
            smaller than splitLocation along splitAxis.

            child[1] contains all values strictly
            larger.

            Both may be NULL if there are not enough
            values to bother recursing.
        */
        Node*               child[2];

        /** Array of values at this node (i.e., values
            straddling the split plane + all values if
            this is a leaf node). 

            This is an array of pointers because that minimizes
            data movement during tree building, which accounts 
            for about 15% of the time cost of tree building.
          */
        Array<Handle*>      valueArray;

        /** For each object in the value array, a copy of its bounds. 
            Packing these into an array at the node level
            instead putting them in the valueArray improves
            cache coherence, which is about a 3x performance
            increase when performing intersection computations.
          */
        Array<AABox>        boundsArray;

        /** Creates node with NULL children */
        Node() {
            splitAxis     = Vector3::X_AXIS;
            splitLocation = 0;
            splitBounds   = AABox(-Vector3::inf(), Vector3::inf());
            for (int i = 0; i < 2; ++i) {
                child[i] = NULL;
            }
        }

        /**
         Doesn't clone children.
         */
        Node(const Node& other) : valueArray(other.valueArray), boundsArray(other.boundsArray) {
            splitAxis       = other.splitAxis;
            splitLocation   = other.splitLocation;
            splitBounds     = other.splitBounds;            
            for (int i = 0; i < 2; ++i) {
                child[i] = NULL;
            }
        }

        /** Copies the specified subarray of pt into point, NULLs the children.
            Assumes a second pass will set splitBounds. */
        Node(const Array<Handle*>& pt) : valueArray(pt) {
            splitAxis     = Vector3::X_AXIS;
            splitLocation = 0;
            for (int i = 0; i < 2; ++i) {
                child[i] = NULL;
            }

            boundsArray.resize(valueArray.size());
            for (int i = 0; i < valueArray.size(); ++i) {
                boundsArray[i] = valueArray[i]->bounds;
            }
        }

        /** Deletes the children (but not the values) */
        ~Node() {
            for (int i = 0; i < 2; ++i) {
                delete child[i];
            }
        }

        /** Returns true if this node is a leaf (no children) */
        inline bool isLeaf() const {
            return (child[0] == NULL) && (child[1] == NULL);
        }


        /**
         Recursively appends all handles and children's handles
         to the array.
         */
        void getHandles(Array<Handle*>& handleArray) const {
            handleArray.append(valueArray);
            for (int i = 0; i < 2; ++i) {
                if (child[i] != NULL) {
                    child[i]->getHandles(handleArray);
                }
            }
        }

        void verifyNode(const Vector3& lo, const Vector3& hi) {
            //        debugPrintf("Verifying: split %d @ %f [%f, %f, %f], [%f, %f, %f]\n",
            //                splitAxis, splitLocation, lo.x, lo.y, lo.z, hi.x, hi.y, hi.z);
            
            debugAssertM(lo == splitBounds.low(),
                         format("lo = %s, splitBounds.lo = %s",
                                lo.toString().c_str(), splitBounds.low().toString().c_str()));
            debugAssert(hi == splitBounds.high());
            
            for (int i = 0; i < valueArray.length(); ++i) {
                const AABox& b = valueArray[i]->bounds;
                debugAssert(b == boundsArray[i]);
                (void)b;
                
                for(int axis = 0; axis < 3; ++axis) {
                    debugAssert(b.low()[axis] <= b.high()[axis]);
                    debugAssert(b.low()[axis] >= lo[axis]);
                    debugAssert(b.high()[axis] <= hi[axis]);
                }
            }
            
            if (child[0] || child[1]) {
                debugAssert(lo[splitAxis] < splitLocation);
                debugAssert(hi[splitAxis] > splitLocation);
            }
            
            Vector3 newLo = lo;
            newLo[splitAxis] = splitLocation;
            Vector3 newHi = hi;
            newHi[splitAxis] = splitLocation;
            
            if (child[0] != NULL) {
                child[0]->verifyNode(lo, newHi);
            }
            
            if (child[1] != NULL) {
                child[1]->verifyNode(newLo, hi);
            }
        }
        

        /**
          Stores the locations of the splitting planes (the structure but not the content)
          so that the tree can be quickly rebuilt from a previous configuration without 
          calling balance.
         */
        static void serializeStructure(const Node* n, BinaryOutput& bo) {
            if (n == NULL) {
                bo.writeUInt8(0);
            } else {
                bo.writeUInt8(1);
                n->splitBounds.serialize(bo);
                serialize(n->splitAxis, bo);
                bo.writeFloat32(n->splitLocation);
                for (int c = 0; c < 2; ++c) {
                    serializeStructure(n->child[c], bo);
                }
            }
        }

        /** Clears the member table */
        static Node* deserializeStructure(BinaryInput& bi) {
            if (bi.readUInt8() == 0) {
                return NULL;
            } else {
                Node* n = new Node();
                n->splitBounds.deserialize(bi);
                deserialize(n->splitAxis, bi);
                n->splitLocation = bi.readFloat32();
                for (int c = 0; c < 2; ++c) {
                    n->child[c] = deserializeStructure(bi);
                }
                return n;
            }
        }

        /** Returns the deepest node that completely contains bounds. */
        Node* findDeepestContainingNode(const AABox& bounds) {

            // See which side of the splitting plane the bounds are on
            if (bounds.high()[splitAxis] < splitLocation) {
                // Bounds are on the low side.  Recurse into the child
                // if it exists.
                if (child[0] != NULL) {
                    return child[0]->findDeepestContainingNode(bounds);
                }
            } else if (bounds.low()[splitAxis] > splitLocation) {
                // Bounds are on the high side, recurse into the child
                // if it exists.
                if (child[1] != NULL) {
                    return child[1]->findDeepestContainingNode(bounds);
                }
            }

            // There was no containing child, so this node is the
            // deepest containing node.
            return this;
        }


        /** Appends all members that intersect the box. 
            If useSphere is true, members that pass the box test
            face a second test against the sphere. */
        void getIntersectingMembers(
            const AABox&        box,
            const Sphere&       sphere,
            Array<T*>&          members,
            bool                useSphere) const {

            // Test all values at this node
            for (int v = 0; v < boundsArray.size(); ++v) {
                const AABox& bounds = boundsArray[v];
                if (bounds.intersects(box) &&
                    (! useSphere || bounds.intersects(sphere))) {
                    members.append(& (valueArray[v]->value));
                }
            }

            // If the left child overlaps the box, recurse into it
            if ((child[0] != NULL) && (box.low()[splitAxis] < splitLocation)) {
                child[0]->getIntersectingMembers(box, sphere, members, useSphere);
            }

            // If the right child overlaps the box, recurse into it
            if ((child[1] != NULL) && (box.high()[splitAxis] > splitLocation)) {
                child[1]->getIntersectingMembers(box, sphere, members, useSphere);
            }
        }

        /**
         Recurse through the tree, assigning splitBounds fields.
         */
        void assignSplitBounds(const AABox& myBounds) {
            splitBounds = myBounds;

            AABox childBounds[2];
            myBounds.split(splitAxis, splitLocation, childBounds[0], childBounds[1]);

#           if defined(G3D_DEBUG) && defined(VERIFY_TREE)
                // Verify the split
                for (int v = 0; v < boundsArray.size(); ++v) {
                    const AABox& bounds = boundsArray[v];
                    debugAssert(myBounds.contains(bounds));
                }
#           endif

            for (int c = 0; c < 2; ++c) {
                if (child[c]) {
                    child[c]->assignSplitBounds(childBounds[c]);
                }
            }
        }

        /** Returns true if the ray intersects this node */
        bool intersects(const Ray& ray, float distance) const {
            // See if the ray will ever hit this node or its children
            Vector3 location;
            bool alreadyInsideBounds = false;
            bool rayWillHitBounds = 
                CollisionDetection::collisionLocationForMovingPointFixedAABox(
                    ray.origin(), ray.direction(), splitBounds, location, alreadyInsideBounds);
           
            bool canHitThisNode = (alreadyInsideBounds ||                
                (rayWillHitBounds && ((location - ray.origin()).squaredLength() < square(distance))));

            return canHitThisNode;
        }

        template<typename RayCallback>
        void intersectRay(
            const Ray& ray, 
            RayCallback& intersectCallback, 
            float& distance,
            bool intersectCallbackIsFast) const {
            
            if (! intersects(ray, distance)) {
                // The ray doesn't hit this node, so it can't hit the children of the node.
                return;
            }

            // Test for intersection against every object at this node.
            for (int v = 0; v < valueArray.size(); ++v) {        
                bool canHitThisObject = true;

                if (! intersectCallbackIsFast) {
                    // See if
                    Vector3 location;
                    const AABox& bounds = boundsArray[v];
                    bool alreadyInsideBounds = false;
                    bool rayWillHitBounds = 
                        CollisionDetection::collisionLocationForMovingPointFixedAABox(
                            ray.origin(), ray.direction(), bounds, location, alreadyInsideBounds);

                    canHitThisObject = (alreadyInsideBounds ||                
                        (rayWillHitBounds && ((location - ray.origin()).squaredLength() < square(distance))));
                }

                if (canHitThisObject) {
                    // It is possible that this ray hits this object.  Look for the intersection using the
                    // callback.
                    const T& value = valueArray[v]->value;
                    intersectCallback(ray, value, distance);
                }
            }

            // There are three cases to consider next:
            // 
            //  1. the ray can start on one side of the splitting plane and never enter the other,
            //  2. the ray can start on one side and enter the other, and
            //  3. the ray can travel exactly down the splitting plane

            enum {NONE = -1};
            int firstChild = NONE;
            int secondChild = NONE;

            if (ray.origin()[splitAxis] < splitLocation) {
                
                // The ray starts on the small side
                firstChild = 0;

                if (ray.direction()[splitAxis] > 0) {
                    // The ray will eventually reach the other side
                    secondChild = 1;
                }

            } else if (ray.origin()[splitAxis] > splitLocation) {

                // The ray starts on the large side
                firstChild = 1;

                if (ray.direction()[splitAxis] < 0) {
                    secondChild = 0;
                }
            } else {
                // The ray starts on the splitting plane
                if (ray.direction()[splitAxis] < 0) {
                    // ...and goes to the small side
                    firstChild = 0;
                } else if (ray.direction()[splitAxis] > 0) {
                    // ...and goes to the large side
                    firstChild = 1;
                }
            }

            // Test on the side closer to the ray origin.
            if ((firstChild != NONE) && child[firstChild]) {
                child[firstChild]->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
            }

            if (ray.direction()[splitAxis] != 0) {
                // See if there was an intersection before hitting the splitting plane.  
                // If so, there is no need to look on the far side and recursion terminates.
                float distanceToSplittingPlane = (splitLocation - ray.origin()[splitAxis]) / ray.direction()[splitAxis];
                if (distanceToSplittingPlane > distance) {
                    // We aren't going to hit anything else before hitting the splitting plane,
                    // so don't bother looking on the far side of the splitting plane at the other
                    // child.
                    return;
                }
            }

            // Test on the side farther from the ray origin.
            if ((secondChild != NONE) && child[secondChild]) {
                child[secondChild]->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
            }

        }
    };


    /**
     Recursively subdivides the subarray.
    
     Clears the source array as soon as it is no longer needed.

     Call assignSplitBounds() on the root node after making a tree.
     */
    Node* makeNode(
        Array<Handle*>& source, 
        int valuesPerNode, 
        int numMeanSplits,
        Array<Handle*>& temp)  {

        Node* node = NULL;
        
        if (source.size() <= valuesPerNode) {
            // Make a new leaf node
            node = new Node(source);
            
            // Set the pointers in the memberTable
            for (int i = 0; i < source.size(); ++i) {
                memberTable.set(Member(source[i]), node);
            }
            source.clear();
            
        } else {
            // Make a new internal node
            node = new Node();
            
            const AABox& bounds = computeBounds(source, 0, source.size() - 1);
            const Vector3& extent = bounds.high() - bounds.low();
            
            Vector3::Axis splitAxis = extent.primaryAxis();
            
            float splitLocation;

            // Arrays for holding the children
            Array<Handle*> lt, gt;
                
            if (numMeanSplits <= 0) {

                source.medianPartition(lt, node->valueArray, gt, temp, CenterComparator(splitAxis));

                // Choose the split location to be the center of whatever fell in the center
                splitLocation = node->valueArray[0]->center[splitAxis];

                // Some of the elements in the lt or gt array might really overlap the split location.
                // Move them as needed.
                for (int i = 0; i < lt.size(); ++i) {
                    const AABox& bounds = lt[i]->bounds;
                    if ((bounds.low()[splitAxis] <= splitLocation) && (bounds.high()[splitAxis] >= splitLocation)) {
                        node->valueArray.append(lt[i]);
                        // Remove this element and process the new one that
                        // is swapped in in its place.
                        lt.fastRemove(i); --i;
                    }
                }

                for (int i = 0; i < gt.size(); ++i) {
                    const AABox& bounds = gt[i]->bounds;
                    if ((bounds.low()[splitAxis] <= splitLocation) && (bounds.high()[splitAxis] >= splitLocation)) {
                        node->valueArray.append(gt[i]);
                        // Remove this element and process the new one that
                        // is swapped in in its place.
                        gt.fastRemove(i); --i;
                    }
                }            

                if ((node->valueArray.size() > (source.size() / 2)) &&
                    (source.size() > 6)) {
                    // This was a bad partition; we ended up putting the splitting plane right in the middle of most of the 
                    // objects.  We could try to split on a different axis, or use a different partition (e.g., the extents mean, 
                    // or geometric mean).  This implementation falls back on the extents mean, since that case is already handled 
                    // below.
                    numMeanSplits = 1;
                }
            }

            // Note: numMeanSplits may have been increased by the code in the previous case above in order to
            // force a re-partition.

            if (numMeanSplits > 0) {
                // Split along the mean
                splitLocation = 
                    bounds.high()[splitAxis] * 0.5f + 
                    bounds.low()[splitAxis] * 0.5f;
                
                debugAssertM(isFinite(splitLocation),
                            "Internal error: split location must be finite.");

                source.partition(NULL, lt, node->valueArray, gt, Comparator(splitAxis, splitLocation));

                // The Comparator ensures that elements are strictly on the correct side of the split
            }


#           if defined(G3D_DEBUG) && defined(VERIFY_TREE)
                debugAssert(lt.size() + node->valueArray.size() + gt.size() == source.size());
                // Verify that all objects ended up on the correct side of the split.
                // (i.e., make sure that the Array partition was correct) 
                for (int i = 0; i < lt.size(); ++i) {
                    const AABox& bounds  = lt[i]->bounds;
                    debugAssert(bounds.high()[splitAxis] < splitLocation);
                }

                for (int i = 0; i < gt.size(); ++i) {
                    const AABox& bounds  = gt[i]->bounds;
                    debugAssert(bounds.low()[splitAxis] > splitLocation);
                }

                for (int i = 0; i < node->valueArray.size(); ++i) {
                    const AABox& bounds  = node->valueArray[i]->bounds;
                    debugAssert(bounds.high()[splitAxis] >= splitLocation);
                    debugAssert(bounds.low()[splitAxis] <= splitLocation);
                }
#           endif

            // The source array is no longer needed
            source.clear();

            node->splitAxis = splitAxis;
            node->splitLocation = splitLocation;

            // Update the bounds array and member table
            node->boundsArray.resize(node->valueArray.size());
            for (int i = 0; i < node->valueArray.size(); ++i) {
                Handle* v = node->valueArray[i];
                node->boundsArray[i] = v->bounds;
                memberTable.set(Member(v), node);
            }

            if (lt.size() > 0) {            
                node->child[0] = makeNode(lt, valuesPerNode, numMeanSplits - 1, temp);
            }
            
            if (gt.size() > 0) {
                node->child[1] = makeNode(gt, valuesPerNode, numMeanSplits - 1, temp);
            }
            
        }
        
        return node;
    }

    /**
     Recursively clone the passed in node tree, setting
     pointers for members in the memberTable as appropriate.
     called by the assignment operator.
     */
    Node* cloneTree(Node* src) {
        Node* dst = new Node(*src);

        // Make back pointers
        for (int i = 0; i < dst->valueArray.size(); ++i) {
            memberTable.set(Member(dst->valueArray[i]), dst);
        }

        // Clone children
        for (int i = 0; i < 2; ++i) {
            if (src->child[i] != NULL) {
                dst->child[i] = cloneTree(src->child[i]);
            }
        }

        return dst;
    }

   /**
    Wrapper for a Handle; used to create a memberTable that acts like Table<Handle, Node*> but
    stores only Handle* internally to avoid memory copies.
    */
    typedef _internal::Indirector<Handle> Member;

    typedef Table<Member, Node*> MemberTable;

    /** Maps members to the node containing them */
    MemberTable             memberTable;

    Node*                   root;

public:

    /** To construct a balanced tree, insert the elements and then call
      KDTree::balance(). */
    KDTree() : root(NULL) {}


    KDTree(const KDTree& src) : root(NULL) {
        *this = src;
    }


    KDTree& operator=(const KDTree& src) {
        delete root;
        // Clone tree takes care of filling out the memberTable.
        root = cloneTree(src.root);
        return *this;
    }


    ~KDTree() {
        clear();
    }

    /**
     Throws out all elements of the set.
     */
    void clear() {
        typedef typename Table<_internal::Indirector<Handle>, Node*>::Iterator It;
  
        // Delete all handles stored in the member table
        It cur = memberTable.begin();
        It end = memberTable.end();
        while (cur != end) {
            delete cur->key.handle;
            cur->key.handle = NULL;
            ++cur;
        }
        memberTable.clear();

        // Delete the tree structure itself
        delete root;
        root = NULL;
    }

    int size() const {
        return memberTable.size();
    }

    /**
     Inserts an object into the set if it is not
     already present.  O(log n) time.  Does not
     cause the tree to be balanced.
     */
    void insert(const T& value) {
        if (contains(value)) {
            // Already in the set
            return;
        }

        Handle* h = new Handle(value);

        if (root == NULL) {
            // This is the first node; create a root node
            root = new Node();
        }

        Node* node = root->findDeepestContainingNode(h->bounds);

        // Insert into the node
        node->valueArray.append(h);
        node->boundsArray.append(h->bounds);
        
        // Insert into the node table
        Member m(h);
        memberTable.set(m, node);
    }

    /** Inserts each elements in the array in turn.  If the tree
        begins empty (no structure and no elements), this is faster
        than inserting each element in turn.  You still need to balance
        the tree at the end.*/
    void insert(const Array<T>& valueArray) {
        if (root == NULL) {
            // Optimized case for an empty tree; don't bother
            // searching or reallocating the root node's valueArray
            // as we incrementally insert.
            root = new Node();
            root->valueArray.resize(valueArray.size());
            root->boundsArray.resize(root->valueArray.size());
            for (int i = 0; i < valueArray.size(); ++i) {
                // Insert in opposite order so that we have the exact same
                // data structure as if we inserted each (i.e., order is reversed
                // from array).
                Handle* h = new Handle(valueArray[i]);
                int j = valueArray.size() - i - 1;
                root->valueArray[j] = h;
                root->boundsArray[j] = h->bounds;
                memberTable.set(Member(h), root);
            }

        } else {
            // Insert at appropriate tree depth.
            for (int i = 0; i < valueArray.size(); ++i) {
                insert(valueArray[i]);
            }
        }
    }


    /**
     Returns true if this object is in the set, otherwise
     returns false.  O(1) time.
     */
    bool contains(const T& value) {
        // Temporarily create a handle and member
        Handle h(value);
        return memberTable.containsKey(Member(&h));
    }


    /**
     Removes an object from the set in O(1) time.
     It is an error to remove members that are not already
     present.  May unbalance the tree.  
     
     Removing an element never causes a node (split plane) to be removed...
     nodes are only changed when the tree is rebalanced.  This behavior
     is desirable because it allows the split planes to be serialized,
     and then deserialized into an empty tree which can be repopulated.
    */
    void remove(const T& value) {
        debugAssertM(contains(value),
            "Tried to remove an element from a "
            "KDTree that was not present");

        // Get the list of elements at the node
        Handle h(value);
        Member m(&h);

        Array<Handle*>& list = memberTable[m]->valueArray;

        Handle* ptr = NULL;

        // Find the element and remove it
        for (int i = list.length() - 1; i >= 0; --i) {
            if (list[i]->value == value) {
                // This was the element.  Grab the pointer so that
                // we can delete it below
                ptr = list[i];

                // Remove the handle from the node
                list.fastRemove(i);

                // Remove the corresponding bounds
                memberTable[m]->boundsArray.fastRemove(i);
                break;
            }
        }

        // Remove the member
        memberTable.remove(m);

        // Delete the handle data structure
        delete ptr;
        ptr = NULL;
    }


    /**
     If the element is in the set, it is removed.
     The element is then inserted.

     This is useful when the == and hashCode methods
     on <I>T</I> are independent of the bounds.  In
     that case, you may call update(v) to insert an
     element for the first time and call update(v)
     again every time it moves to keep the tree 
     up to date.
     */
    void update(const T& value) {
        if (contains(value)) {
            remove(value);
        }
        insert(value);
    }


    /**
     Rebalances the tree (slow).  Call when objects
     have moved substantially from their original positions
     (which unbalances the tree and causes the spatial
     queries to be slow).
     
     @param valuesPerNode Maximum number of elements to put at
     a node.

     @param numMeanSplits numMeanSplits = 0 gives a 
     fully axis aligned BSP-tree, where the balance operation attempts to balance
     the tree so that every splitting plane has an equal number of left
     and right children (i.e. it is a <B>median</B> split along that axis).  
     This tends to maximize average performance.  

     You can override this behavior by
     setting a number of <B>mean</B> (average) splits.  numMeanSplits = MAX_INT
     creates a full oct-tree, which tends to optimize peak performance at the expense of
     average performance.  It tends to have better clustering behavior when
     members are not uniformly distributed.
     */
    void balance(int valuesPerNode = 5, int numMeanSplits = 3) {
        if (root == NULL) {
            // Tree is empty
            return;
        }

        // Get all handles and delete the old tree structure
        Node* oldRoot = root;
        for (int c = 0; c < 2; ++c) {
            if (root->child[c] != NULL) {
                root->child[c]->getHandles(root->valueArray);

                // Delete the child; this will delete all structure below it
                delete root->child[c];
                root->child[c] = NULL;
            }
        }

        Array<Handle*> temp;
        // Make a new root.  Work with a copy of the value array because 
        // makeNode clears the source array as it progresses
        Array<Handle*> copy(oldRoot->valueArray);
        root = makeNode(copy, valuesPerNode, numMeanSplits, temp);

        // Throw away the old root node
        delete oldRoot;
        oldRoot = NULL;

        // Walk the tree, assigning splitBounds.  We start with unbounded
        // space.  This will override the current member table.
        const AABox& LARGE = AABox::large();
        root->assignSplitBounds(LARGE);

#       ifdef _DEBUG
        {
            // Ensure that the balanced tree is still correct
            root->verifyNode(LARGE.low(), LARGE.high());
        }
#       endif
    }


    /** Clear, set the contents to the values in the array, and then balance */
    void setContents(const Array<T>& array, int valuesPerNode = 5, int numMeanSplits = 3) {
        clear();
        insert(array);
        balance(valuesPerNode, numMeanSplits);
    }


protected:

    /**
     @param parentMask The mask that this node returned from culledBy.
     */
    static void getIntersectingMembers(
        const Array<Plane>&         plane,
        Array<T*>&                  members,
        Node*                       node,
        uint32                      parentMask) {

        int dummy;

        if (parentMask == 0) {
            // None of these planes can cull anything
            for (int v = node->valueArray.size() - 1; v >= 0; --v) {
                members.append(& (node->valueArray[v]->value));
            }

            // Iterate through child nodes
            for (int c = 0; c < 2; ++c) {
                if (node->child[c]) {
                    getIntersectingMembers(plane, members, node->child[c], 0);
                }
            }
        } else {

            // Test values at this node against remaining planes
            for (int v = node->boundsArray.size() - 1; v >= 0; --v) {
                if (! node->boundsArray[v].culledBy(plane, dummy, parentMask)) {
                    members.append(&(node->valueArray[v]->value));
                }
            }

            uint32 childMask  = 0xFFFFFF;

            // Iterate through child nodes
            for (int c = 0; c < 2; ++c) {
                if (node->child[c] &&
                    ! node->child[c]->splitBounds.culledBy(plane, dummy, parentMask, childMask)) {
                    // This node was not culled
                    getIntersectingMembers(plane, members, node->child[c], childMask);
                }
            }
        }
    }

public:

    /**
     Returns all members inside the set of planes.  
      @param members The results are appended to this array.
     */
    void getIntersectingMembers(const Array<Plane>& plane, Array<T*>& members) const {
        if (root == NULL) {
            return;
        }

        getIntersectingMembers(plane, members, root, 0xFFFFFF);
    }

    void getIntersectingMembers(const Array<Plane>& plane, Array<T>& members) const {
        Array<T*> temp;
        getIntersectingMembers(plane, temp, root, 0xFFFFFF);
        for (int i = 0; i < temp.size(); ++i) {
            members.append(*temp[i]);
        }
    }

    /**
     Typically used to find all visible
     objects inside the view frustum (see also Camera::getClipPlanes)... i.e. all objects
     <B>not</B> culled by frustum.

     Example:
      <PRE>
        Array<Object*>  visible;
        tree.getIntersectingMembers(camera.frustum(), visible);
        // ... Draw all objects in the visible array.
      </PRE>
      @param members The results are appended to this array.
      */
    void getIntersectingMembers(const Frustum& frustum, Array<T*>& members) const {
        Array<Plane> plane;
        
        for (int i = 0; i < frustum.faceArray.size(); ++i) {
            plane.append(frustum.faceArray[i].plane);
        }

        getIntersectingMembers(plane, members);
    }

    void getIntersectingMembers(const Frustum& frustum, Array<T>& members) const {
        Array<T*> temp;
        getIntersectingMembers(frustum, temp);
        for (int i = 0; i < temp.size(); ++i) {
            members.append(*temp[i]);
        }
    }

    /**
     C++ STL style iterator variable.  See beginBoxIntersection().
     The iterator overloads the -> (dereference) operator, so this
     acts like a pointer to the current member.
     */
    // This iterator turns Node::getIntersectingMembers into a
    // coroutine.  It first translates that method from recursive to
    // stack based, then captures the system state (analogous to a Scheme
    // continuation) after each element is appended to the member array,
    // and allowing the computation to be restarted.
    class BoxIntersectionIterator {
    private:
        friend class TreeType;

        /** True if this is the "end" iterator instance */
        bool            isEnd;

        /** The box that we're testing against. */
        AABox           box;

        /** Node that we're currently looking at.  Undefined if isEnd
            is true. */
        Node*           node;

        /** Nodes waiting to be processed */
        // We could use backpointers within the tree and careful
        // state management to avoid ever storing the stack-- but
        // it is much easier this way and only inefficient if the
        // caller uses post increment (which they shouldn't!).
        Array<Node*>    stack;

        /** The next index of current->valueArray to return. 
            Undefined when isEnd is true.*/
        int             nextValueArrayIndex;

        BoxIntersectionIterator() : isEnd(true) {}
        
        BoxIntersectionIterator(const AABox& b, const Node* root) : 
           isEnd(root == NULL), box(b), 
           node(const_cast<Node*>(root)), nextValueArrayIndex(-1) {

           // We intentionally start at the "-1" index of the current
           // node so we can use the preincrement operator to move
           // ourselves to element 0 instead of repeating all of the
           // code from the preincrement method.  Note that this might
           // cause us to become the "end" instance.
           ++(*this);
        }

    public:

        inline bool operator!=(const BoxIntersectionIterator& other) const {
            return ! (*this == other);
        }

        bool operator==(const BoxIntersectionIterator& other) const {
            if (isEnd) {
                return other.isEnd;
            } else if (other.isEnd) {
                return false;
            } else {
                // Two non-end iterators; see if they match.  This is kind of 
                // silly; users shouldn't call == on iterators in general unless
                // one of them is the end iterator.
                if ((box != other.box) || (node != other.node) || 
                    (nextValueArrayIndex != other.nextValueArrayIndex) ||
                    (stack.length() != other.stack.length())) {
                    return false;
                }

                // See if the stacks are the same
                for (int i = 0; i < stack.length(); ++i) {
                    if (stack[i] != other.stack[i]) {
                        return false;
                    }
                }

                // We failed to find a difference; they must be the same
                return true;
            }
        }

        /**
         Pre increment.
         */
        BoxIntersectionIterator& operator++() {
            ++nextValueArrayIndex;

            bool foundIntersection = false;
            while (! isEnd && ! foundIntersection) {

                // Search for the next node if we've exhausted this one
                while ((! isEnd) &&  (nextValueArrayIndex >= node->valueArray.length())) {
                    // If we entered this loop, then the iterator has exhausted the elements at 
                    // node (possibly because it just switched to a child node with no members).
                    // This loop continues until it finds a node with members or reaches
                    // the end of the whole intersection search.
                    
                    // If the right child overlaps the box, push it onto the stack for
                    // processing.
                    if ((node->child[1] != NULL) &&
                        (box.high()[node->splitAxis] > node->splitLocation)) {
                        stack.push(node->child[1]);
                    }
                    
                    // If the left child overlaps the box, push it onto the stack for
                    // processing.
                    if ((node->child[0] != NULL) &&
                        (box.low()[node->splitAxis] < node->splitLocation)) {
                        stack.push(node->child[0]);
                    }
                    
                    if (stack.length() > 0) {
                        // Go on to the next node (which may be either one of the ones we 
                        // just pushed, or one from farther back the tree).
                        node = stack.pop();
                        nextValueArrayIndex = 0;
                    } else {
                        // That was the last node; we're done iterating
                        isEnd = true;
                    }
                }
                
                // Search for the next intersection at this node until we run out of children
                while (! isEnd && ! foundIntersection && (nextValueArrayIndex < node->valueArray.length())) {
                    if (box.intersects(node->boundsArray[nextValueArrayIndex])) {
                        foundIntersection = true;
                    } else {
                        ++nextValueArrayIndex;
                        // If we exhaust this node, we'll loop around the master loop 
                        // to find a new node.
                    }
                }
            }

            return *this;
        }

    private:
        /**
         Post increment (much slower than preincrement!).
         Intentionally overloaded to preclude accidentally slow code.
         */
        BoxIntersectionIterator operator++(int);
        /*{
            BoxIntersectionIterator old = *this;
            ++this;
            return old;
        }*/

    public:

        /** Overloaded dereference operator so the iterator can masquerade as a pointer
            to a member */
        const T& operator*() const {
            alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
            return node->valueArray[nextValueArrayIndex]->value;
        }

        /** Overloaded dereference operator so the iterator can masquerade as a pointer
            to a member */
        T const * operator->() const {
            alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
            return &(stack.last()->valueArray[nextValueArrayIndex]->value);
        }

        /** Overloaded cast operator so the iterator can masquerade as a pointer
            to a member */
        operator T*() const {
            alwaysAssertM(! isEnd, "Can't dereference the end element of an iterator");
            return &(stack.last()->valueArray[nextValueArrayIndex]->value);
        }
    };


    /**
     Iterates through the members that intersect the box
     */
    BoxIntersectionIterator beginBoxIntersection(const AABox& box) const {
        return BoxIntersectionIterator(box, root);
    }

    BoxIntersectionIterator endBoxIntersection() const {
        // The "end" iterator instance
        return BoxIntersectionIterator();
    }

    /**
     Appends all members whose bounds intersect the box.
     See also KDTree::beginBoxIntersection.
     */
    void getIntersectingMembers(const AABox& box, Array<T*>& members) const {
        if (root == NULL) {
            return;
        }
        root->getIntersectingMembers(box, Sphere(Vector3::zero(), 0), members, false);
    }

    void getIntersectingMembers(const AABox& box, Array<T>& members) const {
        Array<T*> temp;
        getIntersectingMembers(box, temp);
        for (int i = 0; i < temp.size(); ++i) {
            members.append(*temp[i]);
        }
    }


    /**
     Invoke a callback for every member along a ray until the closest intersection is found.

     @param intersectCallback Either a function or an instance of a class with an overloaded operator() of the form:
     <pre>
         void callback(const Ray& ray, const T& object, float& distance).
     </pre>
     If the ray hits the object before travelling distance <code>distance</code>, updates
     <code>distance</code> with the new distance to the intersection, otherwise leaves it
     unmodified.  A common example is:
     \htmlonly
     <pre>
     class Entity {
     public:
         void intersect(const Ray& ray, float& maxDist, Vector3& outLocation, Vector3& outNormal) {
             float d = maxDist;

             // ... search for intersection distance d

             if ((d > 0) && (d < maxDist)) {
                 // Intersection occured
                 maxDist = d;
                 outLocation = ...;
                 outNormal = ...;
             }
         }
     };

     // Finds the surface normal and location of the first intersection with the scene
     class Intersection {
     public:
         Entity*     closestEntity;
         Vector3     hitLocation;
         Vector3     hitNormal;

         void operator()(const Ray& ray, const Entity* entity, float& distance) {
             entity->intersect(ray, distance, hitLocation, hitNormal);
         }
     };

     KDTree<Entity*> scene;

     Intersection intersection;
     float distance = finf();
     scene.intersectRay(camera.worldRay(x, y), intersection, distance);
     </pre>
     \endhtmlonly

     @param distance When the method is invoked, this is the maximum
     distance that the tree should search for an intersection.  On
     return, this is set to the distance to the first intersection
     encountered.

     @param intersectCallbackIsFast If false, each object's bounds are
      tested before the intersectCallback is invoked.  If the
      intersect callback runs at the same speed or faster than
      AABox-ray intersection, set this to true.
     */
    template<typename RayCallback>
    void intersectRay(
        const Ray& ray, 
        RayCallback& intersectCallback, 
        float& distance,
        bool intersectCallbackIsFast = false) const {
        
        root->intersectRay(ray, intersectCallback, distance, intersectCallbackIsFast);
    }


    /**
      @brief Finds all members whose bounding boxes intersect the sphere.  The actual
      elements may not intersect the sphere.

      @param members The results are appended to this array.
     */
    void getIntersectingMembers(const Sphere& sphere, Array<T*>& members) const {
        if (root == NULL) {
            return;
        }

        AABox box;
        sphere.getBounds(box);
        root->getIntersectingMembers(box, sphere, members, true);
    }

    void getIntersectingMembers(const Sphere& sphere, Array<T>& members) const {
        Array<T*> temp;
        getIntersectingMembers(sphere, temp);
        for (int i = 0; i < temp.size(); ++i) {
            members.append(*temp[i]);
        }
    }

    /**
      Stores the locations of the splitting planes (the structure but not the content)
      so that the tree can be quickly rebuilt from a previous configuration without 
      calling balance.
     */
    void serializeStructure(BinaryOutput& bo) const {
        Node::serializeStructure(root, bo);
    }

    /** Clears the member table */
    void deserializeStructure(BinaryInput& bi) {
        clear();
        root = Node::deserializeStructure(bi);
    }

    /**
     Returns an array of all members of the set.  See also KDTree::begin.
     */
    void getMembers(Array<T>& members) const {
        Array<Member> temp;
        memberTable.getKeys(temp);
        for (int i = 0; i < temp.size(); ++i) {
            members.append(*(temp.handle));
        }
    }


    /** If a value that is EqualsFunc to @a value is present, returns a pointer to the 
        version stored in the data structure, otherwise returns NULL.
     */
    const T* getPointer(const T& value) const {
        // Temporarily create a handle and member
        Handle h(value);
        const Member* member = memberTable.getKeyPointer(Member(&h));
        if (member == NULL) {
            // Not found
            return NULL;
        } else {
            return &(member->handle->value);
        }
    }


    /**
     C++ STL style iterator variable.  See begin().
     Overloads the -> (dereference) operator, so this acts like a pointer
     to the current member.
    */
    class Iterator {
    private:
        friend class TreeType;

        // Note: this is a Table iterator, we are currently defining
        // Set iterator
        typename Table<Member, Node*>::Iterator it;

        Iterator(const typename Table<Member, Node*>::Iterator& it) : it(it) {}

    public:

        inline bool operator!=(const Iterator& other) const {
            return !(*this == other);
        }

        bool operator==(const Iterator& other) const {
            return it == other.it;
        }

        /**
         Pre increment.
         */
        Iterator& operator++() {
            ++it;
            return *this;
        }

    private:
        /**
         Post increment (slower than preincrement).  Intentionally unimplemented to prevent slow code.
         */
        Iterator operator++(int);/* {
            Iterator old = *this;
            ++(*this);
            return old;
        }*/
    public:

        const T& operator*() const {
            return it->key.handle->value;
        }

        T* operator->() const {
            return &(it->key.handle->value);
        }

        operator T*() const {
            return &(it->key.handle->value);
        }
    };


    /**
     C++ STL style iterator method.  Returns the first member.  
     Use preincrement (++entry) to get to the next element (iteration
     order is arbitrary).  
     Do not modify the set while iterating.
     */
    Iterator begin() const {
        return Iterator(memberTable.begin());
    }


    /**
     C++ STL style iterator method.  Returns one after the last iterator
     element.
     */
    Iterator end() const {
        return Iterator(memberTable.end());
    }
#undef TreeType
};


}

#endif