/*!
 * \file fgngFib.cpp
 *
 * \brief Implementation of the FGngFib class
 *
 * \author Carlos Augusto Teixeira Mendes
 * \date september, 2011
 */

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

#include "fgngFib.h"
#include "fgngRTree.h"

#if 1
#include <QTime>
#endif

/*! 
\class FGngFib

An RTree is used to speedup nearest neighbor queries and a Fibonacci
heap is used to speedup the search for the node with biggest error.

The Fibonacci heap implementation is based on the algorithms described 
by Cormen in "Introduction to algorithms"

*/

//! Builds a node without neighbors. The position should be initialized later
FGngNode::FGngNode()
{
  _index = -1;
  _error = 0.0;
  _lastErrorUpdate = 0;
  _rtreeNode = NULL; 

  _fibDegree = 0;
  _fibMarked = false;
  _fibFather = NULL;  
  _fibChild  = NULL;   
  _fibNext   = NULL;    
  _fibPrev   = NULL;    
}

//! Updates the error information to the correct value after the end of iteration iter
void FGngNode::updateError(int iter, double beta)
{
  if(iter == _lastErrorUpdate)
    return;

  _error *= pow(1.0 - beta, iter - _lastErrorUpdate);
  _lastErrorUpdate = iter;
}

/*! \brief Returns the error value associated with the node

Does not update the _error attribute with the calculated result in 
order to avoid numerical errors which could affect the heap structure.
Consider two nodes A and B with the same error value (adding a vertex will 
always generate this case) put together as neighbors in the heap with A as
father of B.  If in a moment A or B is updated by a call to error() and the 
other isn't, after some iterations the values are still conceptualy the same,
but numerically this can be false since 
_error * pow(1-beta, a+b) != _error * pow(1-beta, a) * pow(1-beta, b) and
that can cause an inversion of the relation between A and B on the heap.
*/
double FGngNode::error(int iter, double beta)
{
  if(iter == _lastErrorUpdate)
    return _error;
  return _error * pow(1.0 - beta, iter - _lastErrorUpdate);
}

//! Updates the node position by making _pos += epsilon*(newpos - _pos)
void FGngNode::updatePos(double epsilon, const double* newpos)
{
  assert(newpos);
  for(int i=0; i<FGNG_DIMENSION; i++)
    _pos[i] += epsilon*(newpos[i] - _pos[i]);
}

//! Returns the squared euclidian distance between the current node and the received position
double FGngNode::squaredDistanceTo(const double* pos) const
{
  double sum = 0.0;
  for(int i=0; i<FGNG_DIMENSION; i++) 
  {
    double d = _pos[i] - pos[i];
    sum += d * d;
  }
  return sum;
}

//! Increments the age of all edges connecting the node with it's neighbors
void FGngNode::incAges()
{
  assert(_neighbours.size() == _ages.size());

  for(int i=0, size=_neighbours.size(); i<size; i++)
  {
    FGngNode* n = _neighbours[i];
    int pos = n->_neighbours.indexOf(this);
    assert(pos >= 0 && n->_neighbours[pos] == this);

    _ages[i]++;
    n->_ages[pos]++;
  }
}

//! Connects current node with the given node. If a connection already exists, zero its age
void FGngNode::connectTo(FGngNode* node)
{
  assert(node);
  assert(_neighbours.size() == _ages.size());

  int pos = _neighbours.indexOf(node);
  if(pos < 0)
  {
    _neighbours.append(node);
    _ages.append(0);

    assert(node->_neighbours.indexOf(this) == -1);
    node->_neighbours.append(this);
    node->_ages.append(0);
  }
  else
  {
    int nodepos = node->_neighbours.indexOf(this);
    assert(nodepos >= 0 && node->_neighbours[nodepos] == this);
    _ages[pos] = 0;
    node->_ages[nodepos] = 0;
  }
}

