/*!
 * \file fgngRTree.cpp
 *
 * \brief Class implementation FGngRTree
 *
 * \author Carlos Augusto Teixeira Mendes
 * \date september, 2011
 */

#include <assert.h>
#include <float.h>
#include <math.h>

#include "fgngRTree.h"
#include "fgng.h"

#include <QList>
#include <QPair>

#ifdef FGNG_RTREE_NN_STATS
int FGngRTree::FGngRTreeVisitedNodes = 0;
#endif


/*! 
\class FGngRTree

Future adjustments:
 - Include index along with father pointer and pointer of the FGngNode to the RTree 
 - Use a better allocator?
 - evaluate split strategy?
*/

//! Helper class representing a "n" dimensional AABB (axis aligned bounding box) 
class FGngRTreeRect
{
public:
  //! Creates a rectangle invalid, identified by _min [0]> _Max [0]
  FGngRTreeRect()   { _min[0] = 1.0; _max[0] = 0.0; }

  //! Creates a rectangle initialized with the values received as parameters
  FGngRTreeRect(const FGngNode* point)
  {
    assert(point);
    const double* pos = point->pos();
    for(int i=0; i<FGNG_DIMENSION; i++) { _min[i] = _max[i] = pos[i]; }
  }

  //! Creates a rectangle initialized with the values received as parameters
  FGngRTreeRect(const FGngRTreeRect& rect)
  {
    assert(rect.isValid());
    for(int i=0; i<FGNG_DIMENSION; i++) { _min[i] = rect._min[i]; _max[i] = rect._max[i]; }
  }

  //! Updates rectangle. Returns false if values are equal to the previous values
  bool setRect(const FGngRTreeRect& rect)
  {
    assert(rect.isValid());
    bool changed = false;
    for(int i=0; i<FGNG_DIMENSION; i++) 
    { 
      if(_min[i] != rect._min[i] || _max[i] != rect._max[i])
      {
        _min[i] = rect._min[i]; 
        _max[i] = rect._max[i]; 
        changed = true;
      }
    }
    return changed;
  }

  //! Returns false if the rectangle is not valid
  bool isValid() const { return _min[0] <= _max[0]; }

  //! Alters if necessary, to the bounding box to enclose the area received as a parameter
  void addRect(const FGngRTreeRect& rect)
  {
    assert(isValid() && rect.isValid());
    for(int i=0; i<FGNG_DIMENSION; i++)
    {
      _min[i] = qMin(_min[i], rect._min[i]);
      _max[i] = qMax(_max[i], rect._max[i]);
    }
  }

  /*! \brief Alters if necessary, to the bounding box to enclose the point received as a parameter

    Returns true if the bounding box was changed, false otherwise (the point was
    internal to the current rectangle)
  */
  bool addPoint(const FGngNode* point)
  {
    bool changed = false;
    assert(point && isValid());
    const double* pointPos = point->pos();
    for(int i=0; i<FGNG_DIMENSION; i++)
    {
      if(pointPos[i] < _min[i]) 
      { 
        _min[i] = pointPos[i]; 
        changed = true; 
      }
      else if(pointPos[i] > _max[i])
      { 
        _max[i] = pointPos[i]; 
        changed = true; 
      }
    }
    return changed;
  }

  //! Calculate the volume increase which would be generated if the received rectangle were included in this bb
  double calcVolumeIncrease(const FGngRTreeRect& rect, double* currentVol) const
  {
    assert(isValid() && rect.isValid());
    double vol    = 1;
    double newVol = 1;
    for(int i=0; i<FGNG_DIMENSION; i++)
    {
      vol    *= _max[i] - _min[i];
      newVol *= qMax(_max[i], rect._max[i]) - qMin(_min[i], rect._min[i]);
    }        
    *currentVol = vol;
    return newVol - vol;
  }

  //! Calculate the volume occupied by the n dimensional bounding box 
  double volume() const
  {
    assert(isValid());
    double vol = _max[0] - _min[0];
    for(int i=1; i<FGNG_DIMENSION; i++) vol *= (_max[i] - _min[i]);
    return vol;
  }

  /*! \brief Returns the square of the distance between the point and the bounding box.
             Returns zero if the point is inside the box.
   
        It also calculates and returns minMaxDist, the greatest possible distance between the
        point and a point belonging to the box, note that all box hyperplanes 
        have at least one point (metric MINMAXDIST).
  */
  inline double squaredPointToRectDistance(const double* point, double* maxDist) const
  {
    // Calcula MINDIST e MINMAXDIST
    double dist = 0.0;
    *maxDist = DBL_MAX;

    double mid[FGNG_DIMENSION];
    for(int j=0; j<FGNG_DIMENSION; j++)
      mid[j]  = (_min[j] + _max[j]) / 2.0;

    for(int i=0; i<FGNG_DIMENSION; i++) 
    {
      if(point[i] < _min[i])
        dist += (_min[i] - point[i])*(_min[i] - point[i]);
      else if(point[i] > _max[i])
        dist += (point[i] - _max[i])*(point[i] - _max[i]);

      double maxd = 0.0;
      for(int j=0; j<FGNG_DIMENSION; j++)
      {
        double d;
        if(j==i)
          d = (point[j] <= mid[j]) ? _min[j] : _max[j];
        else
          d = (point[j] >= mid[j]) ? _min[j] : _max[j];
        maxd += (point[j] - d) * (point[j] - d);        
      }
      if(maxd < *maxDist)
        *maxDist = maxd;
    }
    return dist;
  }

