#ifndef TREE_H
#define TREE_H

#include <vector>
#include <istream>
#include <ostream>
#include "Math.h"

#ifndef TREE_MAX_POINTS_PER_NODE
    #define TREE_MAX_POINTS_PER_NODE 32
#endif

namespace TREE
{
    /*
    Abstract base class to encapsulate general Tree functions such as
    interpreting and querying of data

    D: data type stored in the tree
    T: dimension type
    */
    template<class D, class T>
    class Basic
    {
    public:
        virtual bool Insert(const D & Data, const T * Point, const unsigned int Mask = ~((unsigned int)(0))) = 0;
        virtual bool Interpolate(const T * Point, const unsigned int Amount, const T MaxDistance, const T AveragePower, D * Result) const = 0;
        virtual bool Intersect(const T * Point, const unsigned int Mask = ~((unsigned int)(0))) = 0;
    };

    /*
    Stores information about points in a spatial tree.
    The tree grows exponentially by the number of dimensions.

    T: dimension type
    Dimensions: number many dimensions
    */
    template <class T, unsigned int Dimensions>
    class Index
    {
    private:

        struct Node
        {
            T              m_clsSplitValue;
            unsigned int * m_ptrIndices;
            unsigned int   m_uintIndices;
            unsigned int   m_uintSplitAxis;
            Node *         m_ptrChildren;

            Node()
                : m_uintIndices(0),
                  m_ptrIndices(NULL),
                  m_ptrChildren(NULL),
                  m_uintSplitAxis((unsigned int)(~0)) {}

            ~Node()
            {
                delete [] m_ptrChildren;
                delete m_ptrIndices;
            }
        };

        MATH::DN::Box<T, Dimensions>     m_stuCorners;
        MATH::DN::Point<T, Dimensions> * m_ptrPoints;
        std::vector<unsigned int>        m_clsBuffer;
        Node                             m_stuRoot;
        unsigned char                    m_uchrAxis;
        unsigned int                     m_uintPoints;
        unsigned int                     m_uintPointsAllocated;
        const float                      m_fltThreshhold;

        void         Insert(Node * Node, const unsigned int PointIndex);
        unsigned int Find(Node * Node, const MATH::DN::Point<T, Dimensions> & PointRef);
        static void  Find(const Node * Node, const MATH::DN::Box<T, Dimensions> * SearchArea, MATH::DN::Box<T, Dimensions> * AreaBox, const MATH::DN::Point<T, Dimensions> * Points, std::vector<unsigned int> & Buffer);
        static void  GetIndices(const Node * Node, unsigned int * Indices);
        static bool  Integrity(const Node * Node, const unsigned int Max);
        static void  Optimize(Node * Node, const MATH::DN::Point<T, Dimensions> * Points, std::vector<unsigned int> & Buffer);
        static void  OptimizeCheck(Node * Node, const float Threshhold, const MATH::DN::Point<T, Dimensions> * Points, std::vector<unsigned int> & Buffer);

    public:

        Index(const float RefitThreshhold = 0.75f)
            : m_ptrPoints(NULL),
              m_uintPoints(0),
              m_uintPointsAllocated(0),
              m_fltThreshhold(( RefitThreshhold >= 0.0f ) ? (( RefitThreshhold <= 1.0f ) ? RefitThreshhold : 1.0f) : 0.0f)
        {
            m_stuCorners.Init();
            m_stuRoot.m_ptrIndices = new unsigned int[TREE_MAX_POINTS_PER_NODE + 1];
        }

        ~Index() {Finish();}

        unsigned int                           Insert(const MATH::DN::Point<T, Dimensions> & Point);
        unsigned int                           Insert(const T * Values);
        void                                   Insert(const MATH::DN::Point<T, Dimensions> * Points, const unsigned * Indices, const unsigned int Amount, unsigned * IndicesSet);
        void                                   Allocate(const unsigned int Amount);
        void                                   AllocateAdd(const unsigned int Amount) {Allocate(m_uintPoints + Amount);}
        void                                   AllocateFix(void);
        void                                   Axis(const unsigned char AxisIndex) {m_uchrAxis = AxisIndex;}
        const MATH::DN::Box<T, Dimensions> &   Bounds(void) const {return m_stuCorners;}
        MATH::DN::Point<T, Dimensions> *       Clear(unsigned int * PointsAmount = NULL);
        unsigned int                           Find(const MATH::DN::Box<T, Dimensions> & SearchArea, std::vector<unsigned int> & Buffer) const;
        void                                   Finish(void);
        bool                                   Integrity(void) const {return Integrity(&m_stuRoot, m_uintPoints);}
        const MATH::DN::Point<T, Dimensions> * Points(unsigned int * Amount = NULL) {if ( Amount ) *Amount = m_uintPoints; return m_ptrPoints;}
        void                                   Optimize(const unsigned char p_uchrFrequency);
        T                                      Value(const int p_intIndex) const {return m_ptrPoints[p_intIndex].m_clsValues[m_uchrAxis];}
        T                                      Value(const int p_intIndex, const unsigned char p_uchrAxis) const {return m_ptrPoints[p_intIndex].m_clsValues[p_uchrAxis];}
    };

    /*
    Stores information about lines in a spatial tree

    T: dimension type
    Dimensions: how many dimensions
    */
    template<class T, unsigned int Dimensions>
    class Line
    {
    public:

        struct Point
        {
            MATH::DN::Point<T, Dimensions> m_stuPoint;
            unsigned int                   m_uintPointPrev;
            unsigned int                   m_uintPointNext;
            bool                           m_blnFlag;
        };

    private:

        Point *       m_ptrPoints;
        unsigned int  m_uintLineIndex;
        unsigned int  m_uintPointsCurrent;
        unsigned int  m_uintPointsAllocated;

        struct Node
        {
            unsigned int *               m_ptrIndices;
            unsigned int                 m_uintIndices;
            Node *                       m_ptrChildren[1 << Dimensions];
            MATH::DN::Box<T, Dimensions> m_stuCorners;

            Node(const MATH::DN::Box<T, Dimensions> & Corners)
                : m_ptrIndices(new unsigned int[TREE_MAX_POINTS_PER_NODE]),
                  m_stuCorners(Corners),
                  m_uintIndices(0)
            {
                memset(&m_ptrChildren, 0, (1 << Dimensions) * sizeof(Line::Node *));
            }

            ~Node()
            {
                delete [] m_ptrIndices;
                for (unsigned int i=0; i<(1 << Dimensions); i++)
                    delete m_ptrChildren[i];
            }
        };

        unsigned int Find(Node ** Leaf, const MATH::DN::Point<T, Dimensions> & Point, const bool IgnoreConnectedPoints = false);
        void         Insert(Node * Node, const unsigned int Index);
        void         Reverse(unsigned int Index, const bool Forward);

        Node m_stuRoot;

    public:

        Line(const MATH::DN::Box<T, Dimensions> & Corners)
            : m_stuRoot(Corners),
              m_ptrPoints(NULL),
              m_uintLineIndex(0),
              m_uintPointsCurrent(0),
              m_uintPointsAllocated(0) {}

        double        Curl(const unsigned int Index, const T MaxDistance, unsigned int * EndIndex, T * Distance = NULL);
        unsigned int  GetLine(unsigned int * Amount = NULL, const unsigned int DimensionIndex = (unsigned int)(~0), const T DimensionValue = (T)(-1));
        const Point * Points(void) const {return m_ptrPoints;}
        bool          Insert(const MATH::DN::Point<T, Dimensions> & LineStart, const MATH::DN::Point<T, Dimensions> & LineEnd);
        void          ResetLines(const bool ResetFlags = true);
    };

    /*
    Stores occurences of arrays of data ranges (such as ascii strings)

    D: data type stored in the tree
    RangeStart: data range start
    RangeEnd: data range end
    */
    template<class D, unsigned char RangeStart, unsigned char RangeEnd>
    class Range
    {
    private:

        struct Node
        {
            D      m_clsData;
            bool   m_blnSet;
            Node * m_ptrChildren[(RangeEnd - RangeStart) + 1];

            Node() : m_blnSet(false)
            {
                memset(m_ptrChildren, 0, sizeof(Node *) * ((RangeEnd - RangeStart) + 1));
            }

            ~Node() {Finish(NULL);}

            void
            Finish(void (* DeleteFunc)(D Value))
            {
                for (unsigned int i=0; i<=(RangeEnd - RangeStart); i++)
                {        
                    if ( m_ptrChildren[i] != NULL )
                    {
                        m_ptrChildren[i]->Finish(DeleteFunc);
                        delete m_ptrChildren[i];
                        m_ptrChildren[i] = NULL;
                    }
                }

                if ( m_blnSet && DeleteFunc )
                    DeleteFunc(m_clsData);
            }
        };

        Node m_stuRoot;

    public:

        ~Range() {Finish();}

        void Finish(void (* DeleteFunc)(D Value) = NULL) {m_stuRoot.Finish(DeleteFunc);}
        D *  Get(const unsigned char * Identifier);
        bool Insert(const D Data, const unsigned char * Identifier, const bool OverWrite = false);
    };

    /*
    Stores two dimensional polygons.  As well as the tree structure being a
    hierarchy, the polygons themselves can have child polygons.
    */
    class Shape
    {
    public:

        struct Area
        {
            Area *            m_ptrParent;
            Area **           m_ptrChildren;
            unsigned long     m_ulngChildren;
            MATH::D2::BoxW    m_stuCorners;
            MATH::D2::Point * m_ptrPoints;
            unsigned long     m_ulngPoints;
            unsigned long     m_ulngDepth;
            char              m_chrPolar;
            char              m_chrContainment;

            int intId;

            Area(Area * Parent, const MATH::D2::Point * Points, const unsigned int Amount);
            Area() {}
            ~Area();

            bool          Clockwise(void) const; // Returns true for clockwise, false for counter-clockwise
            bool          Hit(const MATH::D2::Point & HitPoint) const;
            unsigned long Memory(void) const;
            void          Orient(const bool Clockwise);
        };

    private:

        struct Node
        {
            MATH::D2::BoxW m_stuCorners;
            Area **        m_ptrAreas;
            unsigned long  m_ulngAreas;
            unsigned long  m_ulngLowestDepth;
            unsigned long  m_ulngContainmentIndex;
            Node *         m_ptrChildren[4];
            bool           m_blnContainsRootAreas;

            Node(const MATH::D2::BoxW & Corners);
            ~Node();

