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

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

#if 1
#include <QTime>
#endif

#include "gng.h"


/*! 
\class Gng
*/

//! Builds a node without neighbors. The position should be initialized later
GngNode::GngNode()
{
  _index = -1;
  _error = 0.0;
}

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

//! Returns the squared euclidian distance between the current node and the received position
double GngNode::squaredDistanceTo(const double* pos) const
{
  double sum = 0.0;
  for(int i=0; i<GNG_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 GngNode::incAges()
{
  assert(_neighbours.size() == _ages.size());

  for(int i=0, size=_neighbours.size(); i<size; i++)
  {
    GngNode* 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 GngNode::connectTo(GngNode* 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 GngNode::disconnectFrom(int nodeIndex)
{
  GngNode* 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 GNG_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
*/
Gng::Gng(const GngParameters& pars, GngSampleSrc sampler, void* samplerContext)
  : _par(pars), _sampler(sampler), _samplerContext(samplerContext)
{
  assert(sampler);

  _nodeBuffer    = NULL;
  _firstFreeNode = 0;
}

//! Destructor
Gng::~Gng()
{
  delete[] _nodeBuffer;
}

//! Executes the GNG algorithm.  
void Gng::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(_nodes.size() < _par.nodes)
  {
    iter++;

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

    // Step 2: Update the error of na
    double deltaError = na->squaredDistanceTo(samplePos);
    na->_error += deltaError;

    // 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(na);

    // Setp 7: Add a new node
    if((iter % _par.lambda) == 0)
      addNewNode();

    // Step 8: Decrement the error value of each node
    foreach(GngNode* n, _nodes)
      n->_error *= (1.0 - _par.beta);

#if 1
    if((iter % _par.lambda) == 0 && (_nodes.size() % 1000 == 0))
        printf("%d - %d\n", _nodes.size(), t.restart());
#endif

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

}

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

  // Adds two nodes to the graph
  for(int i=0; i<2; i++)
  {
    GngNode* n = getNode(i);
    _sampler(_samplerContext, n->_pos);
    _nodes.append(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 Gng::getNearestNodes(const double* samplePos, GngNode** na, GngNode** nb)
{
  assert(_nodes.size() >= 2);

  double dist1 = DBL_MAX, dist2 = DBL_MAX;

  foreach(GngNode* n, _nodes)
  {
    double d = n->squaredDistanceTo(samplePos);
    if(d < dist1)
    {
      dist2 = dist1;
      dist1 = d;
      *nb = *na;
      *na = n;
    }
    else if(d < dist2)
    {
      dist2 = d;
      *nb = n;
    }
  }
}

//! Adjusts the position of node and of its neighbors according to the sample position
void Gng::updateNodePositions(GngNode* node, const double* samplePos)
{
  node->updatePos(_par.epsilonB, samplePos);
  foreach(GngNode* n, node->_neighbours)
    n->updatePos(_par.epsilonN, samplePos);
}

/*! \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 Gng::removeOldConnections(GngNode* node)
{
  int nremoved = 0;
  for(int i=0, pos=0, size=node->_neighbours.size(); i<size; i++)
  {
    if(node->_ages[pos] > _par.ageMax)
    {
      GngNode* n = node->_neighbours[pos];
      node->disconnectFrom(pos);
      if(n->_neighbours.size() == 0)
      {
        removeIsolatedNode(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 Gng::removeIsolatedNode(GngNode* node)
{
  assert(node && node->_neighbours.size() == 0);
  assert(_nodes[node->_index] == node);

  // Swaps the position of the last node on the list with the node being removed
  GngNode* lastNode = _nodes.takeLast(); 
  if(node != lastNode)
  {
    lastNode->_index = node->_index;
    _nodes.replace(node->_index, lastNode);
  }
  recycleNode(node);
}

//! Adds a new node to the graph
void Gng::addNewNode()
{
  // Find the node with biggest error.
  int      index;
  GngNode* maxnode = findNodeWithBiggestError(_nodes, &index);

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

  // Creates a new node and adds it to the graph
  GngNode* newnode = getNode(_nodes.size());
  for(int i=0; i<GNG_DIMENSION; i++)
    newnode->_pos[i] = (maxnode->_pos[i] + maxneighbour->_pos[i]) / 2.0;
  _nodes.append(newnode);

  // 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->_error      *= _par.alpha;
  maxneighbour->_error *= _par.alpha;

  // Sets the error in newnode
  newnode->_error = maxnode->_error;
}

/*! \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.
*/
GngNode* Gng::findNodeWithBiggestError(QList<GngNode*>& list, int* index) const
{
  double maxerr = -1;
  for(int i=0, size=list.size(); i<size; i++)
  {
    GngNode* n = list[i];
    if(n->_error > maxerr)
    {
      maxerr = n->_error;
      *index = i;
    }
  }
  return list[*index];
}

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

  GngNode* 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 Gng::recycleNode(GngNode* node)
{
  node->_index = -1;
  _freeNodeList.append(node);
}


#ifdef GNG_CHECK
#include <QSet>
//! Checks the consistency of the information stored in the graph
void Gng::runInternalCheck() const
{
  assert(_nodes.size() >= 2);
  assert(_nodes.size() + _freeNodeList.size() == _firstFreeNode);
  int i = 0;
  foreach(GngNode* n, _nodes)
  {
    assert(n->_index == i++);
    assert(n->_error >= 0.0);
    assert(n->_ages.size() == n->_neighbours.size());

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

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

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