  /*! \brief eturns the square of the distance between the point and the bounding box.
             Returns zero if the point is inside the box.
  */
  inline double squaredPointToRectDistance(const double* point) const
  {
    double dist = 0.0;
    for(int i=0; i<FGNG_DIMENSION; i++) 
    {
      if(point[i] < _min[i])
        dist += (_min[i] - point[i])*(_min[i] - point[i]);
      else if(point[i] > _max[i])
        dist += (point[i] - _max[i])*(point[i] - _max[i]);
    }
    return dist;
  }

  /*! \brief Returns the square of the distance between the point and
              point stored in the current rectangle (ie _min [i] == _Max [i]).
  */
  inline double squaredPointToPointDistance(const double* point) const
  {
    double dist = 0.0;
    for(int i=0; i<FGNG_DIMENSION; i++) 
    {
      assert(_min[i] == _max[i]);
      dist += (_min[i] - point[i])*(_min[i] - point[i]);
    }
  return dist;
  }


#ifdef FGNG_RTREE_CHECK
  //! Checks consistency of stored values
  void check()
  {
    for(int i=0; i<FGNG_DIMENSION; i++) assert(_min[i] <= _max[i]);
  }
#endif

  double _min[FGNG_DIMENSION];  //!< Coordinates of the corner with smallest coordinates value in each axis
  double _max[FGNG_DIMENSION];  //!<  Coordinates of the corner with largest coordinates value in each axis
};


/*! \brief Auxiliary class used to represent a connection node
            with your child or stored data if the node is a leaf

If it is contained in an internal node, it contains the bounding box of the child in
_rect and a pointer to the child node, type * FGngRTreeNode in  _data.
If it is contained in a leaf node, it contains the bounding box of the point in 
_rect and a pointer to the node of the graph, type * FGngNode in _data.
*/
class FGngRTreeChildData
{
public:
  //! Constructs an invalid ChildData  to be filled later
  FGngRTreeChildData()
    : _data(NULL) {}

  //! Builds the ChildData from a point
  FGngRTreeChildData(FGngNode* pointNode)
    : _rect(pointNode), _data(pointNode) {}

  FGngRTreeChildData(FGngRTreeNode* childNode);

  //! Returns a reference to the bounding box of the child
  FGngRTreeRect& rect() { return _rect; }

  //! Retorna ponteiro para o nó do grafo
  FGngNode* dataPtr() { return (FGngNode*)_data; }

  //! Returns pointer to the child node
  FGngRTreeNode* childPtr() { return (FGngRTreeNode*)_data; }

private:
  FGngRTreeRect _rect;  //!< Bounding box associated with _data
  void*         _data;  //!< Pointer to the child node, type FGngRTreeNode or FGngNode
};


/*! \brief Auxiliary structure used to represent a node in the tree.

Each node stores a pointer to the parent so as to allow
functions that update data or remove data from the tree does not need
do a search from the root. The level of the node in the tree is also
stored with a dual function: to facilitate the reintegration of removed nodes and 
the identification leaf nodes (with level zero).

It is not a class, but a C style structure.

Nodes must always be allocated by function FGngRTree :: alocNode () that
allocates space for the complete structure added to the vector space
Data _children.
*/
struct FGngRTreeNode
{
  FGngRTreeNode*     _parent;      //!< Pointer to the parent node
  int                _nchilds;     //!< Number of children of this node
  int                _level;       //!< Level that the node occupies in the tree. Level 0 = leaf
  FGngRTreeChildData _children[0]; //!< Vector data node's children.
};


//! Builds the ChildData from another node of the tree, calculating its the bounding box
FGngRTreeChildData::FGngRTreeChildData(FGngRTreeNode* childNode)
  : _rect(childNode->_children[0].rect()), _data(childNode)
{
  for(int i=1; i<childNode->_nchilds; i++)
    _rect.addRect(childNode->_children[i].rect());
}


//-----------------------------

//! Priority queue used by the alg. that discovers the nearest neighbor
class FGngRTreePQueue
{
public:

  //! Inserts a new node with "distance" as the priority in the queue
  void insert(FGngRTreeNode* node, double distance)
  {
    _data.append(qMakePair(distance, node));  
    int index = _data.size()-1;
    QPair<double, FGngRTreeNode*> val = _data[index];
    int p = parent(index);
    while(index > 0 && _data[p].first > val.first)
    {
      _data[index] = _data[p];
      index = p;
      p = parent(index);
    }
    _data[index] = val;
  }

  //! Removes and returns the smallest value of the queue
  FGngRTreeNode* remove(double* distance)
  {
    assert(_data.size());

    FGngRTreeNode* result = _data[0].second;
    *distance = _data[0].first;

    QPair<double, FGngRTreeNode*> value = _data.takeLast();
    if(!_data.size())
      return result;

    _data[0] = value;

    int index    = 0;
    int size     = _data.size();
    int minchild = left(0);
    while(minchild < size)
    {
      // Checks whether the child's right is smaller than the left
      if(minchild+1 < size && _data[minchild+1].first < _data[minchild].first)
        minchild++;
      //If a minor child is less than value, exchange. if not, done
      if(_data[minchild].first < value.first)
      {
        _data[index] = _data[minchild];
        index        = minchild;
        minchild     = left(index);
      }
      else
        break;
    }
    _data[index] = value;

    return result;
  }