            unsigned long Depth(void) const;
            bool          FindArea(const Area * Parent, const Node ** ParentNodes, const unsigned long Parents, unsigned long * Indices) const;
            unsigned int  Find(const MATH::D2::BoxW & Corners, const bool FindOnlyParents, const Area ** AreaList, const unsigned int AreaListLength) const;
            Area *        Hit(const MATH::D2::Point & HitPoint) const;
            void          Insert(Area * Inserted);
            unsigned long Memory(void) const;
            bool          Retrieve(std::istream & In, std::vector<char> & Buffer, unsigned char * p_ptrData, const Node ** ParentNodes, const unsigned long Parents, const MATH::D2::BoxW * Corners, bool Enclosed);
            unsigned long Size(void) const;
            bool          Store(std::ostream & Out, std::vector<char> & BufferVector, unsigned char * BufferStack, const Node ** ParentNodes, const unsigned long Parents) const;

            void Contain(const unsigned long p_ulngIndex);
            
            bool Integrity(const Node ** Parents, const unsigned long Amount) const
            {
                unsigned int  i;
                unsigned long ulngIndices[2];
                Parents[Amount] = this;
                for (i=0; i<m_ulngAreas; i++)
                {
                    if ( m_ptrAreas[i]->m_ptrParent != NULL &&
                         FindArea(m_ptrAreas[i]->m_ptrParent, 
                                  Parents,
                                  Amount,
                                  &(ulngIndices[0])) == false )
                    {
                        return false;
                    }
                }

                for (i=0; i<4; i++)
                {
                    if ( m_ptrChildren[i] != NULL && m_ptrChildren[i]->Integrity(Parents, Amount + 1) == false )
                        return false;
                }

                return true;
            }
        };

        static void ConvertFloat(float * Floats, const void * Data, const unsigned int Amount)
        {
            const char * ptrData = (const char *)(Data);
            for (unsigned int i=0; i<Amount; i++)
            {
                Floats[i] = ( ptrData[i * 4] & 0x80 )
                    ? (float)(-1.0 * pow(2.0, -24) * (double)((ptrData[(i * 4) + 1] << 16) + (ptrData[(i * 4) + 2] << 8) + ptrData[(i * 4) + 3]) * pow(16.0, (double)(ptrData[i * 4] & (~(0x80))) - 64.0))
                    : (float)(pow(2.0, -24) * (double)((ptrData[(i * 4) + 1] << 16) + (ptrData[(i * 4) + 2] << 8) + ptrData[(i * 4) + 3]) * pow(16.0, (double)(ptrData[i * 4] & (~(0x80))) - 64.0));
            }
        }

        static void ConvertFloat(void * Data, const float * Floats, const unsigned int Amount)
        {
            char * ptrData = (char *)(Data);
            for (unsigned int i=0; i<Amount; i++)
            {
                if ( Floats[i] != 0.0f )
                {
                    unsigned char uchrExponent = 70;
                    bool blnNegative = ( Floats[i] < 0.0f );
                    float fltValue = ( blnNegative ) ? -Floats[i] : Floats[i];

                    while ( fltValue > (float)(1 << 24) )
                    {
                        fltValue /= (float)(1 << 24);
                        uchrExponent += 6;
                    }

                    while ( fltValue < 1.0f )
                    {
                        fltValue *= (float)(1 << 24);
                        uchrExponent -= 6;
                    }

                    while ( fltValue < (float)(1 << 20) )
                    {
                        fltValue *= 16.0f;
                        uchrExponent--;
                    }

                    unsigned int uintInteger = (unsigned int)(fltValue);
                    ptrData[(i * 4) + 0] = (unsigned char)(( blnNegative ) ? uchrExponent | 0x80 : uchrExponent);
                    ptrData[(i * 4) + 1] = (unsigned char)((uintInteger >> 16) & 0xff);
                    ptrData[(i * 4) + 2] = (unsigned char)((uintInteger >> 8) & 0xff);
                    ptrData[(i * 4) + 3] = (unsigned char)(uintInteger & 0xff);
                }
                else
                    ptrData[(i * 4) + 0] = ptrData[(i * 4) + 1] = ptrData[(i * 4) + 2] = ptrData[(i * 4) + 3] = 0;
            }
        }

        Node m_stuRoot;

    public:

        Shape(const MATH::D2::BoxW & Corners) : m_stuRoot(Corners) {}

        unsigned int
        Find(const MATH::D2::BoxW & Corners,
             const bool FindOnlyParents,
             const Area ** AreaList,
             const unsigned int AreaListLength) const
        {
            return m_stuRoot.Find(Corners,
                                  FindOnlyParents,
                                  AreaList,
                                  AreaListLength);
        }

        unsigned long
        Hit(const MATH::D2::Point & p_stuPoint) const
        {
            const Area * ptrArea = m_stuRoot.Hit(p_stuPoint);
            return ( ptrArea != NULL ) ? ptrArea->m_ulngDepth + 1 : 0;
        }

        void          Insert(Area * Inserted) {m_stuRoot.Insert(Inserted);}
        bool          InsertGshhs(std::istream & In, const MATH::D2::BoxW * p_ptrCorners = NULL);
        unsigned long Memory(void) const {return sizeof(TREE::Shape) + m_stuRoot.Memory();}
        bool          Retrieve(std::istream & In, const MATH::D2::BoxW * Corners = NULL);
        bool          Store(std::ostream & Out) const;
    };

    /*
    Stores points of data in an n-dimensional tree, where any dimension of the
    tree can 'wrap' (such as longitude on a globe).

    D: data type stored in the tree
    T: dimension type
    Dimensions: number of dimensions
    */
    template<class D, class T, unsigned int Dimensions>
    class Wrap : public Basic<D, T>
    {
    private:

        struct WrapPoint
        {
            D                              m_clsData;
            MATH::DN::Point<T, Dimensions> m_stuPoint;
        };

        struct WrapNode
        {
            MATH::DN::Box<T, Dimensions> m_stuCorners;
            WrapNode *                   m_ptrChildren[1 << Dimensions];
            WrapPoint *                  m_ptrPoints;
            unsigned int                 m_uintPoints;

            WrapNode(const MATH::DN::Box<T, Dimensions> & Corners) : m_uintPoints(0)
            {
                m_stuCorners = Corners;
                memset(&m_ptrChildren, 0, (1 << Dimensions) * sizeof(WrapNode *));
                m_ptrPoints = new WrapPoint[TREE_MAX_POINTS_PER_NODE];
            }

            ~WrapNode() {Finish();}

            void
            Finish(void)
            {
                if ( m_ptrPoints == NULL )
                {
                    for (unsigned int i=0; i<(1 << Dimensions); i++)
                        UTIL_DELETE(m_ptrChildren[i]);
                }
                else
                {
                    UTIL_DELETE_ARRAY(m_ptrPoints);
                }
            }

            unsigned long Memory(void) const
            {
                unsigned long ulngSize = sizeof(WrapNode);

                if ( m_ptrPoints == NULL )
                {
                    for (unsigned int i=0; i<(1 << Dimensions); i++)
                    {
                        if ( m_ptrChildren[i] )
                            ulngSize += m_ptrChildren[i]->Memory();
                    }
                }
                else
                    ulngSize += m_uintPoints * sizeof(WrapPoint);

                return ulngSize;
            }
        };

        struct InterpolateData
        {
            const MATH::DN::Point<T, Dimensions> * m_ptrPoint;
            T                                      m_clsMaxDistance;
            T                                      m_clsAveragePower;
            T *                                    m_ptrDistancesTemp;
            T *                                    m_ptrDistances;
            D *                                    m_ptrResult;
            D *                                    m_ptrValues;
            WrapPoint **                           m_ptrPoints;
            unsigned int                           m_uintPoints;
        };

        MATH::DN::Box<T, Dimensions>   m_stuFilter;
        MATH::DN::Point<T, Dimensions> m_stuDefaultValues;
        InterpolateData                m_stuInterpolate;
        WrapNode                       m_stuRoot;
        bool                           m_blnFilter;
        bool                           m_blnDistanceSet;
        T                              m_clsDistance;

        static void         Change(WrapNode * Node, const T ValueFrom, const T ValueTo, const T AllowableDifference);
        static unsigned int ChildrenDistances(const WrapNode * Node, const MATH::DN::Point<T, Dimensions> & Point, T * p_ptrDistances, unsigned int * Indices);
        static T            Distance(const WrapNode * Node, const MATH::DN::Point<T, Dimensions> & Point);
        static bool         Extremes(const WrapNode * Node, D & Minimum, D & Maximum, const MATH::DN::Box<T, Dimensions> * Corners, bool Enclosed);
        static void         Interpolate(const WrapNode * Node, const InterpolateData & Data);
        static bool         Insert(WrapNode * Node, const D & Data, const MATH::DN::Point<T, Dimensions> & Point);
        static bool         Interpolate(const InterpolateData & Data);

    public:

        Wrap(const MATH::DN::Box<T, Dimensions> & Corners)
            : m_stuRoot(Corners),
              m_blnFilter(false),
              m_blnDistanceSet(false),
              m_stuDefaultValues((T)(0)),
              m_clsDistance((T)(0))
        {
            m_stuFilter = Corners;
            memset(&m_stuInterpolate, 0, sizeof(InterpolateData));
        }

        ~Wrap() {Finish();}

        inline void
        Change(const D p_clsValueFrom,
               const D p_clsValueTo,
               const D p_clsAllowableDifference)
        {
            Change(&m_stuRoot, p_clsValueFrom, p_clsValueTo, p_clsAllowableDifference);
        }

        inline void
        Default(const T Default, const unsigned int DimensionIndex)
        {
            if ( DimensionIndex < Dimensions )
                m_stuDefaultValues[DimensionIndex] = Default;
        }

        bool
        Intersect(const T * Point, const unsigned int Mask = ~((unsigned int)(0)))
        {
            return m_stuRoot.m_stuCorners.IntersectW(*((MATH::DN::Point<T, Dimensions> *)(Point)), Mask);
        }