//! Disconnects the node from its neighbor with index nodeIndex
void FGngNode::disconnectFrom(int nodeIndex)
{
  FGngNode* n = _neighbours[nodeIndex];
  
  _neighbours.removeAt(nodeIndex);
  _ages.removeAt(nodeIndex);

  int pos = n->_neighbours.indexOf(this);
  assert(pos >= 0 && n->_neighbours[pos] == this);

  n->_neighbours.removeAt(pos);
  n->_ages.removeAt(pos);
}


/*! \brief Constructor

\param pars Configuration parameters for the algorithm
\param sampler User function used to collect samples.  This function 
               receives as a parameter a vector of doubles with FGNG_DIMENSION
               entries wich should be filled with the position of the next sample
\param samplerContext User context sent as a parameter on every call to sampler
*/
FGngFib::FGngFib(const FGngParameters& pars, FGngSampleSrc sampler, void* samplerContext)
  : _par(pars), _sampler(sampler), _samplerContext(samplerContext)
{
  assert(sampler);

  _nodeBuffer    = NULL;
  _nodes         = NULL;
  _nodesSize     = 0;
  _firstFreeNode = 0;

  _rtree = new FGngRTree(pars.rtreeMaxChilds, pars.rtreeMinChilds, pars.rtreeNNAlg);

  _fibN   = 0;
  _fibMax = NULL;
}

//! Destructor
FGngFib::~FGngFib()
{
  delete[] _nodeBuffer;
  delete[] _nodes;
  delete _rtree;
}

//! Executes the FGNG Fib lgorithm.  
void FGngFib::execute()
{
  int iter = 0;

  // Initializes the graph with its two first nodes
  initGraph();

#if 1
  // Start timer used to measure the time taken to add each additional 1000 nodes to the graph
  QTime t;
  t.start();
#endif

  while(_nodesSize < _par.nodes)
  {
    iter++;

    // Step 1: Get a new sample. Find the nearest two neighbors of the sample
    double samplePos[FGNG_DIMENSION]; 
    _sampler(_samplerContext, samplePos);
    FGngNode* na; // Nearest neighbor from samplePos
    FGngNode* nb; // Second nearest neighbor from samplePos
    getNearestNodes(samplePos, &na, &nb);

    // Step 2: Update the error of na
    double deltaError = na->squaredDistanceTo(samplePos);
    na->updateError(iter-1, _par.beta); // We are at step iter, but the last multiplication by beta happened on step iter-1
    na->_error += deltaError;
    fibheapIncVal(iter-1, na);

    // Step 3: Adjust the position of na and of all its neighbors
    updateNodePositions(na, samplePos);

    // Step 4: Increment the age of edges between na and its neighbors
    na->incAges();

    // Step 5: Connect na with nb (or zero its edge)
    na->connectTo(nb);

    // Step 6: Remove older edges.  Remove isolated nodes
    removeOldConnections(iter-1, na);

    // Setp 7: Add a new node
    if((iter % _par.lambda) == 0)
    {
      addNewNode(iter-1); // We are at step iter, but the last multiplication by beta happened on step iter-1
#if 1
      if(_nodesSize % 1000 == 0)
        printf("%d - %d\n", _nodesSize, t.restart());
#endif
    }

#ifdef FGNG_CHECK
    // Check data structures if this option is enabled in the header file. Warning: very slow
    runInternalCheck(iter-1);
#endif
  }

}

//! Initializes the graph structure and includes the two initial nodes on the grpah
void FGngFib::initGraph()
{
  // Allocates structures to store nodes
  _nodeBuffer = new FGngNode[_par.nodes];
  _firstFreeNode = 0;

  _nodes = new FGngNode*[_par.nodes];
  _nodesSize = 0;

  // Adds two nodes to the graph
  // Both nodes have error equal to 0.0
  for(int i=0; i<2; i++)
  {
    FGngNode* n = getNode(i);
    _sampler(_samplerContext, n->_pos);
    _nodes[_nodesSize++] = n;
    _rtree->insertNode(n);
    fibheapInsert(0, n);
  }
}