  //! Returns true if the priority queue is empty
  bool isEmpty() const { return _data.isEmpty(); }

private: 
  int parent(int index) { return (index-1) / 2; }
  int left  (int index) { return 2 * index + 1; }

  QList< QPair<double, FGngRTreeNode*> > _data; //!< data stored in the priority queue
};

//! Auxiliary structure used by nearestNeighboursRKVPP
struct FGngRTreeRKVPPContext
{
  double    minDist1;     // Shortest distance
  double    minDist2;     // Second shortest distance
  FGngNode* node1;        // Node associated with minDist1
  FGngNode* node2;        // Node associated with minDist2
  FGngRTreeNode* ppNode1; // Promise associated with minDist1 
  FGngRTreeNode* ppNode2; // Promise associated with minDist2
};


/*! \brief Builder. Calculates the minimum and maximum size of a node

\ param maxChilds Maximum number of children per node
\ param minChilds Minimum number of children per node
\ param NNalg algorithm used to search for neighbors of a point
                  0 = default = PP RKV
                  1 = HS
                  2 = HS modified
                  3 PP = RKV
*/
FGngRTree::FGngRTree(int maxChilds, int minChilds, int NNalg)
{
  assert(minChilds <= maxChilds/2);
  _minChilds   = minChilds;
  _maxChilds   = maxChilds;
  _root        = alocNode();
  _nodeSet     = new FGngRTreeChildData[maxChilds + 1];
  _numElements = 0;

  switch(NNalg)
  {
    case 0: _nnFunc = &FGngRTree::nearestNeighboursRKVPP;   break;
    case 1: _nnFunc = &FGngRTree::nearestNeighboursHS;      break;
    case 2: _nnFunc = &FGngRTree::nearestNeighboursHSModif; break;
    case 3: _nnFunc = &FGngRTree::nearestNeighboursRKVPP;   break;
    default:
      assert(0);
  }
}

//! Destrutor
FGngRTree::~FGngRTree()
{
  clear(_root);
  delete[] _nodeSet;
}

//! Clears the tree
void FGngRTree::clear(FGngRTreeNode* node)
{
  if(!isLeaf(node))
  {
    for(int i=0; i<node->_nchilds; i++)
      clear(node->_children[i].childPtr());
  }
  freeNode(node);
}

/*! \brief Inserts a new node in the tree.

Updates reference in node to the leaf of the tree where the node was inserted
*/
void FGngRTree::insertNode(FGngNode* pointNode)
{
  insertNode(FGngRTreeChildData(pointNode), 0);

  _numElements++;
}

/*! \brief Removes a node from the tree

Node position in the tree is obtained by the information in "node"
*/
void FGngRTree::removeNode(FGngNode* node)
{
  // Find the tree node that contains "node"
  FGngRTreeNode* p = node->rtreeNode();
  int i = 0;
  while(p->_children[i].dataPtr() != node) i++;

  // Remove node of p
  p->_children[i] = p->_children[p->_nchilds-1];
  p->_nchilds--;

  // Climbs the tree adjusting bounding boxes
  condenseTree(p);

  // If the root was left with only one entry, reduces the height of the tree
  if(_root->_nchilds == 1 && !isLeaf(_root))
  {
    FGngRTreeNode* oldRoot = _root;
    _root = oldRoot->_children[0].childPtr();
    _root->_parent = NULL;
    freeNode(oldRoot);
  }

  _numElements--;
}

//! Refreshes the tree to reflect the new position of node (already updated in node)
void FGngRTree::nodeUpdated(FGngNode* node)
{
#if 1
  // Adjusts bounding boxes up to the root.
  FGngRTreeNode* p = node->rtreeNode();
  int i = 0;
  while(p->_children[i].dataPtr() != node) i++;
  p->_children[i].rect() = FGngRTreeRect(node);

  adjustTree(p, NULL, NULL);
#else
// - If bb has not changed, no need to do anything
// - If bb is reduced, go up the tree while reduction affect bb's father
// - If bb is increased, remove node and reinserts it

// Find out the parent node (p) and the index (i) of node p
// If p is the root, nothing to do.

  FGngRTreeNode* p = node->rtreeNode();
  int i = 0;
  while(p->_children[i].dataPtr() != node) i++;
  if(p == _root)
  {
    p->_children[i].rect() = FGngRTreeRect(node);
    return;
  }
  FGngRTreeRect oldNodePos = p->_children[i].rect();
  p->_children[i].rect() = FGngRTreeRect(node);

  // Find out the bounding box of p
  FGngRTreeNode* pparent = p->_parent;
  int j = 0;
  while(pparent->_children[j].childPtr() != p) j++;
  FGngRTreeRect& r = pparent->_children[j].rect();

  // Checks whether the change in position modified the bounding box of the node ...
  const double* newNodePos = node->pos();
  bool shrink = false;
  for(int k=0; k<FGNG_DIMENSION; k++)
  {
    if(newNodePos[k] < r._min[k] || newNodePos[k] > r._max[k])
    {
      // BB. increased. Removes and reinserts the node
      removeNode(node);
      insertNode(node);
      return;
    }
    if((oldNodePos._min[k] == r._min[k] && newNodePos[k] > r._min[k]) ||
       (oldNodePos._max[k] == r._max[k] && newNodePos[k] < r._max[k]))
      shrink = true;
  }
  if(shrink)
    adjustTree(p, NULL, NULL);
#endif  
}



