/* ============================================================================
 * Copyright (c) 2010, Michael A. Jackson (BlueQuartz Software)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of Michael A. Jackson nor the names of its contributors may
 * be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "InitializationFunctions.h"
//-- C Includes
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

//-- Boost Includes
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

//-- EMMMPM Lib Includes
#include "EMMPMLib/Core/EMMPM.h"
#include "EMMPMLib/Common/MSVCDefines.h"
#include "EMMPMLib/Common/EMMPM_Math.h"
#include "EMMPMLib/Common/EMTime.h"

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
InitializationFunction::InitializationFunction()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
InitializationFunction::~InitializationFunction()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void InitializationFunction::initialize(EMMPM_Data::Pointer data)
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
BasicInitialization::BasicInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
BasicInitialization::~BasicInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void BasicInitialization::initialize(EMMPM_Data::Pointer data)
{
  //FIXME: This needs to be adapted for vector images (dims > 1)
  unsigned int i, k, l;
  real_t mu, sigma;
  char msgbuff[256];
  unsigned int rows = data->rows;
  unsigned int cols = data->columns;
  unsigned int classes = data->classes;
  unsigned char* y = data->y;
  size_t total;

  rows = data->rows;
  cols = data->columns;
  total = data->rows * data->columns;

  memset(msgbuff, 0, 256);

  /* Initialization of parameter estimation */
  mu = 0;
  sigma = 0;
  for (i = 0; i < total; i++) {
      mu += y[i];
  }

  mu /= (rows * cols);

  for (i = 0; i < total; i++) {
      sigma += (y[i] - mu) * (y[i] - mu);
  }

  sigma /= (rows * cols);
  sigma = sqrt((real_t)sigma);

  if (classes % 2 == 0)
  {
    for (k = 0; k < classes / 2; k++)
    {
        data->mean[classes / 2 + k] = mu + (k + 1) * sigma / 2;
        data->mean[classes / 2 - 1 - k] = mu - (k + 1) * sigma / 2;
    }
  }
  else
  {
    data->mean[classes / 2] = mu;
    for (k = 0; k < classes / 2; k++)
    {
      data->mean[classes / 2 + 1 + k] = mu + (k + 1) * sigma / 2;
      data->mean[classes / 2 - 1 - k] = mu - (k + 1) * sigma / 2;
    }
  }

  for (l = 0; l < classes; l++)
  {
    data->variance[l] = 20.0;
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
UserDefinedAreasInitialization::UserDefinedAreasInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
UserDefinedAreasInitialization::~UserDefinedAreasInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void UserDefinedAreasInitialization::initialize(EMMPM_Data::Pointer data)
{

  unsigned int i, j;
  size_t index;
  int c, l;
  real_t mu, sigma;
  unsigned int rows = data->rows;
  unsigned int cols = data->columns;
  char msgbuff[256];
  unsigned char* y = data->y;
  //  unsigned char** xt = data->xt;
  rows = data->rows;
  cols = data->columns;

  sigma = 0;
  mu = 0;

  if (data->dims != 1)
  {
    printf("User Defined Initialization ONLY works with GrayScale images and not vector images.\n  %s(%d)", __FILE__, __LINE__);
    exit(1);
  }

  memset(msgbuff, 0, 256);

  for (c = 0; c < data->classes; c++)
  {
    int x1 = data->initCoords[c][0];
    int y1 = data->initCoords[c][1];
    int x2 = data->initCoords[c][2];
    int y2 = data->initCoords[c][3];
    mu = 0;
    snprintf(msgbuff, 256, "m[%d] Coords: %d %d %d %d", c, x1, y1, x2, y2);
    for (i = data->initCoords[c][1]; i < data->initCoords[c][3]; i++)
    {
      for (j = data->initCoords[c][0]; j < data->initCoords[c][2]; j++)
      {
        index = (cols * i) + j;
        mu += y[index];
      }
    }

    mu /= (y2 - y1) * (x2 - x1);
    data->mean[c] = mu;
    snprintf(msgbuff, 256, "m[%d]=%f", c, mu);
  }

  for (l = 0; l < data->classes; l++)
  {
    data->variance[l] = 20.0;
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
XtArrayInitialization::XtArrayInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
XtArrayInitialization::~XtArrayInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void XtArrayInitialization::initialize(EMMPM_Data::Pointer data)
{
  size_t total;

  total = data->rows * data->columns;

  const float rangeMin = 0.0f;
  const float rangeMax = 1.0f;
  typedef boost::uniform_real<> NumberDistribution;
  typedef boost::mt19937 RandomNumberGenerator;
  typedef boost::variate_generator<RandomNumberGenerator&,
                                   NumberDistribution> Generator;

  NumberDistribution distribution(rangeMin, rangeMax);
  RandomNumberGenerator generator;
  Generator numberGenerator(generator, distribution);
  generator.seed(EMMPM_getMilliSeconds()); // seed with the current time

  /* Initialize classification of each pixel randomly with a uniform disribution */
  for (size_t i = 0; i < total; i++)
  {
      data->xt[i] = numberGenerator() * data->classes;
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
GradientVariablesInitialization::GradientVariablesInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
GradientVariablesInitialization::~GradientVariablesInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void GradientVariablesInitialization::initialize(EMMPM_Data::Pointer data)
{
  size_t ijd, ij, ijd1;

  size_t nsCols = data->columns - 1;
  size_t nsRows = data->rows;
  size_t ewCols = data->columns;
  size_t ewRows = data->rows - 1;
  size_t swCols = data->columns - 1;
  size_t swRows = data->rows - 1;
  size_t nwCols = data->columns - 1;
  size_t nwRows = data->rows - 1;

  int dims = data->dims;
  real_t x;

  /* Allocate for edge images */

  data->ns = (real_t*)malloc(nsCols * nsRows * sizeof(real_t));
  if (data->ns == NULL) { return; }
  data->ew = (real_t*)malloc(ewCols * ewRows * sizeof(real_t));
  if (data->ew == NULL) { return; }
  data->sw = (real_t*)malloc(swCols * swRows * sizeof(real_t));
  if (data->sw == NULL) { return; }
  data->nw = (real_t*)malloc(nwCols * nwRows * sizeof(real_t));
  if (data->nw == NULL) { return; }

  /* Do edge detection for gradient penalty*/
  for (uint32_t i = 0; i < data->rows; i++)
  {
    for (uint32_t j = 0; j < nwCols; j++)
    {
      x = 0;
      for (int32_t d = 0; d < dims; d++)
      {
        ijd = (dims * nwCols * i) + (dims * j) + d;
        ijd1 = (dims * nwCols * (i)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->ns[ij] = data->beta_e * atan((10 - sqrt(x)) / 5);
    }
  }
  for (uint32_t i = 0; i < nwRows; i++)
  {
    for (uint32_t j = 0; j < data->columns; j++)
    {
      x = 0;
      for (int32_t d = 0; d < dims; d++)
      {
        ijd = (dims * data->columns * i) + (dims * j) + d;
        ijd1 = (dims * data->columns * (i + 1)) + (dims * (j)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (data->columns * i) + j;
      data->ew[ij] = data->beta_e * atan((10 - sqrt(x)) / 5);
    }
  }
  nwCols = data->columns - 1;
  nwRows = data->rows - 1;
  for (uint32_t i = 0; i < nwRows; i++)
  {
    for (uint32_t j = 0; j < nwCols; j++)
    {
      x = 0;
      for (uint32_t d = 0; d < data->dims; d++)
      {
        ijd = (dims * data->columns * i) + (dims * j) + d;
        ijd1 = (dims * data->columns * (i + 1)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->sw[ij] = data->beta_e * atan((10 - sqrt(0.5 * x)) / 5);
      x = 0;
      for (uint32_t d = 0; d < data->dims; d++)
      {
        ijd = (dims * data->columns * (i + 1)) + (dims * (j)) + d;
        ijd1 = (dims * data->columns * (i)) + (dims * (j + 1)) + d;
        x += (data->y[ijd] - data->y[ijd1]) * (data->y[ijd] - data->y[ijd1]);
      }
      ij = (nwCols * i) + j;
      data->nw[ij] = data->beta_e * atan((10 - sqrt(0.5 * x)) / 5);
    }
  }

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
CurvatureInitialization::CurvatureInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
CurvatureInitialization::~CurvatureInitialization()
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void CurvatureInitialization::initialize(EMMPM_Data::Pointer data)
{

}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void CurvatureInitialization::initCurvatureVariables(EMMPM_Data::Pointer data)
{
  int l, lij;
  unsigned int i, j;

  data->ccost = (real_t*)malloc(data->classes * data->rows * data->columns * sizeof(real_t));
  if (data->ccost == NULL) { return; }

  /* Initialize Curve Costs to zero */
  for (l = 0; l < data->classes; l++)
  {
    for (i = 0; i < data->rows; i++)
    {
      for (j = 0; j < data->columns; j++)
      {
        {
          lij = (data->columns * data->rows * l) + (data->columns * i) + j;
          data->ccost[lij] = 0;
        }
      }
    }
  }
}


#ifndef ALEPH_PERSISTENT_HOMOLOGY_EXTENDED_PERSISTENCE_HIERARCHY__
#define ALEPH_PERSISTENT_HOMOLOGY_EXTENDED_PERSISTENCE_HIERARCHY__

#include <algorithm>
#include <map>
#include <stdexcept>
#include <set>
#include <vector>

#include <boost/bimap.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

#include <aleph/persistentHomology/PersistencePairing.hh>

#include <aleph/topology/SimplicialComplex.hh>
#include <aleph/topology/UnionFind.hh>

namespace aleph
{

namespace detail
{

using AdjacencyGraph = boost::adjacency_list<
  boost::vecS,
  boost::vecS,
  boost::undirectedS,
  boost::no_property,
  boost::no_property>;

using SizeType = std::size_t;

/** Extracts the adjacency graph of a simplicial complex */
template <class Simplex>
std::pair<boost::bimap<typename Simplex::VertexType, SizeType>, AdjacencyGraph> extractZeroDimensionalAdjacencyGraph( const topology::SimplicialComplex<Simplex>& S )
{
  AdjacencyGraph adjacencyGraph;

  using Vertex           = typename Simplex::VertexType;
  using VertexDescriptor = boost::graph_traits<AdjacencyGraph>::vertex_descriptor;
  using bimap_type       = boost::bimap<Vertex, SizeType>;

  bimap_type indices;
  std::map<Vertex, VertexDescriptor> vdm;

  SizeType vertexIndex = 0;

  for( auto it = S.begin_dimension(); it != S.end_dimension(); ++it )
  {
    // Vertices
    if( it->dimension() == 0 )
    {
      indices.insert( typename bimap_type::value_type( *it->begin(), vertexIndex++ ) );

      auto vertex         = boost::add_vertex( adjacencyGraph );
      vdm[ *it->begin() ] = vertex;
    }

    // Edges
    else if( it->dimension() == 1 )
    {
      auto&& edge = *it;

      auto u = *( edge.begin()     );
      auto v = *( edge.begin() + 1 );

      // It is possible that the simplicial complex, being a _part_ of
      // a larger filtration, contains edges for which no vertices are
      // available.
      if( S.contains(u) && S.contains(v) )
      {
        boost::add_edge( vdm.at(u),
                         vdm.at(v),
                         adjacencyGraph );
      }
    }
  }

  return std::make_pair( indices, adjacencyGraph );
}

} // namespace detail

/**
  @class ExtendedPersistenceHierarchy
  @brief Functor for calculating the extended persistence hierarchy

  This class is a functor that calculates the extended persistence
  hierarchy of a given simplicial complex. The complex is supposed
  to be in filtration order. Currently, only features in dimension
  zero are supported by this functor.

  For more information, please refer to the paper

    Hierarchies and Ranks for Persistence Pairs
    Bastian Rieck, Heike Leitte, and Filip Sadlo
    Proceedings of TopoInVis 2017, Japan
*/

template <class Simplex> class ExtendedPersistenceHierarchy
{
public:
  using SimplicialComplex = topology::SimplicialComplex<Simplex>;
  using Vertex            = typename Simplex::VertexType;
  using SimplexPairing    = PersistencePairing<Vertex>;

  using EdgeType          = std::pair<Vertex, Vertex>;
  using Edges             = std::vector<EdgeType>;

private:

  /**
    Helper function for 'tagging' all edges in the simplicial complex
    with the next critical point. This is a very slow---but simple---
    way of decomposing the domain.
  */

  std::map<Simplex, Vertex> tagEdges( const SimplicialComplex& S )
  {
    std::set<Vertex> vertices;
    S.vertices( std::inserter( vertices, vertices.begin() ) );

    std::map<Vertex, Vertex>  criticalPointMapVertices;  // critical point map kept along with Union--Find structure
    std::map<Simplex, Vertex> criticalPointMapSimplices; // critical point map on edge (simplex) basis (return value)

    for( auto&& vertex : vertices )
      criticalPointMapVertices[vertex] = vertex;

    for( auto&& simplex : S )
    {
      if( simplex.dimension() != 1 )
        continue;

      Vertex u = *( simplex.begin() );
      Vertex v = *( simplex.begin() + 1 );

      auto youngerComponent = u;
      auto olderComponent   = v;

      if( youngerComponent != olderComponent )
      {
        auto index1 = S.index( Simplex(youngerComponent) );
        auto index2 = S.index( Simplex(olderComponent)   );

        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );

        Simplex creator = *( S.find( Simplex( youngerComponent ) ) );

        if( creator.data() == simplex.data() )
          criticalPointMapVertices[ youngerComponent ] = criticalPointMapVertices[ olderComponent ];
      }

      criticalPointMapSimplices[ simplex ] = criticalPointMapVertices[ olderComponent ];
    }

    return criticalPointMapSimplices;
  }

  /**
    Calculates the interlevel set of a given simplicial complex. This
    means extracting a subset in which the assigned weight of  _each_
    simplex lies between the upper and lower value.

    The function will return the interlevel set as a simplicial complex,
    as well as a Union--Find data structure for connectivity queries.
  */

  std::pair< SimplicialComplex, topology::UnionFind<Vertex> >
    makeInterlevelSet( typename Simplex::DataType lower, typename Simplex::DataType upper,
                       const SimplicialComplex& S )
  {
    if( lower > upper )
      std::swap( lower, upper );

    std::vector<Simplex> simplices;

    std::copy_if( S.begin(), S.end(),
                  std::back_inserter( simplices ),
                  [&lower, &upper] ( const Simplex& s )
                  {
                    return s.data() >= lower && s.data() <= upper && s.dimension() <= 1;
                  } );

    SimplicialComplex K = SimplicialComplex( simplices.begin(),
                                             simplices.end() );

    // -----------------------------------------------------------------
    //
    // Find all 'proper' vertices in the simplicial complex. It is
    // possible that not all of them exist as 0-simplices, though.

    std::set<Vertex> vertices;

    for( auto&& simplex : simplices )
    {
      if( simplex.dimension() == 0 )
        vertices.insert( *simplex.begin() );
    }

    // Traversal -------------------------------------------------------

    topology::UnionFind<Vertex> uf( vertices.begin(), vertices.end()  );

    for( auto&& simplex : K )
    {
      if( simplex.dimension() == 1 )
      {
        Vertex u = *( simplex.begin() );
        Vertex v = *( simplex.begin() + 1 );

        if( !uf.contains( u ) || !uf.contains( v ) )
          continue;

        auto youngerComponent = uf.find( u );
        auto olderComponent   = uf.find( v );

        if( youngerComponent == olderComponent )
          continue;

        auto index1 = S.index( Simplex( youngerComponent ) );
        auto index2 = S.index( Simplex( olderComponent ) );

        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );

        uf.merge( youngerComponent, olderComponent );
      }
    }

    return std::make_pair( K, uf );
  }

public:

  /**
    Given a simplicial complex, calculates its 0-dimensional persistent
    homology and the corresponding extended persistence hierarchy. As a
    result, this will return a simplex pairing and all the edges of the
    pairing. Edges refer to indices in the original simplicial complex.
  */

  std::pair<SimplexPairing, Edges> operator()( const SimplicialComplex& simplicialComplex )
  {
    using namespace detail;

    // Extract {0,1}-simplices -----------------------------------------

    // Note that there is a range predicate for the simplicial complex class
    // that does essentially the same. However, the predicate is not stable
    // with respect to the filtration of the simplicial complex. Thus, to
    // extract the desired simplices, the internal function cannot be used.

    std::vector<Simplex> simplices;

    std::copy_if( simplicialComplex.begin(), simplicialComplex.end(),
                  std::back_inserter( simplices ),
                  [] ( const Simplex& s ) { return s.dimension() <= 1; } );

    SimplicialComplex S = SimplicialComplex( simplices.begin(),
                                             simplices.end() );

    // Persistence calculation -----------------------------------------

    std::set<Vertex> vertices;
    S.vertices( std::inserter( vertices,
                               vertices.begin() ) );

    Edges edges;

    // Pairs indices of critical vertices. This may be used later on to
    // obtain a persistence diagram. Using a pairing is advantageous as
    // it does not operate on weights but on indices, which are unique.
    SimplexPairing pairing;

    // This map contains a simple decomposition of the domain in terms
    // of the 'next' critical point.
    //
    // In a proper---and faster---implementation, Morse--Smale complex
    // calculations could be used.
    auto edgeToCriticalPoint = tagEdges( S );

    // Keeps track of the critical points that are created along with
    // hierarchy. This is the key difference to the regular hierarchy
    // and permits the hierarchy to distinguish data sets even though
    // their persistence diagram coincides.
    std::map<Vertex, Vertex> vertexToCriticalPoint;
    for( auto&& vertex : vertices )
      vertexToCriticalPoint[vertex] = vertex;

    // Required in order to obtain persistence pairs along with the
    // edges of the persistence hierarchy.
    topology::UnionFind<Vertex> uf( vertices.begin(), vertices.end() );

    for( auto&& simplex : S )
    {
      // Only edges can destroy a component
      if( simplex.dimension() != 1 )
        continue;

      auto u = *( simplex.begin() );
      auto v = *( simplex.begin() + 1 );

      // ---------------------------------------------------------------
      //
      // Ensure that the younger component is _always_ the first
      // component. A component is younger if its representative
      // vertex precedes the other vertex in the filtration.
      auto youngerComponent = uf.find( u );
      auto olderComponent   = uf.find( v );

      // If the component has already been merged by some other edge, we are
      // not interested in it any longer.
      if( youngerComponent == olderComponent )
        continue;

      {
        auto index1 = S.index( Simplex(youngerComponent) );
        auto index2 = S.index( Simplex(olderComponent)   );

        // The younger component has the _larger_ index as it is born _later_
        // in the filtration.
        if( index1 < index2 )
          std::swap( youngerComponent, olderComponent );
      }

      // Prepare information about creators ----------------------------
      //
      // Creator simplex for the simplex pairing below. I know that this
      // simplex must exist in the complex so I don't check for iterator
      // validity here.
      auto youngerCreator         = *( S.find( Simplex( youngerComponent ) ) );
      auto olderCreator           = *( S.find( Simplex( olderComponent ) ) );
      auto youngerCriticalSimplex = *( S.find( Simplex( vertexToCriticalPoint[youngerComponent] ) ) );
      auto olderCriticalSimplex   = *( S.find( Simplex( vertexToCriticalPoint[olderComponent]   ) ) );

      // Zero-persistence information; assign critical point of the
      // older component directly. This ensures that we are able to
      // obtain a proper decomposition.
      if( youngerCreator.data() == simplex.data() )
        vertexToCriticalPoint[youngerComponent] = olderComponent;
      else
      {
        // Ensures that the oldest, highest/lowest critical simplex is
        // being used to calculate the interlevel set. Else, it may be
        // impossible for a critical point to be reached.
        if( S.index( youngerCriticalSimplex ) < S.index( olderCriticalSimplex ) )
          std::swap( youngerCriticalSimplex, olderCriticalSimplex );

        auto clsPair
          = makeInterlevelSet(
              olderCriticalSimplex.data(), simplex.data(),
              S );

        bool inSameComponent
          =   ( clsPair.second.contains( *olderCriticalSimplex.begin() ) && clsPair.second.contains( *youngerCriticalSimplex.begin() ) )
           && ( clsPair.second.find( *olderCriticalSimplex.begin() ) == clsPair.second.find( *youngerCriticalSimplex.begin() ) );

        if( inSameComponent )
        {
          using V = boost::graph_traits<AdjacencyGraph>::vertex_descriptor;

          boost::bimap<Vertex, SizeType> vim;
          AdjacencyGraph G;

          std::tie( vim, G )
            = extractZeroDimensionalAdjacencyGraph( clsPair.first );

          std::vector<V> p( boost::num_vertices( G ) );

          auto u = vim.left.at( *olderCriticalSimplex.begin() );
          auto v = vim.left.at( *youngerCriticalSimplex.begin() );
          p[u]   = u;

          boost::breadth_first_search( G,
                                       boost::vertex( u, G ),
                                       boost::visitor(
                                        boost::make_bfs_visitor(
                                          boost::record_predecessors( &p[0], boost::on_tree_edge() ) ) ) );

          std::set<Vertex> criticalPoints;

          while( true )
          {
            auto parent = p.at( v );
            auto s      = *S.find( Simplex( {vim.right.at(v),vim.right.at(parent)} ) );

            if( parent == v )
              break;

            v = parent;

            // Find out which critical point the identified edge
            // belongs to.
            criticalPoints.insert( edgeToCriticalPoint.at( s ) );
          }

          // Exactly two critical points (i.e. the ones we were
          // looking for); hence, insert younger component as a
          // child of the youngest critical point.
          if( criticalPoints.size() == 2 )
          {
            edges.push_back( std::make_pair(
              vertexToCriticalPoint[olderComponent],
              youngerComponent )
            );
          }

          // More critical points; connect the critical points
          // according to the usual persistence hierarchy.
          else
          {
            edges.push_back( std::make_pair(
              olderComponent,
              youngerComponent )
            );
          }
        }

        // Not in the same component; connect the critical points
        // according to the usual persistence hierarchy.
        else
        {
          edges.push_back( std::make_pair(
            olderComponent,
            youngerComponent )
          );
        }

        // The youngest critical point along the current connected
        // component has been changed.
        vertexToCriticalPoint[olderComponent] = youngerComponent;
      }

      pairing.add( Vertex( S.index( Simplex( youngerCreator ) ) ),
                   Vertex( S.index( simplex ) ) );

      uf.merge( youngerComponent,
                olderComponent );
    }

    // Add features of infinite persistence to the pairing -------------

    std::set<Vertex> roots;
    uf.roots( std::inserter( roots, roots.begin() ) );

    for( auto&& root : roots )
      pairing.add( Vertex( S.index( root ) ) );

    return std::make_pair( pairing, edges );
  }
};

} // namespace aleph

#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: George Rosenberger $
// $Authors: George Rosenberger, Hannes Roest $
// --------------------------------------------------------------------------

#include <OpenMS/MATH/MISC/RANSAC.h>
#include <OpenMS/MATH/STATISTICS/LinearRegression.h>
#include <OpenMS/CONCEPT/LogStream.h> // LOG_DEBUG

#include <numeric>
#include <boost/math/special_functions/erf.hpp>
#include <algorithm>

namespace OpenMS
{
  std::pair<double, double > Math::RANSAC::llsm_fit_(std::vector<std::pair<double, double> >& pairs)
  {
    std::vector<double> x, y;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      x.push_back(it->first);
      y.push_back(it->second);
    }

    // GSL implementation
    //double c0, c1, cov00, cov01, cov11, sumsq;
    //double* xa = &x[0];
    //double* ya = &y[0];
    //gsl_fit_linear(xa, 1, ya, 1, pairs.size(), &c0, &c1, &cov00, &cov01, &cov11, &sumsq);

    // OpenMS::MATH implementation
    Math::LinearRegression lin_reg;
    lin_reg.computeRegression(0.95, x.begin(), x.end(), y.begin());
    double c0, c1;
    c0 = lin_reg.getIntercept();
    c1 = lin_reg.getSlope();

    return(std::make_pair(c0,c1));
  }

  double Math::RANSAC::llsm_rsq(std::vector<std::pair<double, double> >& pairs)
  {
    std::vector<double> x, y;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      x.push_back(it->first);
      y.push_back(it->second);
    }

    // GSL implementation
    //double* xa = &x[0];
    //double* ya = &y[0];
    //double r = gsl_stats_correlation(xa, 1, ya, 1, pairs.size());
    //return(r);

    // OpenMS::MATH implementation
    Math::LinearRegression lin_reg;
    lin_reg.computeRegression(0.95, x.begin(), x.end(), y.begin());

    return lin_reg.getRSquared();
  }

  double Math::RANSAC::llsm_rss_(std::vector<std::pair<double, double> >& pairs, std::pair<double, double >& coefficients)
  {
    double rss = 0;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      rss += pow(it->second - (coefficients.first + ( coefficients.second * it->first)), 2);
    }

    return rss;
  }

  std::vector<std::pair<double, double> > Math::RANSAC::llsm_rss_inliers_(
      std::vector<std::pair<double, double> >& pairs,
      std::pair<double, double >& coefficients, double max_threshold)
  {
    std::vector<std::pair<double, double> > alsoinliers;

    for (std::vector<std::pair<double, double> >::iterator it = pairs.begin(); it != pairs.end(); ++it)
    {
      if (pow(it->second - (coefficients.first + ( coefficients.second * it->first)), 2) < max_threshold)
      {
        alsoinliers.push_back(*it);
      }
    }

    return alsoinliers;
  }

  std::vector<std::pair<double, double> > Math::RANSAC::ransac(
      std::vector<std::pair<double, double> >& pairs, size_t n, size_t k, double t, size_t d, bool test)
  {
    // implementation of the RANSAC algorithm according to http://wiki.scipy.org/Cookbook/RANSAC.

    std::vector<std::pair<double, double> > maybeinliers, test_points, alsoinliers, betterdata, bestdata;

    double besterror = std::numeric_limits<double>::max();
    double bettererror;
#ifdef DEBUG_MRMRTNORMALIZER
    std::pair<double, double > bestcoeff;
    double betterrsq = 0;
    double bestrsq = 0;
#endif

    for (size_t ransac_int=0; ransac_int<k; ransac_int++)
    {
      std::vector<std::pair<double, double> > pairs_shuffled = pairs;

      if (!test)
      { // disables random selection in test mode
        std::random_shuffle(pairs_shuffled.begin(), pairs_shuffled.end());
      }

      maybeinliers.clear();
      test_points.clear();
      std::copy( pairs_shuffled.begin(), pairs_shuffled.begin()+n, std::back_inserter(maybeinliers) );
      std::copy( pairs_shuffled.begin()+n, pairs_shuffled.end(), std::back_inserter(test_points) );

      std::pair<double, double > coeff = Math::RANSAC::llsm_fit_(maybeinliers);

      alsoinliers = Math::RANSAC::llsm_rss_inliers_(test_points,coeff,t);

      if (alsoinliers.size() > d)
      {
        betterdata = maybeinliers;
        betterdata.insert( betterdata.end(), alsoinliers.begin(), alsoinliers.end() );
        std::pair<double, double > bettercoeff = Math::RANSAC::llsm_fit_(betterdata);
        bettererror = Math::RANSAC::llsm_rss_(betterdata,bettercoeff);
#ifdef DEBUG_MRMRTNORMALIZER
        betterrsq = Math::RANSAC::llsm_rsq(betterdata);
#endif

        if (bettererror < besterror)
        {
          besterror = bettererror;
#ifdef DEBUG_MRMRTNORMALIZER
          bestcoeff = bettercoeff;
#endif
          bestdata = betterdata;

#ifdef DEBUG_MRMRTNORMALIZER
          bestrsq = betterrsq;
          std::cout << "RANSAC " << ransac_int << ": Points: " << betterdata.size() << " RSQ: " << bestrsq << " Error: " << besterror << " c0: " << bestcoeff.first << " c1: " << bestcoeff.second << std::endl;
#endif
        }
      }
    }

#ifdef DEBUG_MRMRTNORMALIZER
    std::cout << "=======STARTPOINTS=======" << std::endl;
    for (std::vector<std::pair<double, double> >::iterator it = bestdata.begin(); it != bestdata.end(); ++it)
    {
      std::cout << it->first << "\t" << it->second << std::endl;
    }
    std::cout << "=======ENDPOINTS=======" << std::endl;
#endif

    return(bestdata);
  }

}


#pragma once

#include <iostream>
#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/StdVector>

#ifdef __SRRG_PARALLEL_KDTREE__
#include <omp.h>
#endif

namespace srrg2_core {

  /**
     KDTree: implements a KDTree, it requires a type T
     and a dimension D
  */
  template <class T, size_t D>
  class KDTree {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // typedef for defining a vector variable sized points
    using VectorTD       = Eigen::Matrix<T, D, 1>;
    using VectorTDVector = std::vector<VectorTD, Eigen::aligned_allocator<VectorTD>>;
    enum NodeType { Leaf = 0x0, Middle = 0x1 };

    /**
       TreeNode class. Represents a base class for a node in the search tree.

       if the type is a Middle node
       it represents a splitting plane, and has 2 child nodes
       that refer to the set of points to the two sides of the splitting plane.
       A splitting plane is parameterized as a point on a plane and as a normal
       to the plane.

       If the type is a leaf
       it represents a bucket containing points in a neighborhood
       To avoid copies, the bucket is kept as a range
       _min_index.._max_indexindices and the points
       in the _points and _indices
    */
    class TreeNode {
    public:
      friend class KDTree;
      //! ctor
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      TreeNode(KDTree<T, D>* tree_, int node_num = 0) :
        _tree(tree_),
        _node_num(node_num),
        _node_type(KDTree<T, D>::Leaf) {
        _mean.setZero();
        _normal.setZero();
        _left_child  = 0;
        _right_child = 0;
        _min_index   = -1;
        _max_index   = -1;
        _num_points  = 0;
      }

      TreeNode(KDTree<T, D>* tree_,
               int node_num,
               const VectorTD& mean_,
               const VectorTD& normal_,
               TreeNode* left_child  = 0,
               TreeNode* right_child = 0) :
        _tree(tree_),
        _node_num(node_num),
        _node_type(KDTree<T, D>::Middle) {
        _min_index = _max_index = -1;
        assert(normal_.rows() == D);
        assert(mean_.rows() == D);
        _normal      = normal_;
        _mean        = mean_;
        _num_points  = 0;
        _left_child  = left_child;
        _right_child = right_child;
        if (_left_child) {
          _num_points += _left_child->numPoints();
        }
        if (_right_child) {
          _num_points += _right_child->numPoints();
        }
      }

      //! dtor
      ~TreeNode() {
        if (_left_child) {
          delete _left_child;
          _left_child = 0;
        }
        if (_right_child) {
          delete _right_child;
          _right_child = 0;
        }
      }

      //! function to search for the neighbor
      //! @param answer: the neighbor found
      //! @param query: the point to search
      //! @param maximum distance allowed for a point
      //! @returns the distance of the closest point. -1 if no point found
      //! within range
      T findNeighbor(VectorTD& answer,
                     int& index,
                     const VectorTD& query,
                     const T max_distance) const {
        switch (_node_type) {
          case KDTree<T, D>::Leaf: {
            T d_max                         = std::numeric_limits<T>::max();
            const VectorTDVector& points    = this->_tree->_points;
            const std::vector<int>& indices = this->_tree->_indices;
            for (size_t i = _min_index; i < _max_index; ++i) {
              T d = (points[i] - query).squaredNorm();
              if (d < d_max) {
                answer = points[i];
                index  = indices[i];
                d_max  = d;
              }
            }

            if (d_max > max_distance * max_distance) {
              index = -1;
              return -1;
            }
            return d_max;
          }

          case KDTree<T, D>::Middle: {
            bool is_left    = side(query);
            TreeNode* child = is_left ? _left_child : _right_child;
            if (child) {
              if (child->_node_type != KDTree<T, D>::Middle &&
                  child->_node_type != KDTree<T, D>::Leaf) {
                std::cerr << "KDTree::findNeighbor|ERROR, calling sanity check (" << _tree << ")\n";
                std::cerr << "KDTree::findNeighbor|good queries" << _tree->good_queries
                          << std::endl;
                std::cerr << "KDTree::findNeighbor|result: " << _tree->sanityCheck() << std::endl;
              }
              return child->findNeighbor(answer, index, query, max_distance);
            }
          }
            return -1;
          default:
            std::cerr << "KDTree::findNeighbor|ERROR, sanity check (" << _tree
                      << ") : " << _tree->sanityCheck() << std::endl;
            throw std::runtime_error("KDTree::findNeighbor|ERROR, unknown node type");
        }
      }

      //! function to search for all the points near to the query point
      //! @param answer: the neighbors found
      //! @param query: the point to search
      //! @param maximum distance allowed for a point
      //! @returns void
      void findNeighbors(VectorTDVector& answers,
                         std::vector<int>& indices,
                         const VectorTD& query,
                         const T maximum_squared_distance) const {
        assert(maximum_squared_distance >= 0);
        switch (_node_type) {
          case KDTree<T, D>::Leaf: {
            // ds return all points in this leaf that satisfy the distance
            assert(_max_index >= _min_index);
            const size_t number_of_points = _max_index - _min_index;
            answers.clear();
            answers.reserve(number_of_points);
            indices.clear();
            indices.reserve(number_of_points);
            for (size_t i = _min_index; i < _max_index; ++i) {
              assert(i < _tree->_points.size());
              const T distance = (_tree->_points[i] - query).squaredNorm();
              if (distance < maximum_squared_distance) {
                answers.emplace_back(_tree->_points[i]);
                indices.emplace_back(_tree->_indices[i]);
              }
            }
            return;
          }

          case KDTree<T, D>::Middle: {
            bool is_left    = side(query);
            TreeNode* child = is_left ? _left_child : _right_child;
            if (child) {
              if (child->_node_type != KDTree<T, D>::Middle &&
                  child->_node_type != KDTree<T, D>::Leaf) {
                std::cerr << "KDTree::findNeighbors|ERROR, calling sanity check (" << _tree << ")"
                          << std::endl;
                std::cerr << "KDTree::findNeighbors|good queries" << _tree->good_queries
                          << std::endl;
                std::cerr << "KDTree::findNeighbors|result: " << _tree->sanityCheck() << std::endl;
              }
              child->findNeighbors(answers, indices, query, maximum_squared_distance);
              return;
            }
          }
            return;

          default:
            std::cerr << "KDTree::findNeighbors|ERROR, sanity check (" << _tree
                      << ") : " << _tree->sanityCheck() << std::endl;
            throw std::runtime_error("KDTree::findNeighbors|ERROR, unknown node type");
        }
      }

      size_t numPoints() const {
        return _num_points;
      }

      size_t minIndex() const {
        return _min_index;
      }

      size_t maxIndex() const {
        return _max_index;
      }

      inline void setMinIndex(size_t min_index) {
        _min_index = min_index;
      }

      inline void setMaxIndex(size_t max_index) {
        _max_index = max_index;
      }

      //! mean const accessor
      inline const VectorTD& mean() const {
        return _mean;
      }

      //! normal const accessor
      inline const VectorTD& normal() const {
        return _normal;
      }

      //! mean left accessor
      inline TreeNode* leftChild() const {
        return _left_child;
      }

      //! mean right accessor
      inline TreeNode* rightChild() const {
        return _right_child;
      }

      inline bool side(const VectorTD& query_point) const {
        return _normal.dot(query_point - _mean) < 0;
      }

    protected:
      KDTree<T, D>* _tree;
      int _node_num;
      const KDTree<T, D>::NodeType _node_type;
      size_t _min_index;
      size_t _max_index;
      VectorTD _normal;
      VectorTD _mean;
      size_t _num_points;
      TreeNode* _left_child;
      TreeNode* _right_child;
    };

    //! ctor
    KDTree(const VectorTDVector& points_, T max_leaf_range, size_t min_leaf_points = 20) {
      good_queries = 0;
      _num_nodes   = 0;
      _points      = points_;
      _aux_points  = points_;
      _indices.resize(points_.size());
      _aux_indices.resize(points_.size());
      _min_leaf_points = min_leaf_points;
      for (size_t i = 0; i < _indices.size(); ++i) {
        _indices[i] = i;
      }
      _root = _buildTree(0, points_.size(), max_leaf_range, 0);
    }

    //! dtor
    ~KDTree() {
      if (_root) {
        delete _root;
      }
      _root = 0;
    }

    bool sanityCheck() const {
      std::vector<int> checked_indices(_points.size());
      std::fill(checked_indices.begin(), checked_indices.end(), -1);
      const size_t k = sanityCheck(checked_indices, 0, _root);
      if (k != _points.size()) {
        throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, illegal size reported");
      }
      std::sort(checked_indices.begin(), checked_indices.end(), std::less<int>());
      for (size_t i = 0; i < checked_indices.size(); i++) {
        if (i != static_cast<size_t>(checked_indices[i])) {
          throw std::runtime_error("KDTree::sanityCheck(void)|ERROR, missing indices");
        }
      }
      return true;
    }

    int sanityCheck(std::vector<int>& checked_indices, int k, const TreeNode* node) const {
      if (!node) {
        return k;
      }
      switch (node->_node_type) {
        case KDTree<T, D>::Leaf:
          for (size_t i = node->_min_index; i < node->_max_index; i++) {
            int idx = _indices[i];
            if (checked_indices[k] != -1) {
              throw std::runtime_error("KDTree::sanityCheck|ERROR, writing on an occupied index");
            }
            checked_indices[k] = idx;
            k++;
          }
          return k;
        case KDTree<T, D>::Middle:
          k = sanityCheck(checked_indices, k, node->_left_child);
          k = sanityCheck(checked_indices, k, node->_right_child);
          return k;
        default:
          throw std::runtime_error("KDTree::sanityCheck|ERROR, illegal type index");
      }
    }

    //! num_nodes accessor
    inline size_t numNodes() const {
      return _num_nodes;
    }

    inline size_t numPoints() const {
      return _points.size();
    }

    //! function to search for the neighbor
    //! @param answer: the neighbor found
    //! @param query: the point to search
    //! @param maximum distance allowed for a point
    //! @returns the distance of the closest point. -1 if no point found within
    //! range
    inline T
    findNeighbor(VectorTD& answer, int& index, const VectorTD& query, const T max_distance) const {
      if (!_root) {
        throw std::runtime_error("KDTree::findNeighbor|ERROR, no root node");
      }
      good_queries++;
      return _root->findNeighbor(answer, index, query, max_distance);
    }

    //! function to search for all the points in the same leaf of the query
    //! point
    //! @param answers: the neighbors found
    //! @param indices: the indices of the neighbors found
    //! @param query: the query point
    //! @param max_distance: maximum distance allowed for a point
    //! @returns void

    inline void findNeighbors(VectorTDVector& answers,
                              std::vector<int>& indices,
                              const VectorTD& query,
                              const T max_distance) const {
      if (!_root) {
        throw std::runtime_error("KDTree::findNeighbors|ERROR, no root node");
      }

      good_queries++;
      _root->findNeighbors(answers, indices, query, max_distance);
    }

    inline void printKDTree() {
      _printKDTree(_root);
    }

    mutable int good_queries;

  protected:
    /**
       Partitions a point vector in two vectors, computing the splitting plane
       as the largest eigenvalue of the point covariance
       @param mean: the returned mean of the splitting plane
       @param normal: the normal of the splitting plane
       @param left: the returned left vector of points
       @param right: the returned right vector of points
       @param points: the array of points
       @returns the distance of the farthest point from the plane
    */
    T _splitPoints(VectorTD& mean,
                   VectorTD& normal,
                   size_t& num_left_points,
                   const size_t min_index,
                   const size_t max_index) {
      // if points empty, nothing to do
      if (min_index == max_index) {
        return 0;
      }

      const size_t num_points            = max_index - min_index;
      const T inverse_num_points         = 1.0 / num_points;
      VectorTD sum                       = VectorTD::Zero();
      Eigen::Matrix<T, D, D> squared_sum = Eigen::Matrix<T, D, D>::Zero();
      Eigen::Matrix<T, D, D> covariance  = Eigen::Matrix<T, D, D>::Zero();
      for (size_t i = min_index; i < max_index; ++i) {
        sum += _points[i];
        squared_sum += _points[i] * _points[i].transpose();
      }
      mean       = sum * inverse_num_points;
      covariance = squared_sum * inverse_num_points - mean * mean.transpose();

      // eigenvalue decomposition
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, D, D>> solver;
      solver.compute(covariance, Eigen::ComputeEigenvectors);
      normal = solver.eigenvectors().col(D - 1).normalized();

      // the following var will contain the range of points along the normal
      // vector
      T max_distance_from_plane = 0;

      // run through the points and split them in the left or the right set
      size_t left_index  = min_index;
      size_t right_index = max_index;

      size_t num_left  = 0;
      size_t num_right = 0;
      for (size_t i = min_index; i < max_index; ++i) {
        T distance_from_plane = normal.dot(_points[i] - mean);
        if (fabs(distance_from_plane) > max_distance_from_plane) {
          max_distance_from_plane = fabs(distance_from_plane);
        }

        bool side = distance_from_plane < 0;
        if (side) {
          _aux_points[left_index]  = _points[i];
          _aux_indices[left_index] = _indices[i];
          left_index++;
          num_left++;
        } else {
          right_index--;
          _aux_points[right_index]  = _points[i];
          _aux_indices[right_index] = _indices[i];
          num_right++;
        }
      }
      assert(max_index - min_index == num_right + num_left);
      for (size_t i = min_index; i < max_index; ++i) {
        _points[i]  = _aux_points[i];
        _indices[i] = _aux_indices[i];
      }

      num_left_points = num_left;
      return max_distance_from_plane;
    }

    //! function to build the tree
    //! @param points: the points
    //! @param max_leaf_range: specify the size of the "box" below which a leaf
    //! node is generated returns the root of the search tree
    TreeNode*
    _buildTree(const size_t min_index, const size_t max_index, const T max_leaf_range, int level) {
      const size_t num_points = max_index - min_index;
      if (!num_points) {
        return 0;
      }

      VectorTD mean;
      VectorTD normal;
      size_t num_left_points = 0;

      const T range = _splitPoints(mean, normal, num_left_points, min_index, max_index);
      assert(range >= 0);

      TreeNode* node = 0;
      if (range < max_leaf_range || num_points < _min_leaf_points) {
        node = new TreeNode(this, _num_nodes);
        node->setMinIndex(min_index);
        node->setMaxIndex(max_index);
        node->_num_points = num_points;
      } else {
        TreeNode *left_tree, *right_tree;

#ifdef __SRRG_PARALLEL_KDTREE__
        int num_threads = omp_get_max_threads();
        int split_level = -1;
        if (level > 0)
          split_level = floor(log(num_threads) / log(2));

        if (split_level == level) {
#pragma omp parallel sections
          {
#pragma omp section
            {
              left_tree =
                _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
            }
#pragma omp section
            {
              right_tree =
                _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
            }
          }
        } else {
#endif //__SRRG_PARALLEL_KDTREE__

          left_tree = _buildTree(min_index, min_index + num_left_points, max_leaf_range, level + 1);
          right_tree =
            _buildTree(min_index + num_left_points, max_index, max_leaf_range, level + 1);
#ifdef __SRRG_PARALLEL_KDTREE__
        }
#endif
        node = new TreeNode(this, _num_nodes, mean, normal, left_tree, right_tree);
      }
      _num_nodes++;
      return node;
    }

    void _printKDTree(TreeNode* node) {
      switch (node->_node_type) {
        case KDTree<T, D>::Leaf:
          std::cerr << "Leaf: " << std::endl;
          for (size_t i = node->_min_index; i < node->_max_index; ++i) {
            std::cerr << _points[i].transpose() << std::endl;
          }
          std::cerr << std::endl;
          break;
        case KDTree<T, D>::Middle:
          _printKDTree(node->leftChild());
          _printKDTree(node->rightChild());
          break;
      }
    }

    size_t _num_nodes;
    size_t _min_leaf_points;
    TreeNode* _root;
    VectorTDVector _aux_points;
    VectorTDVector _points;
    std::vector<int> _indices;
    std::vector<int> _aux_indices;
  };

} // namespace srrg2_core


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T, std::size_t N>
struct make_vector_type {
	using type =
		typename std::vector<typename make_vector_type<T, (N - 1)>::type>;
};

template <typename T>
struct make_vector_type<T, 0> {
	using type = typename std::vector<T>;
};

template <typename T, size_t N>
auto make_vector_impl(const std::vector<std::size_t>& ls, T init_value) {
	if constexpr(N == 0) {
		return std::vector<T>(ls[N], init_value);
	} else {
		return typename make_vector_type<T, N>::type(
			ls[N], make_vector_impl<T, (N - 1)>(ls, init_value));
	}
}

template <typename T, std::size_t N>
auto make_vector(const std::size_t (&ls)[N], T init_value) {
	std::vector<std::size_t> dimensions(N);
	for(int i = 0; i < N; i++) {
		dimensions[N - i - 1] = ls[i];
	}
	return make_vector_impl<T, N - 1>(dimensions, init_value);
}

template <typename T>
std::vector<T> make_vector(std::size_t size, T init_value) {
	return std::vector<T>(size, init_value);
}

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max<T>(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min<T>(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

struct Transform {
	ll e00;
	ll e01;
	ll e10;
	ll e11;
};

Transform dot(Transform a, Transform b) {
	Transform ret = {a.e00 * b.e00 + a.e01 * b.e10,
					 a.e00 * b.e01 + a.e01 * b.e11,
					 a.e10 * b.e00 + a.e11 * b.e10,
					 a.e10 * b.e01 + a.e11 * b.e11};
	return ret;
}

pair<ll, ll> dot(Transform a, pair<ll, ll> b) {
	pair<ll, ll> ret = {b.first * a.e00 + b.second * a.e01,
						b.first * a.e10 + b.second * a.e11};
	return ret;
}

pair<ll, ll> sum(pair<ll, ll> a, pair<ll, ll> b) {
	return make_pair(a.first + b.first, a.second + b.second);
}

int main() {
	int n;
	cin >> n;
	VLL x(n), y(n);
	REP(i, n) { cin >> x[i] >> y[i]; }
	int m;
	cin >> m;
	VI op(m), p(m);
	REP(i, m) {
		cin >> op[i];
		if(op[i] > 2) {
			cin >> p[i];
		}
	}
	int q;
	cin >> q;
	VI a(q), b(q);
	REP(i, q) { cin >> a[i] >> b[i]; }

	vector<Transform> transforms(m + 1, {1, 0, 0, 1});
	vector<pair<ll, ll>> linear(m + 1, {0, 0});
	Transform clockwise_rotate = {0, 1, -1, 0};
	Transform counter_clockwise_rotate = {0, -1, 1, 0};
	REP(i, m) {
		if(op[i] == 1) {
			transforms[i + 1] = dot(clockwise_rotate, transforms[i]);
			linear[i + 1] = dot(clockwise_rotate, linear[i]);
		} else if(op[i] == 2) {
			transforms[i + 1] = dot(counter_clockwise_rotate, transforms[i]);
			linear[i + 1] = dot(counter_clockwise_rotate, linear[i]);
		} else if(op[i] == 3) {
			transforms[i + 1] = dot(Transform({-1, 0, 0, 1}), transforms[i]);
			linear[i + 1] = sum(dot(Transform({-1, 0, 0, 1}), linear[i]),
								make_pair(p[i] * 2, 0));
		} else {
			transforms[i + 1] = dot(Transform({1, 0, 0, -1}), transforms[i]);
			linear[i + 1] = sum(dot(Transform({1, 0, 0, -1}), linear[i]),
								make_pair(0, p[i] * 2));
		}
	}

	REP(i, q) {
		pair<ll, ll> point = make_pair(x[b[i] - 1], y[b[i] - 1]);
		auto result = sum(dot(transforms[a[i]], point), linear[a[i]]);
		cout << result.first << " " << result.second << endl;
	}
	return 0;
}


/*Copyright (c) 2020 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

/*
 * AOCAP.cpp
 */
#include "BasisSet.h"
#include <numgrid.h>
#include "utils.h"
#include "gto_ordering.h"
#include "AOCAP.h"
#include <chrono>
#include <ctime>
#include <thread>
#include <math.h>
#include <vector>
#include <iostream>
#include "opencap_exception.h"
#include <Eigen/Dense>

AOCAP::AOCAP(std::vector<Atom> geometry,std::map<std::string, std::string> params)
{
	verify_cap_parameters(params);
	double capx,capy,capz,rcut,radial,angpts;
	std::stringstream capxss(params["cap_x"]);
	std::stringstream capyss(params["cap_y"]);
	std::stringstream capzss(params["cap_z"]);
	std::stringstream rcutss(params["r_cut"]);
	std::stringstream radialss(params["radial_precision"]);
	std::stringstream angularss(params["angular_points"]);
	capxss >> capx;
	capyss >> capy;
	capzss >> capz;
	rcutss >> rcut;
	radialss >> radial;
	angularss >> angpts;
	//now fill class members
	cap_type = params["cap_type"];
	cap_x = capx;
	cap_y= capy;
	cap_z = capz;
	r_cut = rcut;
	radial_precision = pow(10,-1.0*radial);
	angular_points = angpts;
	atoms = geometry;
}

double AOCAP::eval_voronoi_cap(double x, double y, double z)
{
    double atom_distances[atoms.size()];
    double r_closest=1000.0;
    //find r closest and fill up our distances array
    for(size_t j=0;j<atoms.size();j++)
    {
        if(atoms[j].Z!=0)
        {
			  double dist_x= (x-atoms[j].coords[0]) * (x-atoms[j].coords[0]);
			  double dist_y= (y-atoms[j].coords[1]) * (y-atoms[j].coords[1]);
			  double dist_z= (z-atoms[j].coords[2]) * (z-atoms[j].coords[2]);
			  double dist=sqrt(dist_x+dist_y+dist_z);
			  if(dist<r_closest)
				 r_closest=dist;
			  atom_distances[j]=dist;
        }
    }
    double weights[atoms.size()];
    for(size_t j=0;j<atoms.size();j++)
    {
  	  if(atoms[j].Z!=0)
  		  weights[j]=1/pow((pow(atom_distances[j],2.0)-pow(r_closest,2.0) +1),2.0);
  	  else
  		  weights[j]=0;
    }
    double numerator=0.0;
    double denominator=0.0;
    for(size_t j=0;j<atoms.size();j++)
    {
  	  numerator+=atom_distances[j]*atom_distances[j]*weights[j];
  	  denominator+=weights[j];
    }
    double r=sqrt(numerator/denominator);
    if(r<r_cut)
  	  return 0;
    else
  	  return (r-r_cut)*(r-r_cut);
}

double AOCAP::eval_box_cap(double x, double y, double z)
{
    double result = 0;
    if(abs(x)>cap_x)
	 result += (abs(x)-cap_x) * (abs(x)-cap_x);
    if(abs(y)>cap_y)
   	 result += (abs(y)-cap_y) * (abs(y)-cap_y);
    if(abs(z)>cap_z)
   	 result += (abs(z)-cap_z) * (abs(z)-cap_z);
    return result;
}

double AOCAP::eval_pot(double x, double y, double z)
{
	if(compare_strings(cap_type, "box"))
		return eval_box_cap(x,y,z);
	else if (compare_strings(cap_type,"voronoi"))
		return eval_voronoi_cap(x,y,z);
	return 0;
}

void AOCAP::compute_ao_cap_mat(Eigen::MatrixXd &cap_mat, BasisSet bs)
{
	double x_coords_bohr[atoms.size()];
	double y_coords_bohr[atoms.size()];
	double z_coords_bohr[atoms.size()];
	int nuc_charges[atoms.size()];
	for(size_t i=0;i<atoms.size();i++)
	{
		x_coords_bohr[i]=atoms[i].coords[0];
		y_coords_bohr[i]=atoms[i].coords[1];
		z_coords_bohr[i]=atoms[i].coords[2];
		nuc_charges[i]=atoms[i].Z;
		if (atoms[i].Z==0)
			nuc_charges[i]=1; //choose bragg radius for H for ghost atoms
	}
    //double radial_precision = radial_precision;
    int min_num_angular_points = angular_points;
    int max_num_angular_points = angular_points;
	for(size_t i=0;i<atoms.size();i++)
	{
		//allocate and create grid
		context_t *context = numgrid_new_atom_grid(radial_precision,
		                                 min_num_angular_points,
		                                 max_num_angular_points,
		                                 nuc_charges[i],
		                                 bs.alpha_max(atoms[i]),
		                                 bs.max_L(),
		                                 &bs.alpha_min(atoms[i])[0]);
		int num_points = numgrid_get_num_grid_points(context);
        double *grid_x_bohr = new double[num_points];
        double *grid_y_bohr = new double[num_points];
        double *grid_z_bohr = new double[num_points];
        double *grid_w = new double[num_points];
        numgrid_get_grid(  context,
                           atoms.size(),
                           i,
                           x_coords_bohr,
                           y_coords_bohr,
                           z_coords_bohr,
                           nuc_charges,
                           grid_x_bohr,
                           grid_y_bohr,
                           grid_z_bohr,
                           grid_w);
		evaluate_grid_on_atom(cap_mat,bs,grid_x_bohr,grid_y_bohr,grid_z_bohr,grid_w,num_points);
	}
}

void AOCAP::evaluate_grid_on_atom(Eigen::MatrixXd &cap_mat,BasisSet bs,double* grid_x_bohr,
		double *grid_y_bohr,double *grid_z_bohr,double *grid_w,int num_points)
{
	//pre-calculate cap matrix on grid
	std::vector<float> cap_values (num_points);
	#pragma omp parallel for
	for (int i=0;i<num_points;i++)
		cap_values[i]= eval_pot(grid_x_bohr[i],grid_y_bohr[i],grid_z_bohr[i]);
	std::vector<std::vector<float>> bf_values;
	//pre-calculate basis functions on grid
	for(size_t i=0;i<bs.basis.size();i++)
	{
		Shell my_shell = bs.basis[i];
		std::vector<std::array<size_t,3>> order = opencap_carts_ordering(my_shell.l);
		for(size_t j=0;j<my_shell.num_carts();j++)
		{
			std::vector<float> vec(num_points);
			std::array<size_t,3> cart = order[j];
			#pragma omp parallel for
			for (int k=0;k<num_points;k++)
				vec[k]= my_shell.evaluate(grid_x_bohr[k],grid_y_bohr[k],grid_z_bohr[k],cart[0],cart[1],cart[2]);
			bf_values.push_back(vec);
		}
	}
    //now lets evaluate
	for (size_t i=0;i<bs.num_carts();i++)
	{
		for(size_t j=i;j<bs.num_carts();j++)
		{
			for(int k=0;k<num_points;k++)
			{
				cap_mat(i,j)+=grid_w[k]*cap_values[k]*bf_values[i][k]*bf_values[j][k];
				if (j!=i)
					cap_mat(j,i)+=grid_w[k]*cap_values[k]*bf_values[i][k]*bf_values[j][k];
			}
		}
	}
}

void AOCAP::verify_cap_parameters(std::map<std::string,std::string> &parameters)
{
	std::vector<std::string> missing_keys;
	if(parameters.find("cap_type")==parameters.end())
		opencap_throw("Error: Missing cap_type keyword.");
	if(compare_strings(parameters["cap_type"],"box"))
	{
		if(parameters.find("cap_x")==parameters.end())
			missing_keys.push_back("cap_x");
		if(parameters.find("cap_y")==parameters.end())
			missing_keys.push_back("cap_y");
		if (parameters.find("cap_z")==parameters.end())
			missing_keys.push_back("cap_z");
	}
	else if (compare_strings(parameters["cap_type"],"voronoi"))
	{
		if(parameters.find("r_cut")==parameters.end())
			missing_keys.push_back("r_cut");
	}
	else
		opencap_throw("Error: only box and voronoi CAPs supported.");
	if(missing_keys.size()!=0)
	{
		std::string error_str = "Missing CAP keywords: ";
		for (auto key: missing_keys)
			error_str+=key+" ";
		opencap_throw(error_str);
	}
	std::map<std::string, std::string> defaults = {{"radial_precision", "14"}, {"angular_points", "590"}};
	for (const auto &pair:defaults)
	{
		if(parameters.find(pair.first)==parameters.end())
			parameters[pair.first]=pair.second;
	}
}


#include <iostream>
#include <cstdlib>
#include <cmath>
#include <boost/pending/disjoint_sets.hpp>
#include <vector>
#include <queue>
#include <map>
using namespace std;

template <class T>
class AffinityGraphCompare{
	private:
	    const T * mEdgeWeightArray;
	public:
		AffinityGraphCompare(const T * EdgeWeightArray){
			mEdgeWeightArray = EdgeWeightArray;
		}
		bool operator() (const int& ind1, const int& ind2) const {
			return (mEdgeWeightArray[ind1] > mEdgeWeightArray[ind2]);
		}
};

/*
 * Compute the MALIS loss function and its derivative wrt the affinity graph
 * MAXIMUM spanning tree
 * Author: Srini Turaga (sturaga@mit.edu)
 * All rights reserved
 */
void malis_loss_weights_cpp(const int nVert, const int* seg,
               const int nEdge, const int* node1, const int* node2, const float* edgeWeight,
               const int pos,
               int* nPairPerEdge){

    /* Disjoint sets and sparse overlap vectors */
    vector<map<int,int> > overlap(nVert);
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i){
        dsets.make_set(i);
        if (0!=seg[i]) {
            overlap[i].insert(pair<int,int>(seg[i],1));
        }
    }

    /* Sort all the edges in increasing order of weight */
    std::vector< int > pqueue( nEdge );
    int j = 0;
    for ( int i = 0; i < nEdge; i++ ){
        if ((node1[i]>=0) && (node1[i]<nVert) && (node2[i]>=0) && (node2[i]<nVert))
	        pqueue[ j++ ] = i;
    }
    unsigned long nValidEdge = j;
    pqueue.resize(nValidEdge);
    sort( pqueue.begin(), pqueue.end(), AffinityGraphCompare<float>( edgeWeight ) );

    /* Start MST */
    int e;
    int set1, set2;
    int nPair = 0;
    map<int,int>::iterator it1, it2;

    /* Start Kruskal's */
    for (unsigned int i = 0; i < pqueue.size(); ++i ) {
        e = pqueue[i];

        set1 = dsets.find_set(node1[e]);
        set2 = dsets.find_set(node2[e]);

        if (set1!=set2){
            dsets.link(set1, set2);

            /* compute the number of pairs merged by this MST edge */
            for (it1 = overlap[set1].begin();
                    it1 != overlap[set1].end(); ++it1) {
                for (it2 = overlap[set2].begin();
                        it2 != overlap[set2].end(); ++it2) {

                    nPair = it1->second * it2->second;

                    if (pos && (it1->first == it2->first)) {
                        nPairPerEdge[e] += nPair;
                    } else if ((!pos) && (it1->first != it2->first)) {
                        nPairPerEdge[e] += nPair;
                    }
                }
            }

            /* move the pixel bags of the non-representative to the representative */
            if (dsets.find_set(set1) == set2) // make set1 the rep to keep and set2 the rep to empty
                swap(set1,set2);

            it2 = overlap[set2].begin();
            while (it2 != overlap[set2].end()) {
                it1 = overlap[set1].find(it2->first);
                if (it1 == overlap[set1].end()) {
                    overlap[set1].insert(pair<int,int>(it2->first,it2->second));
                } else {
                    it1->second += it2->second;
                }
                overlap[set2].erase(it2++);
            }
        } // end link

    } // end while
}

void connected_components_cpp(const int nVert,
               const int nEdge, const int* node1, const int* node2, const int* edgeWeight,
               int* seg){

    /* Make disjoint sets */
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i)
        dsets.make_set(i);

    /* union */
    for (int i = 0; i < nEdge; ++i )
         // check bounds to make sure the nodes are valid
        if ((edgeWeight[i]!=0) && (node1[i]>=0) && (node1[i]<nVert) && (node2[i]>=0) && (node2[i]<nVert))
            dsets.union_set(node1[i],node2[i]);

    /* find */
    for (int i = 0; i < nVert; ++i)
        seg[i] = dsets.find_set(i);
}

void marker_watershed_cpp(const int nVert, const int* marker,
               const int nEdge, const int* node1, const int* node2, const float* edgeWeight,
               int* seg){

    /* Make disjoint sets */
    vector<int> rank(nVert);
    vector<int> parent(nVert);
    boost::disjoint_sets<int*, int*> dsets(&rank[0],&parent[0]);
    for (int i=0; i<nVert; ++i)
        dsets.make_set(i);

    /* initialize output array and find representatives of each class */
    std::map<int,int> components;
    for (int i=0; i<nVert; ++i){
        seg[i] = marker[i];
        if (seg[i] > 0)
            components[seg[i]] = i;
    }

    // merge vertices labeled with the same marker
    for (int i=0; i<nVert; ++i)
        if (seg[i] > 0)
            dsets.union_set(components[seg[i]],i);

    /* Sort all the edges in decreasing order of weight */
    std::vector<int> pqueue( nEdge );
    int j = 0;
    for (int i = 0; i < nEdge; ++i)
        if ((edgeWeight[i]!=0) &&
            (node1[i]>=0) && (node1[i]<nVert) &&
            (node2[i]>=0) && (node2[i]<nVert) &&
            (marker[node1[i]]>=0) && (marker[node2[i]]>=0))
                pqueue[ j++ ] = i;
    unsigned long nValidEdge = j;
    pqueue.resize(nValidEdge);
    sort( pqueue.begin(), pqueue.end(), AffinityGraphCompare<float>( edgeWeight ) );

    /* Start MST */
    int set1, set2, label_of_set1, label_of_set2;
    for (unsigned int i = 0; i < pqueue.size(); ++i ) {

        set1=dsets.find_set(node1[i]);
        set2=dsets.find_set(node2[i]);
        label_of_set1 = seg[set1];
        label_of_set2 = seg[set2];

        if ((set1!=set2) &&
            ( ((label_of_set1==0) && (marker[set1]==0)) ||
             ((label_of_set2==0) && (marker[set1]==0))) ){

            dsets.link(set1, set2);
            // either label_of_set1 is 0 or label_of_set2 is 0.
            seg[dsets.find_set(set1)] = std::max(label_of_set1,label_of_set2);

        }

    }

    // write out the final coloring
    for (int i=0; i<nVert; i++)
        seg[i] = seg[dsets.find_set(i)];

}