/*! \brief Returns in na and in nb the two closest nodes to samplePos

na is filled with the nearest neighbor and nb with the second nearest
*/
void FGngFib::getNearestNodes(const double* samplePos, FGngNode** na, FGngNode** nb)
{
  assert(_nodesSize >= 2);
  _rtree->nearestNeighbours(samplePos, na, nb);
}

//! Adjusts the position of node and of its neighbors according to the sample position
void FGngFib::updateNodePositions(FGngNode* node, const double* samplePos)
{
  node->updatePos(_par.epsilonB, samplePos);
  _rtree->nodeUpdated(node);

  foreach(FGngNode* n, node->_neighbours)
  {
    n->updatePos(_par.epsilonN, samplePos);
    _rtree->nodeUpdated(n);
  }
}

/*! \brief Remove old connections between node and its neighbors. It also
           removes eventual nodes that became isolated as a result of 
           removing old edges

Notice that the node "node" will finnish with at least one neighbor as a 
result of its last conection with another node.

Returns the number of nodes removed (nodes, not edges).
*/
int FGngFib::removeOldConnections(int iter, FGngNode* node)
{
  int nremoved = 0;
  for(int i=0, pos=0, size=node->_neighbours.size(); i<size; i++)
  {
    if(node->_ages[pos] > _par.ageMax)
    {
      FGngNode* n = node->_neighbours[pos];
      node->disconnectFrom(pos);
      if(n->_neighbours.size() == 0)
      {
        removeIsolatedNode(iter, n);
        nremoved++;
      }
    }
    else
      pos++;
  }
  assert(node->_neighbours.size() > 0); // New connection can not be an old one...
  return nremoved;
}

//! Removes from the graph an isolated node (that has no neighbors).  Deletes the node.
void FGngFib::removeIsolatedNode(int iter, FGngNode* node)
{
  assert(node && node->_neighbours.size() == 0);
  assert(_nodes[node->_index] == node);

  // Removes the node from the heap and recycles it
  fibheapRemove(iter, node);
  _rtree->removeNode(node);
  recycleNode(node);
}

//! Adds a new node to the graph
void FGngFib::addNewNode(int iter)
{
  // Find the node with biggest error.
  FGngNode* maxnode = _fibMax;

  // Find the neighbor of maxnode with biggest error
  // It's guaranteed that maxnode has at least one neighbor
  int index;
  assert(maxnode->_neighbours.size() > 0); 
  FGngNode* maxneighbour = findNodeWithBiggestError(iter, maxnode->_neighbours, &index);

  // Creates a new node and adds it to the graph
  FGngNode* newnode = getNode(_nodesSize);
  _nodes[_nodesSize++] = newnode;  

  for(int i=0; i<FGNG_DIMENSION; i++)
    newnode->_pos[i] = (maxnode->_pos[i] + maxneighbour->_pos[i]) / 2.0;

  // Disconnects maxnode from maxneighbour
  maxnode->disconnectFrom(index);

  // Connects newnode with maxnode and with maxneighbour
  newnode->connectTo(maxnode);
  newnode->connectTo(maxneighbour);

  // Adjusts the error in maxnode and in maxneighbour
  maxnode->updateError(iter, _par.beta);
  maxnode->_error *= _par.alpha;
  fibheapDecVal(iter, maxnode);

  maxneighbour->updateError(iter, _par.beta);
  maxneighbour->_error *= _par.alpha;
  fibheapDecVal(iter, maxneighbour);

  // Sets the error in newnode and includes it on the heap and on the RTree
  newnode->_error           = maxnode->_error;
  newnode->_lastErrorUpdate = iter; 
  fibheapInsert(iter, newnode);
  _rtree->insertNode(newnode);
}