/*! \brief Returns the two nodes closest to the query point.

Uses the algorithm HS

Assumes that tree has at least two values
*/
void FGngRTree::nearestNeighboursHS(const double* position, FGngNode** n1, FGngNode** n2) const
{
  assert(_numElements >= 2);

  FGngRTreePQueue pqueue;
  double minDist1 = DBL_MAX;  // Shortest distance
  double minDist2 = DBL_MAX;  // Second shortest distance

  pqueue.insert(_root, 0);

  while(!pqueue.isEmpty())
  {
    double boxDist;
    FGngRTreeNode* node = pqueue.remove(&boxDist);

#ifdef FGNG_RTREE_NN_STATS
  FGngRTreeVisitedNodes++;
#endif

    // If the distance from point A to box is greater than the shortest distance already found, returns
    if(boxDist > minDist2) break;

    if(isLeaf(node)) //Tests distance of the points on the leaft to position
    {
      for(int i=0; i<node->_nchilds; i++)
      {
        double d = node->_children[i].rect().squaredPointToPointDistance(position);
        if(d < minDist1)
        {
          minDist2 = minDist1;
          *n2      = *n1;
          minDist1 = d;
          *n1      = node->_children[i].dataPtr();
        }
        else if(d < minDist2)
        {
          minDist2 = d;
          *n2      = node->_children[i].dataPtr();
        }
      }
    }
    else // Inserts children in priority queue
    {
      for(int i=0; i<node->_nchilds; i++)
      {
        double d = node->_children[i].rect().squaredPointToRectDistance(position);
        pqueue.insert(node->_children[i].childPtr(), d);
      }
    }
  }
}

/*! \brief Returns the two nodes closest to the query point.

Uses the HS algorithm modified by the use of the metric MINMAXDIST

Assumes that tree has at least two values
*/
void FGngRTree::nearestNeighboursHSModif(const double* position, FGngNode** n1, FGngNode** n2) const
{
  assert(_numElements >= 2);

  FGngRTreePQueue pqueue;
  double minDist1   = DBL_MAX;   // Shortest distance
  double minDist2   = DBL_MAX;   // Second shortest distance
  double minMaxDist1 = DBL_MAX;  // Shorter among the larger distances from point to the box
  double minMaxDist2 = DBL_MAX;  // Second shortest distance among the larger distances from point to the box
  FGngRTreeNode* minMaxDist1Node = NULL;

  pqueue.insert(_root, 0);

  while(!pqueue.isEmpty())
  {
    double boxDist;
    FGngRTreeNode* node = pqueue.remove(&boxDist);

#ifdef FGNG_RTREE_NN_STATS
  FGngRTreeVisitedNodes++;
#endif

    // If the distance from point A to box is greater than the shortest distance already found, returns
    if(boxDist > minDist2) break;

    if(isLeaf(node)) // Tests distance of the points on the leaf to position
    {
      for(int i=0; i<node->_nchilds; i++)
      {
        double d = node->_children[i].rect().squaredPointToPointDistance(position);
        if(d < minDist1)
        {
          minDist2 = minDist1;
          *n2      = *n1;
          minDist1 = d;
          *n1      = node->_children[i].dataPtr();
        }
        else if(d < minDist2)
        {
          minDist2 = d;
          *n2      = node->_children[i].dataPtr();
        }
      }
    }
    else // Insere filhos na fila de prioridades
    {
      for(int i=0; i<node->_nchilds; i++)
      {
        // Calculates the minimum and maximum distance between the point and the elements box, reminding
        // all hyperplanes of the box have at least one point
        double maxDist;
        double minDist = node->_children[i].rect().squaredPointToRectDistance(position, &maxDist);

        // If the shortest distance to the box is larger than the maximum distance to the boxes 
        // already processed, do not need to put this box in the queue
        if(minDist > minMaxDist2)
          continue;

        // Update minMaxDist
        if(maxDist <= minMaxDist1) // <= critical to ensure that ptrs comparison is made only with parent
        {
          if(node != minMaxDist1Node)
            minMaxDist2 = minMaxDist1;
          minMaxDist1     = maxDist;
          minMaxDist1Node = node->_children[i].childPtr();
        }
        else if(maxDist < minMaxDist2)
          minMaxDist2 = maxDist;

        // Insere nó na fila de prioridades
        pqueue.insert(node->_children[i].childPtr(), minDist);
      }
    }
  }
}

/*! \brief Returns the two nodes closest to the query point.

uses the algorithm RKV PP

Assumes that tree has at least two values
*/
void FGngRTree::nearestNeighboursRKVPP(const double* position, FGngNode** n1, FGngNode** n2) const
{
  assert(_numElements >= 2);
  assert(_maxChilds <= 64); // Remove when alter vector in nearestNeighboursRKVPPAux

  FGngRTreeRKVPPContext context = {DBL_MAX, DBL_MAX, NULL, NULL, NULL, NULL};

  nearestNeighboursRKVPPAux(_root, position, &context);

  *n1 = context.node1;
  *n2 = context.node2;
  assert(*n1 && *n2);
}