        bool          Extremes(D & Minimum, D & Maximum, const MATH::DN::Box<T, Dimensions> * Corners) const;
        void          Finish(void);
        bool          Insert(const D & Data, const T * PointRef, const unsigned int Mask = ~((unsigned int)(0)));
        void          Distance(const T Distance);
        void          Filter(const bool Toggle, const MATH::DN::Box<T, Dimensions> * Corners = NULL);
        unsigned int  Interpolate(D * Values) const;
        D             Interpolate(const D * Values, const unsigned int Amount) const;
        bool          Interpolate(void) const;
        bool          Interpolate(const T * PointRef, const unsigned int Points, const T MaxDistance, const T AveragePower, D * Result) const;
        unsigned long Memory(void) const {return sizeof(TREE::Wrap<D, T, Dimensions>) + m_stuRoot.Memory();}
        void          SetInterpolateData(const unsigned int Points, const MATH::DN::Point<T, Dimensions> * PointRef = NULL, const T MaxDistance = (T)(0), const T AveragePower = (T)(0), D * Result = NULL);
    };
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Function definitions
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////
// Index
//////////////////////////////////////////////////////////////////////////////////////////////////////////

template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Allocate(const unsigned int p_uintAmount)
{
    if ( p_uintAmount > m_uintPointsAllocated )
    {
        m_uintPointsAllocated = p_uintAmount;
        m_ptrPoints = (MATH::DN::Point<T, Dimensions> *)realloc(m_ptrPoints, m_uintPointsAllocated * sizeof(MATH::DN::Point<T, Dimensions>));
    }
}

template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
AllocateFix(void)
{
    if ( m_uintPointsAllocated != m_uintPoints )
    {
        m_uintPointsAllocated = m_uintPoints;
        m_ptrPoints = (MATH::DN::Point<T, Dimensions> *)realloc(m_ptrPoints, m_uintPoints * sizeof(MATH::DN::Point<T, Dimensions>));
    }
}

/*
Wrapper for the primary Insert() method
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Index<T, Dimensions>::
Insert(const T * p_ptrValues)
{
    MATH::DN::Point<T, Dimensions> stuPoint(p_ptrValues);
    return Insert(stuPoint);
}

/*
Inserts a point and returns its index in the point array
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Index<T, Dimensions>::
Insert(const MATH::DN::Point<T, Dimensions> & p_stuPoint)
{
    unsigned int uintIndex = Find(&m_stuRoot, p_stuPoint);
    if ( uintIndex == (unsigned int)(~0) )
    {
        if ( m_uintPoints >= m_uintPointsAllocated )
        {
            m_uintPointsAllocated = ((m_uintPoints / TREE_MAX_POINTS_PER_NODE) + 1) * TREE_MAX_POINTS_PER_NODE;
            m_ptrPoints = (MATH::DN::Point<T, Dimensions> *)realloc(m_ptrPoints, m_uintPointsAllocated * sizeof(MATH::DN::Point<T, Dimensions>));
        }
        uintIndex = m_uintPoints++;
        m_ptrPoints[uintIndex] = p_stuPoint;
        Insert(&m_stuRoot, uintIndex);
        m_stuCorners.Add(p_stuPoint);
        TREE::Index<T, Dimensions>::OptimizeCheck(&m_stuRoot,
                                                  m_fltThreshhold,
                                                  m_ptrPoints,
                                                  &m_clsBuffer);
    }

    return uintIndex;
}

/*
Private routine called when optimizing the tree structure
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Insert(typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
       const unsigned int p_uintIndex)
{
    for (;;)
    {
        p_ptrNode->m_uintIndices++;
        if ( p_ptrNode->m_ptrIndices == NULL )
        {
            if ( m_ptrPoints[p_uintIndex][p_ptrNode->m_uintSplitAxis] < p_ptrNode->m_clsSplitValue )
                p_ptrNode = p_ptrNode->m_ptrChildren[0];
            else if ( m_ptrPoints[p_uintIndex][p_ptrNode->m_uintSplitAxis] > p_ptrNode->m_clsSplitValue )
                p_ptrNode = p_ptrNode->m_ptrChildren[1];
            else if ( p_ptrNode->m_ptrChildren[0]->m_uintIndices < p_ptrNode->m_ptrChildren[1]->m_uintIndices )
                p_ptrNode = p_ptrNode->m_ptrChildren[0];
            else
                p_ptrNode = p_ptrNode->m_ptrChildren[1];
        }
        else
        {
            p_ptrNode->m_ptrIndices[p_ptrNode->m_uintIndices - 1] = p_uintIndex;
            if ( p_ptrNode->m_uintIndices > TREE_MAX_POINTS_PER_NODE )
            {
                TREE::Index<T, Dimensions>::Optimize(p_ptrNode,
                                                     m_ptrPoints,
                                                     m_clsBuffer);
            }
            return;
        }
    }
}

/*
Inserts an array of points into the tree and optionally caches each point index
into a parameter index array
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Insert(const MATH::DN::Point<T, Dimensions> * p_ptrPoints,
       const unsigned * p_ptrIndices,
       const unsigned int p_uintAmount,
       unsigned * p_ptrIndicesSet)
{
    if ( p_ptrIndicesSet )
    {
        for (unsigned int i=0; i<p_uintAmount; i++)
            p_ptrIndicesSet[i] = Insert(p_ptrPoints[p_ptrIndices[i]]);
    }
    else
    {
        for (unsigned int i=0; i<p_uintAmount; i++)
            Insert(p_ptrPoints[p_ptrIndices[i]]);
    }
}

/*
Returns the point array and clears the internal pointer (so it will not be
released in the destructor)
*/
template <class T, unsigned int Dimensions>
MATH::DN::Point<T, Dimensions> *
TREE::Index<T, Dimensions>::
Clear(unsigned int * PointsAmount)
{
    MATH::DN::Point<T, Dimensions> * ptrTemp = m_ptrPoints;
    if ( PointsAmount )
        *PointsAmount = m_uintPoints;
    m_ptrPoints = NULL;
    Finish();
    return ptrTemp;
}

/*
Private routine that searches for a point
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Index<T, Dimensions>::
Find(typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
     const typename MATH::DN::Point<T, Dimensions> & p_stuPoint)
{
    for (;;)
    {
        if ( p_ptrNode->m_ptrIndices == NULL )
        {
            //
            // Branch node
            //
            if ( p_stuPoint.m_clsValues[p_ptrNode->m_uintSplitAxis] < p_ptrNode->m_clsSplitValue )
                p_ptrNode = p_ptrNode->m_ptrChildren[0];
            else if ( p_stuPoint.m_clsValues[p_ptrNode->m_uintSplitAxis] > p_ptrNode->m_clsSplitValue )
                p_ptrNode = p_ptrNode->m_ptrChildren[1];
            else
            {
                unsigned int uintIndex = Find(p_ptrNode->m_ptrChildren[0], p_stuPoint);
                if ( uintIndex == (unsigned int)(~0) )
                    uintIndex = Find(p_ptrNode->m_ptrChildren[1], p_stuPoint);
                return uintIndex;
            }
        }
        else
        {
            //
            // Leaf node
            //
            unsigned int * ptrIndices = p_ptrNode->m_ptrIndices;
            unsigned int   uintIndices = p_ptrNode->m_uintIndices;
            for (unsigned int i=0; i<uintIndices; i++)
            {
                if ( m_ptrPoints[ptrIndices[i]] == p_stuPoint )
                    return ptrIndices[i];
            }

            return (unsigned int)(~0);
        }
    }
}

/*
Fills the p_stuIndices parameter with indices to all points that lie within the
specified area
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Index<T, Dimensions>::
Find(const typename MATH::DN::Box<T, Dimensions> & p_stuSearchArea,
     std::vector<unsigned int> & p_stuIndices) const
{
    p_stuIndices.clear();
    MATH::DN::ResultBox<T, Dimensions> stuResult;
    MATH::DN::Box<T, Dimensions> stuCorners = m_stuCorners;
    if ( p_stuSearchArea.Intersect(m_stuCorners, &stuResult) )
    {
        if ( stuResult.m_intType != 2 && stuResult.m_intType != 4 )
        {
            //
            // The root node lies completely within the search area
            //
            TREE::Index<T, Dimensions>::Find(&m_stuRoot,
                                             &p_stuSearchArea,
                                             &stuCorners,
                                             m_ptrPoints,
                                             p_stuIndices);
        }
        else
        {
            //
            // The root node does not lie completely within the search area
            //
            TREE::Index<T, Dimensions>::Find(&m_stuRoot,
                                             NULL, NULL,
                                             m_ptrPoints,
                                             p_stuIndices);
        }
    }

    return p_stuIndices.size();
}

/*
Private routine for finding points
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Find(const typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
     const typename MATH::DN::Box<T, Dimensions> * p_ptrSearchArea,
     typename MATH::DN::Box<T, Dimensions> * p_ptrNodeArea,
     const typename MATH::DN::Point<T, Dimensions> * p_ptrPoints,
     std::vector<unsigned int> & p_stuIndices)
{
    if ( p_ptrNode->m_ptrIndices == NULL )
    {
        //
        // This is a branch node
        //
        if ( p_ptrSearchArea && p_ptrNodeArea )
        {
            //
            // Check the first child
            //
            T clsValues[2];
            clsValues[0] = p_ptrNodeArea->m_clsValues[2 * p_ptrNode->m_uintSplitAxis];
            clsValues[1] = p_ptrNodeArea->m_clsValues[(2 * p_ptrNode->m_uintSplitAxis) + 1];
            p_ptrNodeArea->m_clsValues[(2 * p_ptrNode->m_uintSplitAxis) + 1] = p_ptrNode->m_clsSplitValue;
            MATH::DN::ResultBox<T, Dimensions> stuResult;
            if ( p_ptrSearchArea->Intersect(*p_ptrNodeArea, &stuResult) )
            {
                //
                // The first child intersects the search area
                //
                if ( stuResult.m_intType != 2 && stuResult.m_intType != 4 )
                {
                    //
                    // The first child is not completely within the search area
                    //
                    TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[0],
                                                     p_ptrSearchArea,
                                                     p_ptrNodeArea,
                                                     p_ptrPoints,
                                                     p_stuIndices);
                }
                else
                {
                    //
                    // The first child is completely within the search area
                    //
                    TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[0],
                                                     NULL, NULL,
                                                     p_ptrPoints,
                                                     p_stuIndices);
                }
            }

            //
            // Check the second child node
            //
            p_ptrNodeArea->m_clsValues[2 * p_ptrNode->m_uintSplitAxis] = p_ptrNode->m_clsSplitValue;
            p_ptrNodeArea->m_clsValues[(2 * p_ptrNode->m_uintSplitAxis) + 1] = clsValues[1];
            if ( p_ptrSearchArea->Intersect(*p_ptrNodeArea, &stuResult) )
            {
                //
                // The second child node intersects the search area
                //
                if ( stuResult.m_intType != 2 && stuResult.m_intType != 4 )
                {
                    //
                    // The second child node is not completely within the search area
                    //
                    TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[1],
                                                     p_ptrSearchArea,
                                                     p_ptrNodeArea,
                                                     p_ptrPoints,
                                                     p_stuIndices);
                }
                else
                {
                    //
                    // The second child node is completely within the search area
                    //
                    TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[1],
                                                     NULL, NULL,
                                                     p_ptrPoints,
                                                     p_stuIndices);
                }
            }
            p_ptrNodeArea->m_clsValues[2 * p_ptrNode->m_uintSplitAxis] = clsValues[0];
            p_ptrNodeArea->m_clsValues[(2 * p_ptrNode->m_uintSplitAxis) + 1] = clsValues[1];
        }
        else
        {
            //
            // This node is completely within the search area, so no need to
            // check child node intersections
            //
            TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[0],
                                             NULL, NULL,
                                             p_ptrPoints,
                                             p_stuIndices);
            TREE::Index<T, Dimensions>::Find(p_ptrNode->m_ptrChildren[1],
                                             NULL, NULL,
                                             p_ptrPoints,
                                             p_stuIndices);
        }
    }
    else
    {
        //
        // Leaf node
        //
        if ( p_ptrSearchArea )
        {
            //
            // This node is not completely within the search area, so we need
            // to check each point against the search area
            //
            for (unsigned int i=0; i<p_ptrNode->m_uintIndices; i++)
            {
                if ( p_ptrSearchArea->Intersect(p_ptrPoints[p_ptrNode->m_ptrIndices[i]]) )
                    p_stuIndices.push_back(p_ptrNode->m_ptrIndices[i]);
            }
        }
        else
        {
            //
            // This node is completely within the search area, so no need to
            // check for intersections
            //
            p_stuIndices.reserve(p_stuIndices.size() + p_ptrNode->m_uintIndices);
            p_stuIndices.insert(p_stuIndices.begin() + p_stuIndices.size(),
                                p_ptrNode->m_ptrIndices,
                                p_ptrNode->m_ptrIndices + p_ptrNode->m_uintIndices);
        }
    }
}

/*
Releases all resources
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Finish(void)
{
    m_stuRoot.Finish();
    m_uintPoints = 0;
    m_uintPointsAllocated = 0;
    UTIL_SAFE_FREE(m_ptrPoints);
}

/*
Private routine used when optimizing the tree to get all points below a node
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
GetIndices(const typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
           unsigned int * p_ptrIndices)
{
    if ( p_ptrNode->m_ptrIndices == NULL )
    {
        TREE::Index<T, Dimensions>::GetIndices(p_ptrNode->m_ptrChildren[0], p_ptrIndices);
        TREE::Index<T, Dimensions>::GetIndices(p_ptrNode->m_ptrChildren[1], p_ptrIndices + p_ptrNode->m_ptrChildren[0]->m_uintIndices);
    }
    else
        memcpy(p_ptrIndices, p_ptrNode->m_ptrIndices, p_ptrNode->m_uintIndices * sizeof(unsigned int));
}

/*
Private routine that checks to make sure all indices are valid
*/
template <class T, unsigned int Dimensions>
bool
TREE::Index<T, Dimensions>::
Integrity(const typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
          const unsigned int p_uintMax)
{
    if ( p_ptrNode->m_ptrIndices == NULL )
    {
        return ( TREE::Index<T, Dimensions>::Integrity(p_ptrNode->m_ptrChildren[0], p_uintMax) == true &&
                 TREE::Index<T, Dimensions>::Integrity(p_ptrNode->m_ptrChildren[1], p_uintMax) == true );
    }
    else
    {
        for (unsigned int i=0; i<p_ptrNode->m_uintIndices; i++)
        {
            if ( p_ptrNode->m_ptrIndices[i] >= p_uintMax )
                return false;
        }

        return true;
    }
}

/*
Private routine that optimizes searching by re-ordering the hierarchy
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
Optimize(typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
         const typename MATH::DN::Point<T, Dimensions> * p_ptrPoints,
         std::vector<unsigned int> & p_clsBuffer)
{
    //
    // Check if we should create a branch or leaf node
    //
    //p_ptrNode->m_uintIndices = p_uintIndices;
    if ( p_ptrNode->m_uintIndices <= TREE_MAX_POINTS_PER_NODE )
    //{
    //    p_ptrNode->m_ptrIndices = p_ptrIndices;
        return;
    //}

    //
    // Delete child nodes
    //
    delete p_ptrNode->m_ptrChildren[0];
    delete p_ptrNode->m_ptrChildren[1];

    //
    // Find the optimal split axis given the points in this node.  We do
    // this by counting the number of points on either side of each
    // dimensional value of each point, and choosing the point/dimension
    // with a similar number of points on either side
    //
    unsigned int j;
    unsigned int uintSplitAmounts[2];
    unsigned int uintDifference = (unsigned int)(~0);
    unsigned int uintIndices = p_ptrNode->m_uintIndices;
    unsigned int * ptrIndices = p_ptrNode->m_ptrIndices;
    for (unsigned int i=0; i<Dimensions; i++)
    {
        for (j=0; j<uintIndices; j++)
        {
            T clsValue = p_ptrPoints[ptrIndices[j]][i];
            unsigned int uintAmounts[3];
            uintAmounts[0] = uintAmounts[1] = uintAmounts[2] = 0;
            for (unsigned int k=0; k<uintIndices; k++)
            {
                if ( p_ptrPoints[ptrIndices[k]][i] < clsValue )
                    uintAmounts[0]++;
                else if ( p_ptrPoints[ptrIndices[k]][i] > clsValue )
                    uintAmounts[2]++;
                else
                    uintAmounts[1]++;
            }

            if ( uintAmounts[0] < uintAmounts[2] )
            {
                if ( (uintAmounts[2] - uintAmounts[0]) >= uintAmounts[1] )
                {
                    uintAmounts[0] += uintAmounts[1];
                    uintAmounts[1] = 0;
                }
                else
                {
                    uintAmounts[1] -= (uintAmounts[2] - uintAmounts[0]);
                    uintAmounts[0] += (uintAmounts[2] - uintAmounts[0]);
                }
            }
            else if ( uintAmounts[0] > uintAmounts[2] )
            {
                if ( (uintAmounts[0] - uintAmounts[2]) >= uintAmounts[1] )
                {
                    uintAmounts[2] += uintAmounts[1];
                    uintAmounts[1] = 0;
                }
                else
                {
                    uintAmounts[1] -= (uintAmounts[0] - uintAmounts[2]);
                    uintAmounts[2] += (uintAmounts[0] - uintAmounts[2]);
                }
            }

            uintAmounts[0] += uintAmounts[1] / 2;
            uintAmounts[2] += uintAmounts[1] / 2;
            if ( uintAmounts[1] % 2 )
                uintAmounts[0]++;

            unsigned int uintTemp = ( uintAmounts[0] < uintAmounts[2] )
                ? uintAmounts[2] - uintAmounts[0]
                : uintAmounts[0] - uintAmounts[2];

            if ( uintTemp < uintDifference )
            {
                uintSplitAmounts[0] = uintAmounts[0];
                uintSplitAmounts[1] = uintAmounts[2];
                uintDifference = uintTemp;
                p_ptrNode->m_clsSplitValue = clsValue;
                p_ptrNode->m_uintSplitAxis = i;
            }
        }
    }

    //
    // Based on the optimal split axis calculated above, put points on
    // either side of the split axis
    //
    p_clsBuffer.clear();
    unsigned int   uintAmountLesser = 0;
    unsigned int   uintAmountGreater = 0;
    unsigned int * ptrIndicesLesser = new unsigned int[( uintSplitAmounts[0] > (TREE_MAX_POINTS_PER_NODE + 1) ) ? uintSplitAmounts[0] : (TREE_MAX_POINTS_PER_NODE + 1)];
    unsigned int * ptrIndicesGreater = new unsigned int[( uintSplitAmounts[1] > (TREE_MAX_POINTS_PER_NODE + 1) ) ? uintSplitAmounts[1] : (TREE_MAX_POINTS_PER_NODE + 1)];
    for (j=0; j<uintIndices; j++)
    {
        if ( p_ptrPoints[ptrIndices[j]][p_ptrNode->m_uintSplitAxis] < p_ptrNode->m_clsSplitValue )
            ptrIndicesLesser[uintAmountLesser++] = ptrIndices[j];
        else if ( p_ptrPoints[ptrIndices[j]][p_ptrNode->m_uintSplitAxis] > p_ptrNode->m_clsSplitValue )
            ptrIndicesGreater[uintAmountGreater++] = ptrIndices[j];
        else
            p_clsBuffer.push_back(ptrIndices[j]);
    }
    delete [] ptrIndices;
    p_ptrNode->m_ptrIndices = NULL;

    for (j=0; j<p_clsBuffer.size(); j++)
    {
        if ( uintAmountLesser <= uintAmountGreater )
            ptrIndicesLesser[uintAmountLesser++] = p_clsBuffer[j];
        else
            ptrIndicesGreater[uintAmountGreater++] = p_clsBuffer[j];
    }

    //
    // The first child will contain points lesser than the split axis.
    //
    p_ptrNode->m_ptrChildren[0] = new typename TREE::Index<T, Dimensions>::Node;
    p_ptrNode->m_ptrChildren[0]->m_ptrIndices = ptrIndicesLesser;
    p_ptrNode->m_ptrChildren[0]->m_uintIndices = uintAmountLesser;
    TREE::Index<T, Dimensions>::Optimize(p_ptrNode->m_ptrChildren[0],
                                         p_ptrPoints,
                                         p_clsBuffer);

    //
    // The second child will contain points greater than the split axis.
    //
    p_ptrNode->m_ptrChildren[1] = new typename TREE::Index<T, Dimensions>::Node;
    p_ptrNode->m_ptrChildren[1]->m_ptrIndices = ptrIndicesGreater;
    p_ptrNode->m_ptrChildren[1]->m_uintIndices = uintAmountGreater;
    TREE::Index<T, Dimensions>::Optimize(p_ptrNode->m_ptrChildren[1],
                                         p_ptrPoints,
                                         p_clsBuffer);
}

/*
Private routine that checks to see if we should optimize the tree or not
*/
template <class T, unsigned int Dimensions>
void
TREE::Index<T, Dimensions>::
OptimizeCheck(typename TREE::Index<T, Dimensions>::Node * p_ptrNode,
              const float p_fltThreshhold,
              const typename MATH::DN::Point<T, Dimensions> * p_ptrPoints,
              std::vector<unsigned int> & p_clsBuffer)
{
    //
    // We only care about optimizing branch nodes
    //
    if ( p_ptrNode->m_ptrIndices == NULL )
    {
        unsigned int uintAmountLesser = p_ptrNode->m_ptrChildren[0]->m_uintIndices;
        unsigned int uintAmountGreater = p_ptrNode->m_ptrChildren[1]->m_uintIndices;

        //
        // Compute the ratio of points in one child to the other child
        //
        float fltRatio = ( uintAmountLesser < uintAmountGreater )
            ? ((float)(uintAmountLesser)) / ((float)(uintAmountGreater))
            : ((float)(uintAmountGreater)) / ((float)(uintAmountLesser));

        //
        // If the ratio is less than the threshhold, we optimize
        //
        if ( fltRatio < p_fltThreshhold )
        {
            unsigned int uintIndices = p_ptrNode->m_uintIndices;
            unsigned int * ptrIndices = new unsigned int[p_ptrNode->m_uintIndices];
            TREE::Index<T, Dimensions>::GetIndices(p_ptrNode, ptrIndices);
            //delete p_ptrNode->m_ptrChildren[0];
            //delete p_ptrNode->m_ptrChildren[1];
            p_ptrNode->Finish();
            p_ptrNode->m_ptrIndices = ptrIndices;
            p_ptrNode->m_uintIndices = uintIndices;
            TREE::Index<T, Dimensions>::Optimize(p_ptrNode,
                                                 //ptrIndices,
                                                 //uintIndices,
                                                 p_ptrPoints,
                                                 p_clsBuffer);
        }
        else
        {
            TREE::Index<T, Dimensions>::OptimizeCheck(p_ptrNode->m_ptrChildren[0], p_fltThreshhold, p_ptrPoints, p_ptrBuffer);
            TREE::Index<T, Dimensions>::OptimizeCheck(p_ptrNode->m_ptrChildren[1], p_fltThreshhold, p_ptrPoints, p_ptrBuffer);
        }
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
// Line
//////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
Computes the sum of the squares of angles between adjacent lines.  This routine
returns larger values for more "curvy" lines, and smaller values for more
"straight" lines.
*/
template <class T, unsigned int Dimensions>
double
TREE::Line<T, Dimensions>::
Curl(const unsigned int p_uintIndex,
     const T p_clsMaxDistance,
     unsigned int * p_ptrEndIndex,
     T * p_ptrDistance)
{
    if ( p_uintIndex == (unsigned int)(~0) ||
         m_ptrPoints[p_uintIndex].m_uintPointNext == (unsigned int)(~0) )
    {
        return -1.0;
    }

    T                              clsDistance = (T)(0);
    T                              clsMaxDistance = p_clsMaxDistance * p_clsMaxDistance;
    MATH::DN::Point<T, Dimensions> stuPoint = m_ptrPoints[p_uintIndex].m_stuPoint;
    double                         dblCurl = 0.0;
    unsigned int                   uintIndex = m_ptrPoints[p_uintIndex].m_uintPointNext;

    //
    // Initialize the first line and distance
    //
    MATH::DN::Point<T, Dimensions> stuLine2 = m_ptrPoints[m_ptrPoints[p_uintIndex].m_uintPointNext].m_stuPoint - stuPoint;
    double                         dblLength2 = stuLine2.LengthD();

    //
    // Loop until we have either:
    //
    // 1. Exceeded the maximum distance
    // 2. Found the end of the line
    // 3. Found the starting point of the line (the line forms a loop)
    //
    while ( clsDistance <= clsMaxDistance &&
            m_ptrPoints[uintIndex].m_uintPointNext != (unsigned int)(~0) &&
            uintIndex != p_uintIndex )
    {
        //
        // Create a new line and length, setting them to be the previous line and length
        //
        MATH::DN::Point<T, Dimensions> stuLine1 = stuLine2;
        double dblLength1 = dblLength2;

        //
        // Set the line and length to the next values
        //
        stuLine2 = m_ptrPoints[m_ptrPoints[uintIndex].m_uintPointNext].m_stuPoint - m_ptrPoints[uintIndex].m_stuPoint;
        if ( stuLine1 != stuLine2 )
        {
            //
            // Calculate the angle between the lines and add it to the curl
            //
            dblLength2 = stuLine2.LengthD();
            double dblTemp = ((double)(stuLine1.Dot(stuLine2))) / (dblLength1 * dblLength2);
            if ( dblTemp != 1.0 )
            {
                double dblAngle = acos(dblTemp);
                dblCurl += dblAngle * dblAngle;
            }
        }

        uintIndex = m_ptrPoints[uintIndex].m_uintPointNext;
        clsDistance = stuPoint.DistanceSquared(&(m_ptrPoints[uintIndex].m_stuPoint));
    }

    if ( p_ptrEndIndex )
        *p_ptrEndIndex = uintIndex;

    if ( p_ptrDistance )
        *p_ptrDistance = clsDistance;

    return dblCurl;
}

/*
Searches for a point.  Optionally ignores points that are used by two lines
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Line<T, Dimensions>::
Find(typename TREE::Line<T, Dimensions>::Node ** p_ptrLeaf,
     const typename MATH::DN::Point<T, Dimensions> & p_stuPoint,
     const bool p_blnIgnoreConnectedPoints)
{
    typename TREE::Line<T, Dimensions>::Node * ptrNode = &m_stuRoot;

    while ( ptrNode != NULL )
    {
        *p_ptrLeaf = ptrNode;
        if ( ptrNode->m_ptrIndices )
        {
            unsigned int * ptrIndices = ptrNode->m_ptrIndices;
            unsigned int   uintIndices = ptrNode->m_uintIndices;
            for (unsigned int i=0; i<uintIndices; i++)
            {
                if ( m_ptrPoints[ptrIndices[i]].m_stuPoint == p_stuPoint &&
                     (p_blnIgnoreConnectedPoints == false ||
                      (m_ptrPoints[ptrIndices[i]].m_uintPointPrev == (unsigned int)(~0) ||
                       m_ptrPoints[ptrIndices[i]].m_uintPointNext == (unsigned int)(~0))) )
                {
                    return ptrIndices[i];
                }
            }

            ptrNode = NULL;
        }
        else
            ptrNode = ptrNode->m_ptrChildren[ptrNode->m_stuCorners.SectorW(p_stuPoint)];
    }

    return (unsigned int)(~0);
}

/*
Queries for a line that has not been queried for yet
*/
template <class T, unsigned int Dimensions>
unsigned int
TREE::Line<T, Dimensions>::
GetLine(unsigned int * p_ptrAmount,
        const unsigned int p_uintDimensionIndex,
        const T p_clsDimensionValue)
{
    if ( p_uintDimensionIndex != (unsigned int)(~0) &&
         p_clsDimensionValue != (T)(-1) )
    {
        //
        // Search for a point that has the specfied dimension value
        //
        while ( m_uintLineIndex < m_uintPointsCurrent &&
                (m_ptrPoints[m_uintLineIndex].m_blnFlag ||
                 m_ptrPoints[m_uintLineIndex].m_stuPoint[p_uintDimensionIndex] != p_clsDimensionValue) )
        {
            m_uintLineIndex++;
        }
    }
    else
    {
        //
        // Search for any point, since invalid dimension values were specified
        //
        while ( m_uintLineIndex < m_uintPointsCurrent &&
                m_ptrPoints[m_uintLineIndex].m_blnFlag )
        {
            m_uintLineIndex++;
        }
    }

    if ( m_uintLineIndex < m_uintPointsCurrent )
    {
        //
        // Flag each point on the line so future queries don't return them
        //
        unsigned int uintAmount = 0;
        unsigned int uintReset = m_uintLineIndex;
        unsigned int uintStart = m_uintLineIndex;
        do
        {
            uintAmount++;
            m_ptrPoints[uintReset].m_blnFlag = true;
            uintReset = m_ptrPoints[uintReset].m_uintPointNext;
        } while ( uintReset != (unsigned int)(~0) && uintReset != m_uintLineIndex );

        //
        // If the initial point was in the "middle" of a line, flag all those
        // "behind" the initial point
        //
        if ( uintReset != m_uintLineIndex )
        {
            unsigned int uintReset = m_ptrPoints[m_uintLineIndex].m_uintPointPrev;
            while ( uintReset != (unsigned int)(~0) )
            {
                uintAmount++;
                uintStart = uintReset;
                m_ptrPoints[uintReset].m_blnFlag = true;
                uintReset = m_ptrPoints[uintReset].m_uintPointPrev;
            }
        }

        m_uintLineIndex++;
        if ( p_ptrAmount )
            *p_ptrAmount = (uintAmount - 1);

        return uintStart;
    }
    else
        return (unsigned int)(~0);
}

/*
Private routine that inserts a point
*/
template <class T, unsigned int Dimensions>
void
TREE::Line<T, Dimensions>::
Insert(typename TREE::Line<T, Dimensions>::Node * p_ptrNode,
       const unsigned int p_uintIndex)
{
    for (;;)
    {
        if ( p_ptrNode->m_ptrIndices == NULL )
        {
            //
            // Branch node, check which child the point should go in
            //
            MATH::DN::Box<T, Dimensions> stuCorners;
            unsigned long ulngIndex = p_ptrNode->m_stuCorners.SectorW(m_ptrPoints[p_uintIndex].m_stuPoint, &stuCorners);
            if ( p_ptrNode->m_ptrChildren[ulngIndex] == NULL )
                p_ptrNode->m_ptrChildren[ulngIndex] = new typename TREE::Line<T, Dimensions>::Node(stuCorners);
            p_ptrNode = p_ptrNode->m_ptrChildren[ulngIndex];
        }
        else if ( p_ptrNode->m_uintIndices >= TREE_MAX_POINTS_PER_NODE )
        {
            //
            // Full leaf node, make this a brance node
            //
            unsigned int   uintIndices = p_ptrNode->m_uintIndices;
            unsigned int * ptrIndices = p_ptrNode->m_ptrIndices;
            p_ptrNode->m_ptrIndices = NULL;
            p_ptrNode->m_uintIndices = 0;

            for (unsigned int i=0; i<uintIndices; i++)
                Insert(p_ptrNode, ptrIndices[i]);
            delete [] ptrIndices;
        }
        else
        {
            //
            // Leaf node
            //
            p_ptrNode->m_ptrIndices[p_ptrNode->m_uintIndices++] = p_uintIndex;
            return;
        }
    }
}

/*
Inserts a line, as defined by a start and end point
*/
template <class T, unsigned int Dimensions>
bool
TREE::Line<T, Dimensions>::
Insert(const typename MATH::DN::Point<T, Dimensions> & p_stuLineStart,
       const typename MATH::DN::Point<T, Dimensions> & p_stuLineEnd)
{
    //
    // See if the start point has been inserted yet
    //
    typename TREE::Line<T, Dimensions>::Node * ptrNodeStart;
    unsigned int uintNodeStart = Find(&ptrNodeStart, p_stuLineStart, true);
    if ( uintNodeStart != (unsigned int)(~0) &&
         m_ptrPoints[uintNodeStart].m_uintPointPrev != (unsigned int)(~0) &&
         m_ptrPoints[uintNodeStart].m_uintPointNext != (unsigned int)(~0) )
    {
        return false;
    }

    //
    // See if the end point has been inserted yet
    //
    typename TREE::Line<T, Dimensions>::Node * ptrNodeEnd;
    unsigned int uintNodeEnd = Find(&ptrNodeEnd, p_stuLineEnd, true);
    if ( uintNodeEnd != (unsigned int)(~0) &&
         m_ptrPoints[uintNodeEnd].m_uintPointPrev != (unsigned int)(~0) &&
         m_ptrPoints[uintNodeEnd].m_uintPointNext != (unsigned int)(~0) )
    {
        return false;
    }

    //
    // Insert the start point if it has not been inserted
    //
    if ( uintNodeStart == (unsigned int)(~0) )
    {
        if ( m_uintPointsCurrent >= m_uintPointsAllocated )
        {
            m_ptrPoints = (typename TREE::Line<T, Dimensions>::Point *)realloc(m_ptrPoints, (m_uintPointsAllocated + TREE_MAX_POINTS_PER_NODE) * sizeof(typename TREE::Line<T, Dimensions>::Point));
            m_uintPointsAllocated += TREE_MAX_POINTS_PER_NODE;
        }

        m_ptrPoints[m_uintPointsCurrent].m_uintPointPrev = m_ptrPoints[m_uintPointsCurrent].m_uintPointNext = (unsigned int)(~0);
        m_ptrPoints[m_uintPointsCurrent].m_stuPoint = p_stuLineStart;
        m_ptrPoints[m_uintPointsCurrent].m_blnFlag = false;
        uintNodeStart = m_uintPointsCurrent++;
        Insert(ptrNodeStart, uintNodeStart);
    }

    //
    // Insert the end point if it has not been inserted
    //
    if ( uintNodeEnd == (unsigned int)(~0) )
    {
        if ( m_uintPointsCurrent >= m_uintPointsAllocated )
        {
            m_ptrPoints = (typename TREE::Line<T, Dimensions>::Point *)realloc(m_ptrPoints, (m_uintPointsAllocated + TREE_MAX_POINTS_PER_NODE) * sizeof(typename TREE::Line<T, Dimensions>::Point));
            m_uintPointsAllocated += TREE_MAX_POINTS_PER_NODE;
        }

        m_ptrPoints[m_uintPointsCurrent].m_uintPointPrev = m_ptrPoints[m_uintPointsCurrent].m_uintPointNext = (unsigned int)(~0);
        m_ptrPoints[m_uintPointsCurrent].m_stuPoint = p_stuLineEnd;
        m_ptrPoints[m_uintPointsCurrent].m_blnFlag = false;
        uintNodeEnd = m_uintPointsCurrent++;
        Insert(ptrNodeEnd, uintNodeEnd);
    }

    //
    // Reverse the lines if they are facing the wrong direction
    //
    if ( m_ptrPoints[uintNodeStart].m_uintPointNext != (unsigned int)(~0) )
        Reverse(uintNodeStart, true);
    if ( m_ptrPoints[uintNodeEnd].m_uintPointPrev != (unsigned int)(~0) )
        Reverse(uintNodeEnd, false);

    //
    // Connect the lines in each direction
    //
    m_ptrPoints[uintNodeStart].m_uintPointNext = uintNodeEnd;
    m_ptrPoints[uintNodeEnd].m_uintPointPrev = uintNodeStart;
    return true;
}

/*
Resets the counter used in GetLine() such that all lines will be queried.  Also
optionally unflags all lines so they will be returned again
*/
template <class T, unsigned int Dimensions>
void
TREE::Line<T, Dimensions>::
ResetLines(const bool p_blnResetFlags)
{
    m_uintLineIndex = 0;
    if ( p_blnResetFlags )
    {
        for (unsigned int i=0; i<m_uintPointsCurrent; i++)
            m_ptrPoints[i].m_blnFlag = false;
    }
}

/*
Changes the direction of a line
*/
template <class T, unsigned int Dimensions>
void
TREE::Line<T, Dimensions>::
Reverse(unsigned int p_uintIndex,
        const bool p_blnForward)
{
    while ( p_uintIndex != (unsigned int)(~0) )
    {
        unsigned int uintTempIndex = m_ptrPoints[p_uintIndex].m_uintPointPrev;
        m_ptrPoints[p_uintIndex].m_uintPointPrev = m_ptrPoints[p_uintIndex].m_uintPointNext;
        m_ptrPoints[p_uintIndex].m_uintPointNext = uintTempIndex;
        p_uintIndex = ( p_blnForward )
            ? m_ptrPoints[p_uintIndex].m_uintPointPrev
            : m_ptrPoints[p_uintIndex].m_uintPointNext;
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
// Range
//////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
Gets a reference to an inserted value based on its identifier, or NULL if the
identifier cannot be found
*/
template <class D, unsigned char RangeStart, unsigned char RangeEnd>
D *
TREE::Range<D, RangeStart, RangeEnd>::
Get(const unsigned char * p_ptrIdentifier)
{
    typename TREE::Range<D, RangeStart, RangeEnd>::Node * ptrNode = &m_stuRoot;

    for (; (*p_ptrIdentifier) != 0; p_ptrIdentifier++)
    {
        if ( (*p_ptrIdentifier) < RangeStart||
             (*p_ptrIdentifier) > RangeEnd ||
             ptrNode->m_ptrChildren[(*p_ptrIdentifier) - RangeStart] == NULL )
        {
            return NULL;
        }
        ptrNode = ptrNode->m_ptrChildren[(*p_ptrIdentifier) - RangeStart];
    }

    return &(ptrNode->m_clsData);
}

/*
Inserts a value based on an identifier, and optionally overwrites a value
if there is already one with the same identifier
*/
template <class D, unsigned char RangeStart, unsigned char RangeEnd>
bool
TREE::Range<D, RangeStart, RangeEnd>::
Insert(const D p_clsData,
       const unsigned char * p_ptrIdentifier,
       const bool p_blnOverWrite)
{
    typename TREE::Range<D, RangeStart, RangeEnd>::Node * ptrNode = &m_stuRoot;

    for (; (*p_ptrIdentifier) != 0; p_ptrIdentifier++)
    {
        if ( (*p_ptrIdentifier) < RangeStart || (*p_ptrIdentifier) > RangeEnd )
            return false;
        else if ( ptrNode->m_ptrChildren[(*p_ptrIdentifier) - RangeStart] == NULL )
            ptrNode->m_ptrChildren[(*p_ptrIdentifier) - RangeStart] = new typename TREE::Range<D, RangeStart, RangeEnd>::Node;
        ptrNode = ptrNode->m_ptrChildren[(*p_ptrIdentifier) - RangeStart];
    }

    if ( ptrNode->m_blnSet && p_blnOverWrite == false )
        return false;

    ptrNode->m_clsData = p_clsData;
    ptrNode->m_blnSet = true;
    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////
// Wrap
//////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
Changes values in the tree from one to another, where the target value can be
+/- a difference
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
Change(typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
       const T p_clsValueFrom,
       const T p_clsValueTo,
       const T p_clsAllowableDifference)
{
    if ( p_ptrNode->m_ptrPoints == NULL )
    {
        for (unsigned int i=0; i<(1 << Dimensions); i++)
        {
            if ( p_ptrNode->m_ptrChildren[i] )
            {
                TREE::Wrap<D, T, Dimensions>::Change(p_ptrNode->m_ptrChildren[i],
                                                     p_clsValueFrom,
                                                     p_clsValueTo,
                                                     p_clsAllowableDifference);
            }
        }
    }
    else
    {
        unsigned int uintPoints = p_ptrNode->m_uintPoints;
        const typename TREE::Wrap<D, T, Dimensions>::WrapPoint * ptrPoints = p_ptrNode->m_ptrPoints;
        for (unsigned int i=0; i<uintPoints; i++)
        {
            if ( p_clsValueFrom >= (ptrPoints[i].m_clsData - p_clsAllowableDifference) &&
                 p_clsValueFrom <= (ptrPoints[i].m_clsData + p_clsAllowableDifference) )
            {
                p_ptrNode->m_ptrPoints[i].m_clsData = p_clsValueTo;
            }
        }
    }
}

/*
Private routine that calculates the distance from a point to each child node
*/
template <class D, class T, unsigned int Dimensions>
unsigned int
TREE::Wrap<D, T, Dimensions>::
ChildrenDistances(const typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
                  const typename MATH::DN::Point<T, Dimensions> & p_stuPoint,
                  T * p_ptrDistances,
                  unsigned int * p_ptrIndices)
{
    unsigned int uintCount = 0;
    for (unsigned int i=0; i<(1 << Dimensions); i++)
    {
        if ( p_ptrNode->m_ptrChildren[i] )
        {
            p_ptrIndices[uintCount] = (unsigned char)(i);
            p_ptrDistances[uintCount] = p_ptrNode->m_ptrChildren[i]->m_stuCorners.DistanceW(p_stuPoint);
            for (unsigned int j=uintCount; j>0 && p_ptrDistances[j] < p_ptrDistances[j - 1]; j-- )
            {
                UTIL::Swap(p_ptrDistances + (j - 1), p_ptrDistances + j, sizeof(D));
                UTIL::Swap(p_ptrIndices + (j - 1), p_ptrIndices + j, sizeof(unsigned int));
            }
            uintCount++;
        }
    }

    return uintCount;
}

/*
Sets the minimum distance allowed when inserting points.  If a potential point
is closer to another point than this distance, it will not be inserted
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
Distance(const T p_clsDistance)
{
    if ( p_clsDistance >= (T)(0) )
    {
        m_blnDistanceSet = true;
        m_clsDistance = (p_clsDistance * p_clsDistance);
    }
    else
        m_blnDistanceSet = false;
}

/*
Private routine that calculates the distance to the closest point
*/
template <class D, class T, unsigned int Dimensions>
T
TREE::Wrap<D, T, Dimensions>::
Distance(const typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
         const typename MATH::DN::Point<T, Dimensions> & p_stuPoint)
{
    T clsDistance = UTIL::Max((T)(0));

    if ( p_ptrNode->m_ptrPoints == NULL )
    {
        //
        // Calculate the distance to each child and sort by shortest disance
        // first
        //
        T            clsDistances[1 << Dimensions];
        unsigned int uintIndices[1 << Dimensions];
        unsigned int uintCount = ChildrenDistances(p_ptrNode,
                                                   p_stuPoint,
                                                   clsDistances,
                                                   uintIndices);

        //
        // Check each child node, stopping if the child is farther than the
        // current shortest distance
        //
        for (unsigned int i=0; i<uintCount && clsDistance > clsDistances[i]; i++)
        {
            T clsTemp = TREE::Wrap<D, T, Dimensions>::Distance(p_ptrNode->m_ptrChildren[uintIndices[i]], p_stuPoint);
            if ( clsTemp < clsDistance )
                clsDistance = clsTemp;
        }
    }
    else
    {
        unsigned int uintPoints = p_ptrNode->m_uintPoints;
        const typename TREE::Wrap<D, T, Dimensions>::WrapPoint * ptrPoints = p_ptrNode->m_ptrPoints;
        for (unsigned int i=0; i<uintPoints; i++)
        {
            T clsTemp = ptrPoints[i].m_stuPoint.DistanceW(p_stuPoint);
            if ( clsTemp < clsDistance )
                clsDistance = clsTemp;
        }
    }

    return clsDistance;
}

/*
Calculates the range of values for a given area, or the entire tree if the area
is NULL
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Extremes(D & p_clsMinimum,
         D & p_clsMaximum,
         const typename MATH::DN::Box<T, Dimensions> * p_ptrCorners) const
{
    p_clsMinimum = UTIL::Max((D)(0));
    p_clsMaximum = UTIL::Min((D)(0));

    return TREE::Wrap<D, T, Dimensions>::Extremes(&m_stuRoot,
                                                  p_clsMinimum,
                                                  p_clsMaximum,
                                                  p_ptrCorners,
                                                  (p_ptrCorners == NULL));
}

/*
Private routine that calculates the range of values for a given area
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Extremes(const typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
         D & p_clsMinimum,
         D & p_clsMaximum,
         const typename MATH::DN::Box<T, Dimensions> * p_ptrCorners,
         bool p_blnEnclosed)
{
    //
    // If the parent node wasn't totally inside the search area, then check
    // this node
    //
    if ( p_blnEnclosed == false )
    {                        
        MATH::DN::ResultBox<T, Dimensions> clsResult;
        if ( p_ptrNode->m_stuCorners.IntersectW(*p_ptrCorners, &clsResult) == false )
            return false;
        p_blnEnclosed = ( clsResult.m_intType == 3 || clsResult.m_intType == 5 );
    }

    bool blnReturn = false;
    if ( p_ptrNode->m_ptrPoints == NULL )
    {
        //
        // Brance node, check each child
        //
        for (unsigned int i=0; i<(1 << Dimensions); i++)
        {
            if ( p_ptrNode->m_ptrChildren[i] &&
                 TREE::Wrap<D, T, Dimensions>::Extremes(p_ptrNode->m_ptrChildren[i],
                                                        p_clsMinimum,
                                                        p_clsMaximum,
                                                        p_ptrCorners,
                                                        p_blnEnclosed) )
            {
                blnReturn = true;
            }
        }
    }
    else
    {
        //
        // Leaf node, check each point
        //
        unsigned int uintPoints = p_ptrNode->m_uintPoints;
        const typename TREE::Wrap<D, T, Dimensions>::WrapPoint * ptrPoints = p_ptrNode->m_ptrPoints;
        if ( p_blnEnclosed )
        {
            //
            // This node is totally inside the search area, so no need to check
            // each point
            //
            blnReturn = ( uintPoints > 0 );
            for (unsigned int i=0; i<uintPoints; i++)
            {
                if ( p_clsMinimum > ptrPoints[i].m_clsData )
                    p_clsMinimum = ptrPoints[i].m_clsData;
                if ( p_clsMaximum < ptrPoints[i].m_clsData )
                    p_clsMaximum = ptrPoints[i].m_clsData;
            }
        }
        else
        {
            //
            // This node is not totally inside the area, so we need to check
            // each point
            //
            for (unsigned int i=0; i<uintPoints; i++)
            {
                if ( p_ptrCorners->IntersectW(ptrPoints[i].m_stuPoint) )
                {
                    blnReturn = true;
                    if ( p_clsMinimum > ptrPoints[i].m_clsData )
                        p_clsMinimum = ptrPoints[i].m_clsData;
                    if ( p_clsMaximum < ptrPoints[i].m_clsData )
                        p_clsMaximum = ptrPoints[i].m_clsData;
                }
            }
        }
    }

    return blnReturn;
}

/*
Sets the filter toggle and area.  If enabled, point insertion will succeed if
inside the filter area
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
Filter(const bool p_blnToggle,
       const typename MATH::DN::Box<T, Dimensions> * p_ptrCorners)
{
    m_blnFilter = p_blnToggle;
    if ( p_ptrCorners )
        m_stuFilter = *p_ptrCorners;
}

/*
Releases all resources
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
Finish(void)
{
    m_stuRoot.Finish();
    UTIL_DELETE_ARRAY(m_stuInterpolate.m_ptrPoints);
    UTIL_DELETE_ARRAY(m_stuInterpolate.m_ptrValues);
    UTIL_DELETE_ARRAY(m_stuInterpolate.m_ptrDistances);
    UTIL_DELETE_ARRAY(m_stuInterpolate.m_ptrDistancesTemp);
    memset(&m_stuInterpolate, 0, sizeof(typename TREE::Wrap<D, T, Dimensions>::InterpolateData));
}

/*
Private routine that inserts a data point
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Insert(typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
       const D & p_clsData,
       const MATH::DN::Point<T, Dimensions> & p_stuPoint)
{
    MATH::DN::Box<T, Dimensions> stuCorners;

    for (;;)
    {
        if ( p_ptrNode->m_ptrPoints == NULL )
        {
            //
            // Branch node;  find the child node that the point goes in
            //
            unsigned long ulngIndex = p_ptrNode->m_stuCorners.SectorW(p_stuPoint, &stuCorners);

            if ( p_ptrNode->m_ptrChildren[ulngIndex] == NULL )
                p_ptrNode->m_ptrChildren[ulngIndex] = new typename TREE::Wrap<D, T, Dimensions>::WrapNode(stuCorners);
            p_ptrNode = p_ptrNode->m_ptrChildren[ulngIndex];
        }
        else if ( p_ptrNode->m_uintPoints >= TREE_MAX_POINTS_PER_NODE )
        {
            //
            // Full leaf node;  make it a brance node
            //
            typename TREE::Wrap<D, T, Dimensions>::WrapPoint * ptrPoints = p_ptrNode->m_ptrPoints;
            unsigned int uintPoints = p_ptrNode->m_uintPoints;
            p_ptrNode->m_ptrPoints = NULL;
            p_ptrNode->m_uintPoints = 0;

            for (unsigned int i=0; i<uintPoints; i++)
            {
                TREE::Wrap<D, T, Dimensions>::Insert(p_ptrNode,
                                                     ptrPoints[i].m_clsData,
                                                     ptrPoints[i].m_stuPoint);
            }
            delete [] ptrPoints;
        }
        else
        {
            //
            // Leaf node;  make sure the point doesn't already exist
            //
            for (unsigned int i=0; i<p_ptrNode->m_uintPoints; i++)
            {
                if ( p_ptrNode->m_ptrPoints[i].m_stuPoint == p_stuPoint )
                    return false;
            }

            p_ptrNode->m_ptrPoints[p_ptrNode->m_uintPoints].m_stuPoint = p_stuPoint;
            p_ptrNode->m_ptrPoints[p_ptrNode->m_uintPoints].m_clsData = p_clsData;
            p_ptrNode->m_uintPoints++;
            return true;
        }
    }
}

/*
Finds values of data points nearby a position, but does not perform any averaging

The functionality provided here is similarly provided in other Interpolate()
routines, but this version is paired with
Interpolate(const D *, const unsigned int),  These two routines should be used
when the caller needs to manipulate the data point values after they are found,
but before they are averaged.  An example would be radians or degrees (0 and
360 would average to 180, which is not correct).

Example pseudocode:

SetInterpolateData()
...
Interpolate(D *)
...
(manipulate values)
...
Interpolate(const D *, const unsigned int)
*/

template <class D, class T, unsigned int Dimensions>
unsigned int
TREE::Wrap<D, T, Dimensions>::
Interpolate(D * p_ptrValues) const
{
    unsigned int i;
    for (i=0; i<=m_stuInterpolate.m_uintPoints; i++)
    {
        m_stuInterpolate.m_ptrDistances[i] = m_stuInterpolate.m_clsMaxDistance;
        m_stuInterpolate.m_ptrPoints[i] = NULL;
    }

    TREE::Wrap<D, T, Dimensions>::Interpolate(&m_stuRoot, m_stuInterpolate);

    for (i=0; i<m_stuInterpolate.m_uintPoints && m_stuInterpolate.m_ptrPoints[i] != NULL; i++)
        p_ptrValues[i] = m_stuInterpolate.m_ptrPoints[i]->m_clsData;

    return i;
}

/*
Uses the inverse distance weighted algorithm to find an average value

See comments for Interpolate(D *)
*/
template <class D, class T, unsigned int Dimensions>
D
TREE::Wrap<D, T, Dimensions>::
Interpolate(const D * p_ptrValues,
            const unsigned int p_uintAmount) const
{
    unsigned int i;
    T clsTotalDistance = (T)(0);
    for (i=0; i<p_uintAmount; i++)
    {
        if ( m_stuInterpolate.m_ptrDistances[i] )
        {
            m_stuInterpolate.m_ptrDistancesTemp[i] = (T)(1.0 / pow((double)(m_stuInterpolate.m_ptrDistances[i]), (double)(m_stuInterpolate.m_clsAveragePower)));
            clsTotalDistance += m_stuInterpolate.m_ptrDistancesTemp[i];
        }
        else
            m_stuInterpolate.m_ptrDistancesTemp[i] = (T)(0);
    }

    D clsResult = (D)(0);
	if ( clsTotalDistance > (T)(0) )
	{
		clsTotalDistance = (T)(1) / clsTotalDistance;
		for (i=0; i<p_uintAmount; i++)
			clsResult += ((D)(m_stuInterpolate.m_ptrDistancesTemp[i] * clsTotalDistance)) * p_ptrValues[i];
	}
	else
	{
		for (i=0; i<p_uintAmount; i++)
			clsResult += p_ptrValues[i];
		clsResult /= (D)(i);
	}
    return clsResult;
}

/*
Computes the average data value of a position using nearby data points and the
inverse distance weighted algorithm.

The functionality provided here is similarly provided in other Interpolate()
routines, but this version takes no direct arguements and gets all it's
parameters from value specified in SetInterpolateData().
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Interpolate(void) const
{
    //
    // Initialize the interpolation arrays, which are updated in the private
    // Interpolate() routine
    //
    unsigned int i;
    for (i=0; i<=m_stuInterpolate.m_uintPoints; i++)
    {
        m_stuInterpolate.m_ptrDistances[i] = m_stuInterpolate.m_clsMaxDistance;
        m_stuInterpolate.m_ptrPoints[i] = NULL;
    }

    //
    // Find the closest points and their data values
    //
    TREE::Wrap<D, T, Dimensions>::Interpolate(&m_stuRoot, m_stuInterpolate);

    return TREE::Wrap<D, T, Dimensions>::Interpolate(m_stuInterpolate);
}

/*
Computes the average data value of a position using nearby data points and the
inverse distance weighted algorithm.

The functionality provided here is similarly provided in other Interpolate()
routines, but this version takes direct arguements.
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Interpolate(const T * p_ptrPoint,
            const unsigned int p_uintPoints,
            const T p_clsMaxDistance,
            const T p_clsAveragePower,
            D * p_ptrResult) const
{
    InterpolateData stuInterpolate;

    stuInterpolate.m_ptrResult = p_ptrResult;
    stuInterpolate.m_uintPoints = p_uintPoints;
    stuInterpolate.m_clsMaxDistance = p_clsMaxDistance;
    stuInterpolate.m_clsAveragePower = p_clsAveragePower;
    stuInterpolate.m_ptrPoint = (const MATH::DN::Point<T, Dimensions> *)(p_ptrPoint);
    stuInterpolate.m_ptrDistances = (T *)(alloca((p_uintPoints + 1) * sizeof(T)));
    stuInterpolate.m_ptrPoints = (WrapPoint **)(alloca((p_uintPoints + 1) * sizeof(WrapPoint *)));
    if ( stuInterpolate.m_ptrPoints == NULL || stuInterpolate.m_ptrDistances == NULL )
        return false;

    //
    // Initialize the interpolation arrays, which are updated in the private
    // Interpolate() routine
    //
    for (unsigned int i=0; i<=p_uintPoints; i++)
    {
        stuInterpolate.m_ptrDistances[i] = p_clsMaxDistance;
        stuInterpolate.m_ptrPoints[i] = NULL;
    }

    //
    // Find the closest points and their data values
    //
    TREE::Wrap<D, T, Dimensions>::Interpolate(&m_stuRoot, stuInterpolate);

    return TREE::Wrap<D, T, Dimensions>::Interpolate(stuInterpolate);
}

/*
Private routine that finds a certain number of points close to a position
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
Interpolate(const typename TREE::Wrap<D, T, Dimensions>::WrapNode * p_ptrNode,
            const typename TREE::Wrap<D, T, Dimensions>::InterpolateData & p_stuData) 
{
    if ( p_ptrNode->m_ptrPoints == NULL )
    {
        //
        // Branch node; check each child in the order that they are closest to
        // the position
        //
        T            clsDistances[1 << Dimensions];
        unsigned int uintIndices[1 << Dimensions];
        unsigned int uintCount = TREE::Wrap<D, T, Dimensions>::ChildrenDistances(p_ptrNode,
                                                                                 *(p_stuData.m_ptrPoint),
                                                                                 clsDistances,
                                                                                 uintIndices);
        for (unsigned int i=0; i<uintCount && clsDistances[i] < p_stuData.m_ptrDistances[p_stuData.m_uintPoints - 1]; i++)
            TREE::Wrap<D, T, Dimensions>::Interpolate(p_ptrNode->m_ptrChildren[uintIndices[i]], p_stuData);
    }
    else
    {
        //
        // Leaf node; check each point for how close it is to the position
        //
        T   clsDistance;
        T * ptrDistances = p_stuData.m_ptrDistances;
        typename TREE::Wrap<D, T, Dimensions>::WrapPoint *  ptrPoint;
        typename TREE::Wrap<D, T, Dimensions>::WrapPoint ** ptrPoints = p_stuData.m_ptrPoints;
        unsigned int uintPoints = p_stuData.m_uintPoints;
        for (unsigned int i=0; i<p_ptrNode->m_uintPoints; i++)
        {
            ptrPoints[uintPoints] = p_ptrNode->m_ptrPoints + i;
            ptrDistances[uintPoints] = p_ptrNode->m_ptrPoints[i].m_stuPoint.DistanceW(*(p_stuData.m_ptrPoint));

            for (unsigned int j=uintPoints; j>0 && ptrDistances[j] < ptrDistances[j - 1]; j--)
            {
                clsDistance = ptrDistances[j - 1];
                ptrDistances[j - 1] = ptrDistances[j];
                ptrDistances[j] = clsDistance;

                ptrPoint = ptrPoints[j - 1];
                ptrPoints[j - 1] = p_stuData.m_ptrPoints[j];
                ptrPoints[j] = ptrPoint;
            }
        }
    }
}

/*
Private routine that performs the inverse distance weighted althorithm
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Interpolate(const typename TREE::Wrap<D, T, Dimensions>::InterpolateData & p_stuData)
{
    //
    // If only one data point was found, no averaging is necessary
    //
    if ( p_stuData.m_uintPoints > 1 && p_stuData.m_ptrPoints[1] != NULL )
    {
        //
        // Compute the sum of the inverse distances weights
        //
        unsigned int i;
        T            clsTotalDistance = (T)(0);
        for (i=0; i<p_stuData.m_uintPoints && p_stuData.m_ptrPoints[i] != NULL; i++)
        {
            if ( p_stuData.m_ptrDistances[i] )
            {
                p_stuData.m_ptrDistancesTemp[i] = (T)(1.0 / pow((double)(p_stuData.m_ptrDistances[i]), (double)(p_stuData.m_clsAveragePower)));
                clsTotalDistance += p_stuData.m_ptrDistancesTemp[i];
            }
            else
                p_stuData.m_ptrDistancesTemp[i] = (T)(0);
        }

        //
        // Compute the weighted average
        //
        D clsResult = (D)(0);
	    if ( clsTotalDistance > (T)(0) )
	    {
		    clsTotalDistance = (T)(1) / clsTotalDistance;
		    for (unsigned int j=0; j<i; j++)
			    clsResult = (D)(clsResult + (p_stuData.m_ptrPoints[j]->m_clsData * (p_stuData.m_ptrDistancesTemp[j] * clsTotalDistance)));
            *(p_stuData.m_ptrResult) = clsResult;
	    }
	    else
	    {
		    for (unsigned int j=0; j<i; j++)
			    clsResult = (D)(clsResult + p_stuData.m_ptrPoints[j]->m_clsData);
		    *(p_stuData.m_ptrResult) = (D)(clsResult / (D)(i));
	    }
        return true;
    }
    else if ( p_stuData.m_ptrPoints[0] != NULL )
    {
        *(p_stuData.m_ptrResult) = p_stuData.m_ptrPoints[0]->m_clsData;
        return true;
    }
    else
        return false;
}

/*
Inserts a data point into the tree
*/
template <class D, class T, unsigned int Dimensions>
bool
TREE::Wrap<D, T, Dimensions>::
Insert(const D & p_clsData,
       const T * p_ptrPoint,
       const unsigned int p_uintMask)
{
    //
    // Use default values for those dimensions masked out
    //
    MATH::DN::Point<T, Dimensions> stuPoint;
    for (unsigned int i=0; i<Dimensions; i++)
        stuPoint[i] = ( p_uintMask & (1 << i) ) ? p_ptrPoint[i] : m_stuDefaultValues[i];

    //
    // Reject the point if it is filtered out or is outside the tree area
    //
    if ( (m_blnFilter && m_stuFilter.IntersectW(stuPoint) == false) ||
          m_stuRoot.m_stuCorners.IntersectW(stuPoint) == false )
    {
        return false;
    }

    return ( m_blnDistanceSet == false || m_clsDistance < Distance(&m_stuRoot, stuPoint) )
        ? TREE::Wrap<D, T, Dimensions>::Insert(&m_stuRoot, p_clsData, stuPoint)
        : false;
}

/*
Sets parameters used by versions of Interpolate().  Those parameters that are
references should remain valid for as long as Interpolate() is called, with the
exception of the Interpolate() version that has all parameters directly passed
to it
*/
template <class D, class T, unsigned int Dimensions>
void
TREE::Wrap<D, T, Dimensions>::
SetInterpolateData(const unsigned int p_uintPoints,
                   const typename MATH::DN::Point<T, Dimensions> * p_ptrPoint,
                   const T p_clsMaxDistance,
                   const T p_clsAveragePower,
                   D * p_ptrResult)
{
    if ( m_stuInterpolate.m_uintPoints != p_uintPoints )
    {
        if ( m_stuInterpolate.m_ptrPoints )
            delete [] m_stuInterpolate.m_ptrPoints;
        if ( m_stuInterpolate.m_ptrValues )
            delete [] m_stuInterpolate.m_ptrValues;
        if ( m_stuInterpolate.m_ptrDistances )
            delete [] m_stuInterpolate.m_ptrDistances;
        if ( m_stuInterpolate.m_ptrDistancesTemp )
            delete [] m_stuInterpolate.m_ptrDistancesTemp;

        m_stuInterpolate.m_ptrValues = new D[p_uintPoints];
        m_stuInterpolate.m_ptrPoints = new WrapPoint *[p_uintPoints + 1];
        m_stuInterpolate.m_ptrDistances = new T[p_uintPoints + 1];
        m_stuInterpolate.m_ptrDistancesTemp = new T[p_uintPoints];
    }

    m_stuInterpolate.m_ptrPoint = p_ptrPoint;
    m_stuInterpolate.m_uintPoints = p_uintPoints;
    m_stuInterpolate.m_ptrResult = p_ptrResult;
    m_stuInterpolate.m_clsMaxDistance = p_clsMaxDistance;
    m_stuInterpolate.m_clsAveragePower = p_clsAveragePower;
}

#endif