/*! \brief Returns the node from "list" with bigger error value

The list should have at least one value. index returns the position on the list
where the node with biggest error was found.
*/
FGngNode* FGngFib::findNodeWithBiggestError(int iter, QList<FGngNode*>& list, int* index) const
{
  double maxerr = -1;
  for(int i=0, size=list.size(); i<size; i++)
  {
    FGngNode* n = list[i];
    double err = n->error(iter, _par.beta);
    if(err > maxerr)
    {
      maxerr = err;
      *index = i;
    }
  }
  return list[*index];
}

//! Returns an unused node to be included in the graph
FGngNode* FGngFib::getNode(int index)
{
  assert(index < _par.nodes);
  assert(_nodesSize + _freeNodeList.size() == _firstFreeNode);
  assert(_freeNodeList.size() || _firstFreeNode < _par.nodes);

  FGngNode* n;
  if(_freeNodeList.size())
    n = _freeNodeList.takeLast();
  else
    n = &_nodeBuffer[_firstFreeNode++];

  n->_index = index;
  return n;
}

//! Returns a node to the list of unused nodes so that it can be reused later
void FGngFib::recycleNode(FGngNode* node)
{
  node->_index = -1;
  _freeNodeList.append(node);
}

//! Inserts a new node on the heap. Assumes that the error value is alreadt filled
void FGngFib::fibheapInsert(int iter, FGngNode* node)
{
  node->_fibDegree = 0;
  node->_fibMarked = false;
  node->_fibFather = NULL;
  node->_fibChild  = NULL;

  fibAddToList(_fibMax, node);

  if(!_fibMax || node->error(iter, _par.beta) > _fibMax->error(iter, _par.beta))
    _fibMax = node;

  _fibN++;
}

//! Removes the node with biggest error from the heap. Returns the removed node.
FGngNode* FGngFib::fibheapRemoveMax(int iter)
{
  FGngNode* z = _fibMax;
  if(!z)
    return NULL;

  // Insert sons of z on the root list
  FGngNode* x = z->_fibChild;
  for(int i=0, size=z->_fibDegree; i<size; i++, x = x->_fibNext)
    x->_fibFather = NULL;
  fibMergeLists(_fibMax, z->_fibChild);

  // Remove z from the list
  fibRemoveFromList(z);
  if(z == z->_fibNext)
    _fibMax = NULL;
  else 
  {
    _fibMax = z->_fibNext;
    fibConsolidate(iter);
  }
  _fibN--;
  return z;
}

//! Removes the node from the heap
void FGngFib::fibheapRemove(int iter, FGngNode* node)
{
  node->_error           = DBL_MAX;
  node->_lastErrorUpdate = iter;
  fibheapIncVal(iter, node);
  fibheapRemoveMax(iter);
}

//! Increments the value of a node. New value should be greater than the actual value on the node
void FGngFib::fibheapIncVal(int iter, FGngNode* node)
{
  FGngNode* y = node->_fibFather;
  if(y && node->error(iter, _par.beta) > y->error(iter, _par.beta))
    fibCut(node, y);  // Includes cut + cascadingCut

  if(node->error(iter, _par.beta) > _fibMax->error(iter, _par.beta))
    _fibMax = node;
}

//! Decrements the node value.  For that, removes and reinserts the node
void FGngFib::fibheapDecVal(int iter, FGngNode* node)
{
  double err    = node->_error;
  int    lastUp = node->_lastErrorUpdate;
  
  fibheapRemove(iter, node);

  node->_error = err;
  node->_lastErrorUpdate = lastUp;

  fibheapInsert(iter, node);
}

/*! \brief Calculates the maximum number of children for a node in the heap

D(n) <= floor( log n on base phi), phi = (1+sqrt(5))/2
*/
inline int FGngFib::fibD() const
{
  const double c = log10((1 + sqrt(5.0)) / 2.0);

  return floor(log10((double)_fibN) / c);
}

/*! \brief Adds the node x to the circular list that has listNode 
           as a member. If listNode == NULL, prepares x to be the only
           element of the list
*/
inline void FGngFib::fibAddToList(FGngNode* listNode, FGngNode* x)
{
  if(!listNode)
  {
    x->_fibNext = x;
    x->_fibPrev = x;
  }
  else
  {
    x->_fibNext = listNode->_fibNext;
    x->_fibPrev = listNode;
    listNode->_fibNext = x;
    x->_fibNext->_fibPrev = x;
  }
}