//! Recursive function used in nearestNeighboursRKVPP
void FGngRTree::nearestNeighboursRKVPPAux(FGngRTreeNode* node, const double* position, FGngRTreeRKVPPContext* context) const
{
#ifdef FGNG_RTREE_NN_STATS
  FGngRTreeVisitedNodes++;
#endif

  if(isLeaf(node))
  {
    // Compares object distances with minimum. updated context
    for(int i=0; i<node->_nchilds; i++)
    {
      double d = node->_children[i].rect().squaredPointToPointDistance(position);
      if(d < context->minDist1)
      {
        context->minDist2 = context->minDist1;
        context->node2    = context->node1;
        context->ppNode2  = context->ppNode1;
        context->minDist1 = d;
        context->node1    = node->_children[i].dataPtr();
        context->ppNode1  = NULL;
      }
      else if(d < context->minDist2)
      {
        context->minDist2 = d;
        context->node2    = node->_children[i].dataPtr();
        context->ppNode2  = NULL;
      }
    }
  }
  else
  {
    // Generate ABL sorted by the distance to the point
    // Insertion sort should be better than Quick sort since the set is small
    int childrenPos[64];
    double minDist[64];
    double minMaxDist[64];
    for(int i=0; i<node->_nchilds; i++)
    {
      double maxDist;
      double d = node->_children[i].rect().squaredPointToRectDistance(position, &maxDist);
      int j;
      for(j=i-1; j>= 0 && minDist[j] > d; j--)
      {
        childrenPos[j+1] = childrenPos[j];
        minDist[j+1]     = minDist[j];
        minMaxDist[j+1]  = minMaxDist[j];
      }
      childrenPos[j+1] = i;
      minDist[j+1]     = d;
      minMaxDist[j+1]  = maxDist;
    }
    // Promise Pruning (PP)
    for(int i=0; i<node->_nchilds; i++)
    {
      if(minMaxDist[i] < context->minDist1)
      {
        context->minDist2 = context->minDist1;
        context->node2    = context->node1;
        context->ppNode2  = context->ppNode1;
        context->minDist1 = minMaxDist[i];
        context->node1    = NULL;
        context->ppNode1  = node->_children[childrenPos[i]].childPtr();
      }
      else if(minMaxDist[i] < context->minDist2)
      {
        context->minDist2 = minMaxDist[i];
        context->node2    = NULL;
        context->ppNode2  = node->_children[childrenPos[i]].childPtr();
      }
    }
    // K1 Prunning and recursive call to children
    for(int i=0; i<node->_nchilds; i++)
    {
      // Use <= (not < as in the article) this is important because the case of a degenerate BB
      // where one of the dimensions have size zero, MINDIST = MINMAXDIST and the promise eventually
      // put in the previous context will remove it
      if(minDist[i] <= context->minDist2) 
      {
        FGngRTreeNode* child = node->_children[childrenPos[i]].childPtr();
        if(context->ppNode1 == child)
        {
          assert(context->ppNode2 != child);
          context->minDist1 = context->minDist2;
          context->node1    = context->node2;
          context->ppNode1  = context->ppNode2;
          context->minDist2 = DBL_MAX;
          context->node2    = NULL;
          context->ppNode2  = NULL;
        }
        else if(context->ppNode2 == child) 
        {
          context->minDist2 = DBL_MAX;
          context->node2    = NULL;
          context->ppNode2  = NULL;
        }
        nearestNeighboursRKVPPAux(child, position, context);
      }
    }
  }
}

//! Check if the node is a leaf
inline bool FGngRTree::isLeaf(const FGngRTreeNode* node) const
{
  assert(node);
  return node->_level == 0;
}

//! Inserts data in the especified level 
void FGngRTree::insertNode(FGngRTreeChildData& data, int level)
{
  // find the node where the point must be included
  FGngRTreeNode* node = chooseNode(data.rect(), level);

  // Inserts pointNode in the chosen node 
  FGngRTreeNode* splitNode = addChildData(node, data, level == 0); 

  // Adjusts parent rectangles 
  splitNode = adjustTree(node, (level == 0) ? data.dataPtr() : NULL, splitNode);
  if(splitNode)
  {
    // Root had to be divided
    // Creates new root node with _root and splitNode as children
    FGngRTreeNode* newRoot = alocNode();
    newRoot->_level = _root->_level + 1;

    addChildData(newRoot, FGngRTreeChildData(_root),     false);
    addChildData(newRoot, FGngRTreeChildData(splitNode), false);

    _root = newRoot;
  }
}


/*! \brief Chooses the node level "level" where should be the insertion point received

At each level, pick the branch that needs the lower growth to accommodate the 
point received. In case of a tie, choose the node / leaf lower volume.
*/
FGngRTreeNode* FGngRTree::chooseNode(const FGngRTreeRect& rect, int level) const
{
  FGngRTreeNode* node = _root;
  assert(level <= node->_level && level >= 0);
  
  while(node->_level > level)
  {
    double minVolInc = DBL_MAX;
    double minVol    = 0;
    int    minChild  = 0;
    for(int i=0; i<node->_nchilds; i++)
    {
      const FGngRTreeRect& r = node->_children[i].rect();
      double currentVol;
      double volInc = r.calcVolumeIncrease(rect, &currentVol);
      if(volInc < minVolInc || (volInc == minVolInc && currentVol < minVol))
      {
        minVolInc = volInc;
        minVol    = currentVol;
        minChild  = i;
      }
    }
    node = node->_children[minChild].childPtr();
  }
  return node;
}

/*! \brief Adds data to the new child node node

When inserting a child of type FGngRTreeNode, adjusts its _parent fields.

If node has no empty spaces, split it.

\ param node Node where new data will be inserted
\ param data Data of the child node to be inserted in node
\ param IsLeaf Indicates whether the data refers to a point (true) or to an intermediate node of the tree

\ return Returns the pointer of the new created node by the function if a split had occured. Otherwise, it returns NULL
*/
FGngRTreeNode* FGngRTree::addChildData(FGngRTreeNode* node, FGngRTreeChildData& data, bool isLeaf)
{
  assert(node);

  // If there is space, simply insert the data in the new child "node" and returns
  if(node->_nchilds < _maxChilds)
  {
    node->_children[node->_nchilds++] = data;
    if(isLeaf)
    {
      data.dataPtr()->setRTreeNode(node);
    }
    else
    {
      data.childPtr()->_parent = node;
      assert(data.childPtr()->_level == node->_level-1);
    }
    return NULL;
  }

  // There is no space. We need to split the node.
  return splitNode(node, data, isLeaf);
}

/*! \ brief propagates inserting a new node to the root

Adjusts bounding boxes from the parent of the node up to the root.
Breaks when you can.

\param node      Node that had its set of children changed
\param pointNode Point that was inserted in node (or NULL if we are reinserting a branch after a remove)
\param splitNode If node was divided when its set of children has changed,
                  splitNode should contain a pointer to the node created as a result
                  the division. Otherwise, splitNode must be equal to NULL.
\return Returns new node created if the root had to be divided, NULL otherwise.
*/
FGngRTreeNode* FGngRTree::adjustTree(FGngRTreeNode* node, FGngNode* pointNode, FGngRTreeNode* splitNode)
{
  while(node != _root)
  {
    // Find out the parent node (p) and the index (i) of node p
    FGngRTreeNode* p = node->_parent;
    int i = 0;
    while(p->_children[i].childPtr() != node) i++;

    // Adjust the rectangle at position i
    // If there was no split, just add the point to the rectangle to make the adjustment. if
    // This addition did not affect the rectangle does not have to clean continue to traverse the tree
    if(!splitNode && pointNode)
    {
      if(!p->_children[i].rect().addPoint(pointNode))
        return NULL;
    }
    else
    {
     // There was a split (or not have pointNode - update). We need to completely rebuild the node bb
      FGngRTreeRect rect(node->_children[0].rect());
      for(int j=1; j<node->_nchilds; j++)
        rect.addRect(node->_children[j].rect());
      bool updated = p->_children[i].rect().setRect(rect);

      // IF splitNode != NULL, we insert the additional node as a child of p
      if(splitNode)
        splitNode = addChildData(p, FGngRTreeChildData(splitNode), false); 
      else if(!updated) // We do not have a split and reconstruction bb node does not affect the parent. We can finish.
        return NULL;
    }

    // Prepare next iteration.
    node = p;
  }
  return splitNode;
}

/*! \brief Divide node into two nodes containing as children the union of the children of node with data.
*/
FGngRTreeNode* FGngRTree::splitNode(FGngRTreeNode* node, FGngRTreeChildData& data, bool isLeaf)
{
  assert(node->_nchilds == _maxChilds);
  FGngRTreeNode* newNode = alocNode();
  newNode->_level = node->_level;

  // Put all nodes that must be processed in _nodeSet
  for(int i=0; i<_maxChilds; i++) 
    _nodeSet[i] = node->_children[i];
  _nodeSet[_maxChilds] = data;

  // Clear old node
  node->_nchilds = 0;

  // Choose seeds of each of the groups. Includes seed node and newNode and 
  // remove them from _nodeSet (moving them to the end of the set)
  pickSeeds(_nodeSet, _maxChilds+1, node, newNode, isLeaf);

  FGngRTreeRect nodeRect(node->_children[0].rect());
  FGngRTreeRect newNodeRect(newNode->_children[0].rect());

  // processes each of the next entries
  int nodesToProcess = _maxChilds + 1 - 2;  //(-2 because seeds aready have been processed)
  while(nodesToProcess && (nodesToProcess > _minChilds - node->_nchilds) &&
        (nodesToProcess > _minChilds - newNode->_nchilds))
  {
    // Get next input from _nodeSet and includes in the same node or newNode whenever is more appropriate, 
    // removing it from _nodeSet (moving it to the end of the set)
    pickNext(_nodeSet, nodesToProcess, node, newNode, &nodeRect, &newNodeRect, isLeaf);
    nodesToProcess--;
  }

  // Puts the remained nodes in the node that is still incomplete
  if(nodesToProcess)
  {
    assert((nodesToProcess == _minChilds-node->_nchilds) || (nodesToProcess == _minChilds-newNode->_nchilds));
    FGngRTreeNode* p = (node->_nchilds < _minChilds) ? node : newNode;
    for(int i=0; i<nodesToProcess; i++)
      addChildData(p, _nodeSet[i], isLeaf);
  }

  assert(node->_nchilds >= _minChilds); 
  assert(newNode->_nchilds >= _minChilds); 
  assert(node->_nchilds + newNode->_nchilds == _maxChilds + 1);

  return newNode;
}