//! Removes x from the circular list where it is currently inserted. Does not change x
inline void FGngFib::fibRemoveFromList(const FGngNode* x)
{
  if(x->_fibNext == x)
    return;
  
  x->_fibPrev->_fibNext = x->_fibNext;
  x->_fibNext->_fibPrev = x->_fibPrev;
}

/*! \brief Concatenates all elements from the second list into the first

\param firstListNode Pointer to some node of the base list where the other nodes
                     will be inserted
\param secondListNode Pointer to some node of the list that will be concatenated. Can
                      be NULL
*/
inline void FGngFib::fibMergeLists(FGngNode* firstListNode, FGngNode* secondListNode)
{
  assert(firstListNode);
  if(!secondListNode)
    return;

  FGngNode* l1First = firstListNode;
  FGngNode* l1Last  = firstListNode->_fibPrev;
  FGngNode* l2First = secondListNode;
  FGngNode* l2Last  = secondListNode->_fibPrev;

  l1Last->_fibNext  = l2First;
  l2First->_fibPrev = l1Last;

  l1First->_fibPrev = l2Last;
  l2Last->_fibNext  = l1First;
}

/*! Auxiliary function used to consolidate trees from the root list

After its execution, all trees on the root list have a different 
_degree value (number of children)
*/
inline void FGngFib::fibConsolidate(int iter)
{
  assert(_fibMax);

  int maxchilds = fibD() + 1; // +1 to make sure there is a NULL entry at the end of vector A
  assert(maxchilds <= 45); // 45 is enough to allow a heap with more then one billion entries

  // Initializes an auxiliar vector used to keep tree roots
  FGngNode* A[45];
  for(int i=0; i<maxchilds; i++) 
    A[i] = NULL;

  // Consolidates in A trees with equal number of children
  FGngNode* w = _fibMax;
  for(bool first=true; w != _fibMax || first; first=false)
  {
    FGngNode* x = w;
    int      d = x->_fibDegree;
    w = w->_fibNext;

    while(A[d])
    {
      FGngNode* y = A[d];
      if(x->error(iter, _par.beta) < y->error(iter, _par.beta))
        qSwap(x, y);

      // No need to remove from the list.  It will be reconstructed later.
      // This guarantees that the for ends as the last element will keep 
      // pointing to the next unitl the end
      // removeFromList(y);  
      fibAddToList(x->_fibChild, y);
      if(!x->_fibChild)
        x->_fibChild = y;
      y->_fibFather = x;

      x->_fibDegree++;
      y->_fibMarked = false;

      A[d] = NULL;
      d++;
    }
    A[d] = x;
  }

  // Rebuilds the root list
  _fibMax = NULL;
  for(int i=0; i<maxchilds; i++)
  {
    if(A[i])
    {
      fibAddToList(_fibMax, A[i]);
      if(!_fibMax || A[i]->error(iter, _par.beta) > _fibMax->error(iter, _par.beta))
        _fibMax = A[i];
    }
  }
}


/*! \brief Removes the connection between x ant its father y. Includex x as the new tree root.
           Propagates cuts whenever necessary.

Includes the implementation of cut and cascadingCut from Cormens description, 
removing the recursive calls.
*/
inline void FGngFib::fibCut(FGngNode* x, FGngNode* y)
{
  while(1)
  {
    // cut
    assert(x->_fibFather == y);

    if(y->_fibChild == x)
    {
      y->_fibChild = x->_fibNext;
      if(y->_fibChild == x)
        y->_fibChild = NULL;
    }
    fibRemoveFromList(x);  
    y->_fibDegree--;

    fibAddToList(_fibMax, x);
    x->_fibFather = NULL;
    x->_fibMarked = false;

    // cascadingCut
    if(!y->_fibFather)
      return;

    if(!y->_fibMarked)
    {
      y->_fibMarked = true;
      return;
    }

    x = y;
    y = y->_fibFather;
  }
}