/*! \brief Choose seeds of each of the groups. Includes seeds in n1 and n2
            removing the same from nodeSet (moving them to the end of the set)
*/
void FGngRTree::pickSeeds(FGngRTreeChildData* nodeSet, int setsize, 
                         FGngRTreeNode* n1, FGngRTreeNode* n2, bool isLeaf)
{
  double maxDiff = -DBL_MAX;
  int    index1  = 0;
  int    index2  = 0;

  // Find two initial seeds
  for(int i=0; i<setsize-1; i++)
  {
    double vi = nodeSet[i].rect().volume();
    for(int j=i+1; j<setsize; j++)
    {
      double vj = nodeSet[j].rect().volume();

      FGngRTreeRect r(nodeSet[i].rect());
      r.addRect(nodeSet[j].rect());
      double diff = r.volume() - vi - vj;

      if(diff > maxDiff)
      {
        maxDiff = diff;
        index1  = i;
        index2  = j;
      }
    }
  }

  // Update n1 and n2
  addChildData(n1, _nodeSet[index1], isLeaf);
  addChildData(n2, _nodeSet[index2], isLeaf);

  // Update nodeSet
  assert(index1 < index2);
  qSwap(_nodeSet[index2], _nodeSet[setsize-1]);
  qSwap(_nodeSet[index1], _nodeSet[setsize-2]);
}

/*! \brief Get next input nodeSet and includes them in n1 or n2, whenever is
           more appropriate, removing it from nodeset (moving it to the end of the set)
*/
void FGngRTree::pickNext(FGngRTreeChildData* nodeSet, int setsize, FGngRTreeNode* n1, FGngRTreeNode* n2,
                        FGngRTreeRect* n1rect, FGngRTreeRect* n2rect, bool isLeaf)
{
  double maxDiff      = -1;
  int    maxDiffIndex = 0;
  int    insNode      = 0; 

  // Chooses the next rectangle to be inserted
  for(int i=0; i<setsize; i++)
  {
    double n1Volume, n2Volume;
    double d1 = n1rect->calcVolumeIncrease(nodeSet[i].rect(), &n1Volume);
    double d2 = n2rect->calcVolumeIncrease(nodeSet[i].rect(), &n2Volume);

    if(fabs(d1-d2) > maxDiff)
    {
      maxDiff      = fabs(d1-d2);
      maxDiffIndex = i;
      if(d1 != d2)
        insNode = (d1 < d2) ? 1 : 2;
      else if(n1Volume != n2Volume)
        insNode = (n1Volume < n2Volume) ? 1 : 2;
      else 
        insNode = (n1->_nchilds < n2->_nchilds) ? 1 : 2;
    }
  }

  // Insert node and update rectacgle
  if(insNode == 1)
  {
    addChildData(n1, _nodeSet[maxDiffIndex], isLeaf);
    n1rect->addRect(_nodeSet[maxDiffIndex].rect());
  }
  else if(insNode == 2)
  {
    addChildData(n2, _nodeSet[maxDiffIndex], isLeaf);
    n2rect->addRect(_nodeSet[maxDiffIndex].rect());
  }
  else
    assert(0);

  // Adjust nodeSet
  qSwap(_nodeSet[maxDiffIndex], _nodeSet[setsize-1]);
}

//! Propagate the removal of node adjusting the tree 
void FGngRTree::condenseTree(FGngRTreeNode* node)
{
  QList<FGngRTreeNode*> orphanNodes;

  // Propagates the update of the bb of the tree
  while(node != _root)
  {
    // Find parent node and its position in the parent
    FGngRTreeNode* p = node->_parent;
    int i = 0;
    while(p->_children[i].childPtr() != node) i++;

    // Check the need to remove node from its parent
    if(node->_nchilds < _minChilds)
    {
      // Remove node from p
      p->_children[i] = p->_children[p->_nchilds-1];
      p->_nchilds--;
      orphanNodes.append(node);
    }
    else
    {
      // Adjusts bounding box of node in p. If no change can stop going up the tree
      FGngRTreeRect rect(node->_children[0].rect());
      for(int j=1; j<node->_nchilds; j++)
        rect.addRect(node->_children[j].rect());
      if(!p->_children[i].rect().setRect(rect))
        break;
    }

    node = p;
  }

  // reinserts orphans
  foreach(FGngRTreeNode* node, orphanNodes)
  {
    for(int j=0; j<node->_nchilds; j++)
    {
      // Recalcula bounding box
      FGngRTreeChildData nodeData = isLeaf(node) ? FGngRTreeChildData(node->_children[j].dataPtr()) :
                                                  FGngRTreeChildData(node->_children[j].childPtr());  
      insertNode(nodeData, node->_level);
    }
    freeNode(node);
  }
}


//! Allocates space for a new node of the tree
FGngRTreeNode* FGngRTree::alocNode() const
{
  FGngRTreeNode* node = (FGngRTreeNode*)malloc(sizeof(FGngRTreeNode)+_maxChilds*sizeof(FGngRTreeChildData));
  assert(node);
  node->_nchilds = 0;
  node->_parent  = NULL;
  node->_level   = 0;
  return node;
}

//! Free memory allocated for a node
void FGngRTree::freeNode(FGngRTreeNode* node) const
{
  free(node);
}

#ifdef FGNG_RTREE_CHECK
#include <QQueue>

int FGngRTree::nodeCount(FGngRTreeNode* node) const
{
  if(isLeaf(node))
    return 1;

 int c = 1;
 for(int i=0; i<node->_nchilds; i++)
    c += nodeCount(node->_children[i].childPtr());
 return c;
}

int FGngRTree::leafCount(FGngRTreeNode* node) const
{
  if(isLeaf(node))
    return 1;

 int c = 0;
 for(int i=0; i<node->_nchilds; i++)
    c += leafCount(node->_children[i].childPtr());
 return c;
}

//! Check tree structure
void FGngRTree::check() const
{
  QQueue<FGngRTreeNode*> queue;
  queue.enqueue(_root);

  int ndata = 0;

  while(!queue.isEmpty())
  {
    FGngRTreeNode* p = queue.dequeue();

    assert(p->_nchilds <= _maxChilds);
    if(p != _root)
      assert(p->_nchilds >= _minChilds);
    else
      assert(p->_parent == NULL);

    if(p->_level > 0)
    {
      for(int i=0; i<p->_nchilds; i++)
      {
        FGngRTreeRect& r = p->_children[i].rect();
        r.check();
        FGngRTreeNode* q = p->_children[i].childPtr();
        assert(q);

        assert(q->_parent == p);
        assert(q->_level == p->_level-1);

        FGngRTreeRect childRect(q->_children[0].rect());
        for(int j=1; j<q->_nchilds; j++)
          childRect.addRect(q->_children[j].rect());
        for(int j=0; j<FGNG_DIMENSION; j++)
        {
          assert(r._min[j] == childRect._min[j]);
          assert(r._max[j] == childRect._max[j]);
        }

        queue.enqueue(q);
      }
    }
    else
    {
      for(int i=0; i<p->_nchilds; i++)
      {
        FGngRTreeRect& r = p->_children[i].rect();
        r.check();
        FGngNode* q = p->_children[i].dataPtr();
        assert(q);

        assert(q->rtreeNode() == p);

        const double* pos = q->pos();
        for(int j=0; j<FGNG_DIMENSION; j++)
        {
          assert(r._min[j] == r._max[j]);
          assert(r._min[j] == pos[j]);
        }

        ndata++;
      }
    }
  }

  assert(ndata == _numElements);  // we checked all inserted elements!
}

//! Print the tree by leves
void FGngRTree::print() const
{
  QQueue<FGngRTreeNode*> queue;
  QQueue<FGngNode*> nodes;

  queue.enqueue(_root);

  printf("-----------------\n");
  printf("Nodes of indices:\n");
  printf("-----------------\n");
  while(!queue.isEmpty())
  {
    FGngRTreeNode* p = queue.dequeue();

    FGngRTreeNode* parent = p->_parent;
    if(parent)
    {
      int i = 0;
      while(parent->_children[i].childPtr() != p) i++;

      FGngRTreeRect& r = parent->_children[i].rect();
      printf("Level = %d, parent id = %d, bb=(%.2f, %.2f, %.2f, %.2f), #children = %d\n",
             p->_level, i, r._min[0], r._min[1], r._max[0], r._max[1], p->_nchilds);
    }
    else
      printf("Level = %d, numb of children = %d\n", p->_level, p->_nchilds);

    if(p->_level > 0)
    {
      for(int i=0; i<p->_nchilds; i++)
        queue.enqueue(p->_children[i].childPtr());
    }
    else
    {
      for(int i=0; i<p->_nchilds; i++)
        nodes.enqueue(p->_children[i].dataPtr());
    }
  }
  printf("---------------\n");
  printf("Data:\n");
  printf("---------------\n");
  while(!nodes.isEmpty())
  {
    FGngNode* p = nodes.dequeue();
    printf("point = (%.2f, %.2f)\n", p->pos()[0], p->pos()[1]);
  }
}


#endif

#ifdef GNG_RTREE_DRAW_2D
#include <QPainter>
#include <QQueue>

//! Draws the tree in the painter (Qt)
void FGngRTree::draw(QPainter* painter) const
{
  assert(FGNG_DIMENSION == 2);

  static QColor colorList[15] = {Qt::black, Qt::red, Qt::green, Qt::blue, 
                                 Qt::cyan, Qt::magenta, Qt::yellow, Qt::lightGray,
                                 Qt::darkRed, Qt::darkGreen, Qt::darkBlue, Qt::darkCyan, 
                                 Qt::darkMagenta, Qt::darkYellow, Qt::darkGray};

  QQueue<FGngRTreeNode*> queue;
  queue.enqueue(_root);

  QPen pen;
  int lastLevel = -1;

  while(!queue.isEmpty())
  {
    FGngRTreeNode* p = queue.dequeue();

    if(p->_level != lastLevel)
    {
      lastLevel = p->_level;
      pen.setWidth(lastLevel ? 0 : 3);
      pen.setColor(colorList[lastLevel%15]);
      painter->setPen(pen);
    }

    for(int i=0; i<p->_nchilds; i++)
    {
      FGngRTreeRect& r = p->_children[i].rect();
      
      if(p->_level > 0)
      {
        painter->drawRect(r._min[0], r._min[1], r._max[0] - r._min[0] + 1, r._max[1] - r._min[1] + 1);
        queue.enqueue(p->_children[i].childPtr());
      }
      else
        painter->drawPoint(r._min[0], r._min[1]);
    }
  }
}
#endif