#ifdef FGNG_CHECK

//! Check heap structure
void FGngFib::fibCheck(int iter) const
{
  // Checks the structure of the linked list
  // Pointer to father must be NULL
  // Check the total number of elements in the heap
  // Calls checkTree() to validate the structure of each tree
  // _max should point to the biggest value
  int    n = 0;
  double m = -DBL_MAX;

  FGngNode* w = _fibMax;
  if(!w)
    return;
  for(bool first=true; w != _fibMax || first; w=w->_fibNext, first=false)
  {
    assert(w->_fibNext->_fibPrev == w);
    assert(w->_fibPrev->_fibNext == w);
    assert(w->_fibFather == NULL);
    assert(w->_fibFather != w->_fibNext);
    assert(w->_fibFather != w->_fibPrev);
    assert(w->_fibChild  != w->_fibNext);
    assert(w->_fibChild  != w->_fibPrev);
    n += fibCheckTree(iter, w, NULL);

    double e  = w->error(iter, _par.beta);
    if(e > m)
      m = e;
  }
  assert(_fibMax->error(iter, _par.beta) == m);
  assert(_fibN == n);
}

//! Checks the structure of the tree
int FGngFib::fibCheckTree(int iter, FGngNode* node, FGngNode* father) const
{
  assert(node->_fibFather == father);
  if(father)
  {
    double e  = node->error(iter, _par.beta);
    double ef = father->error(iter, _par.beta);
    assert(ef >= e);

    int d = 0;
    FGngNode* x = node;
    for(bool first=true; x != node || first; x=x->_fibNext, first=false)
    {
      assert(x->_fibNext->_fibPrev == x);
      assert(x->_fibPrev->_fibNext == x);
      assert(x->_fibFather == father);
      assert(x->_fibFather != x->_fibChild);
      assert(x->_fibFather != x->_fibNext);
      assert(x->_fibFather != x->_fibPrev);
      assert(x->_fibChild  != x->_fibNext);
      assert(x->_fibChild  != x->_fibPrev);
      d++;
    }
    assert(d == father->_fibDegree);
  }

  int n = 1;
  if(node->_fibChild)
  {
    FGngNode* x = node->_fibChild;
    for(int i=0, size=node->_fibDegree; i<size; i++, x = x->_fibNext)
      n += fibCheckTree(iter, x, node);
  }
  else
  {
    assert(node->_fibDegree == 0);
  }
  return n;
}

#include <QSet>
//! Checks the consistency of the information stored in the graph
void FGngFib::runInternalCheck(int iter) const
{
  assert(_nodesSize >= 2);
  assert(_nodesSize == _fibN);
  assert(_nodesSize + _freeNodeList.size() == _firstFreeNode);
  int i = 0;

#ifdef GNG_RTREE_CHECK
  _rtree->check();
#endif
  fibCheck(iter);

  for(int k=0; k<_nodesSize; k++)
  {
    FGngNode* n = _nodes[k];
    assert(n->_index == i);
    assert(n->_error >= 0.0);
    assert(n->_ages.size() == n->_neighbours.size());
    assert(n->_rtreeNode); 

    i++;

    if(n->_error != 0.0)
      assert(n->_ages.size() > 0);

    foreach(int age, n->_ages)
      assert(age <= _par.ageMax);

    QSet<FGngNode*> nodeset;
    for(int j=0, size=n->_neighbours.size(); j<size; j++)
    {
      FGngNode* p = n->_neighbours[j];
      int pos = p->_neighbours.indexOf(n);
      assert(pos >= 0);
      assert(n == p->_neighbours[pos]);
      assert(n->_ages[j] == p->_ages[pos]);
      assert(!nodeset.contains(p));
      nodeset.insert(p);
    }
  }
}
#endif