/*
* This file is part of Fieldosophy, a toolkit for random fields.
*
* Copyright (C) 2021 Anders Gunnar Felix Hildeman <fieldosophySPDEC@gmail.com>
*
* This Source Code is subject to the terms of the BSD 3-Clause License.
* If a copy of the license was not distributed with this file, you can obtain one at https://opensource.org/licenses/BSD-3-Clause.
*
*/

#ifndef MESH_HXX
#define MESH_HXX

#include <Eigen/Dense>

#include <set>
#include <vector>
#include <list>

// Forward declaration
class MeshGraph;

// Class representing a mesh initiated from pointers to triangles and nodes
class ConstMesh
{
    public:

        ConstMesh( const double * const pNodes, const unsigned int pD, const unsigned int pNumNodes,
            const unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
            const unsigned int * const pNeighs = NULL ) :
            mNodes(pNodes), mD(pD), mNumNodes(pNumNodes), mSimplices(pSimplices), mNumSimplices(pNumSimplices), mTopD(pTopD), mNeighs(pNeighs) {}

        ConstMesh( const ConstMesh & pMesh )
        {
            mNodes = pMesh.getNodes();
            mD = pMesh.getD();
            mNumNodes = pMesh.getNN();
            mSimplices = pMesh.getSimplices();
            mNumSimplices = pMesh.getNT();
            mTopD = pMesh.getTopD();
            mNeighs = pMesh.mNeighs;
        }

        ConstMesh & operator=(const ConstMesh & pMesh)
        {
            if (&pMesh != this)
                *this = ConstMesh(pMesh);

            return *this;
        }

        ~ConstMesh();

        // Get variables
        inline const double * getNodes() const {return mNodes;}
        inline const unsigned int * getSimplices() const {return mSimplices;}
        inline const unsigned int * getNeighs() const {return mNeighs;}
        inline const unsigned int getNN() const {return mNumNodes;}
        inline const unsigned int getNT() const {return mNumSimplices;}
        inline const unsigned int getD() const {return mD;}
        inline const unsigned int getTopD() const {return mTopD;}
        inline const bool hasNeighs() const {return (mNeighs != NULL);}

        // Get diameter of simplex
        double getDiameter( const unsigned int pSimplexInd ) const;
        // Get a simplex index for a simplex where points are part
        int getASimplexForPoint( const double * const pPoints, const unsigned int pNumPoints,
            unsigned int * const pSimplexIds, double * const pBarycentricCoords,
            const double pEmbTol, const double * const pCenterOfCurvature, const unsigned int pNumCentersOfCurvature) const;
        // Get a set of all simplices for which the given point is a member.
        int getAllSimplicesForPoint( const double * const pPoint, unsigned int pSimplexId,
            std::set<unsigned int> & pOutput,
            const double pEmbTol, const double * const pCenterOfCurvature ) const;
        // Get a simplex index for a simplex where node is a part
        int getASimplexForNode( const unsigned int * const pNodes, const unsigned int pNumNodes, unsigned int * const pSimplexIds) const;
        // Get a simplex index for a simplex where set is a part
        int getASimplexForSet( const std::set<unsigned int> & pSet, unsigned int & pSimplexId) const;
        // Get a set of all simplices for which the given node is a member.
        int getAllSimplicesForNode( const unsigned int pNode, unsigned int pSimplexId,
            std::set<unsigned int> & pOutput ) const
        {
            std::set<unsigned int> lSet;
            lSet.insert(pNode);
            return getAllSimplicesForSet( lSet, pSimplexId, pOutput );
        }
        // Get a set of all simplices for which the given set is a member.
        int getAllSimplicesForSet( const std::set<unsigned int> & pSet, unsigned int pSimplexId,
            std::set<unsigned int> & pOutput ) const;
        // Get coefficients of gradient of linear function on face
        int getGradientChainCoefficientsOfSimplex( double * const pGradientCoefficients, const unsigned int pNumRows, const unsigned int pNumCols,
            const unsigned int pSimplexInd, double * const pArea = NULL ) const;

        // Get standard- and/or barycentric coordinates for points given simplex
        int getCoordinatesGivenSimplex( const double * const pPoints, const unsigned int pNumPoints, const unsigned int pSimplexId,
            double * const pStandardCoords, double * const pBarycentricCoords,
            const double pEmbTol, const double * const pCenterOfCurvature, const unsigned int pNumCentersOfCurvature, double * const pBaryOutsidedness) const;
        // Get a set of all node indices part of a collection of simplices
        std::set<unsigned int> getUniqueNodesOfSimplexCollection( const unsigned int * const pSimplices, const unsigned int pNumSimplices ) const;
        // Populate arrays with corresponding mesh
        int populateArrays( double * const pNodes, const unsigned int pD, const unsigned int pNumNodes,
            unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
            unsigned int * const pNeighs = NULL ) const;
        // Computes a mesh graph of mesh
        int computeMeshGraph( const unsigned int pMaxNumNodes, const double pMinDiam, const unsigned int pMinNumTriangles,
            const double * const pPoints = NULL, const unsigned int * const pNumPoints = NULL );
        // See if two simplices are neighbors
        bool areSimplicesNeighbors( const unsigned int pSimpInd1, const unsigned int pSimpInd2 ) const;
        // See if node is part of simplex
        inline bool isNodePartOfSimplex( const unsigned int pNode, const unsigned int pSimplex ) const
        {
            if (pSimplex > getNT())
                return false;
            for (unsigned int lIter = 0; lIter < (getTopD()+1); lIter++ )
            {
                if ( pNode == getSimplices()[pSimplex * (getTopD()+1) + lIter] )
                    return true;
            }
            return false;
        }

        // See if node is part of simplex
        inline bool isSetPartOfSimplex( const std::set<unsigned int> & pSet, const unsigned int pSimplex ) const
        {
            if (pSimplex > getNT())
                return false;
            unsigned int lNumMatches = 0;
            // Loop through simplex
            for (unsigned int lIter = 0; lIter < (getTopD()+1); lIter++ )
            {
                const unsigned int lCurNode = getSimplices()[pSimplex * (getTopD()+1) + lIter];
                if ( pSet.count(lCurNode) > 0 )
                    ++lNumMatches;
            }
            if (lNumMatches == pSet.size() )
                return true;

            return false;
        }

    protected:

        const double * mNodes;
        unsigned int mD;
        unsigned int mNumNodes;

        const unsigned int * mSimplices;
        unsigned int mNumSimplices;
        unsigned int mTopD;

        const unsigned int * mNeighs;

        // Pointer to storage of mesh graph for node
        MeshGraph * mMeshGraph = NULL;
        // Compute the standard simplex coordinates of chosen point
        Eigen::VectorXd getSimplexStandardCoords( const double * const pPoint, const unsigned int pSimplexId, std::vector< double > & pCurNodeCoords,
            double * const pDivergence = NULL, const double * const pCenterOfCurvature = NULL ) const;

};

// Class representing a full mesh (saving nodes and triangles inside)
class FullMesh : public ConstMesh
{
    public:

        FullMesh( const double * const pNodes, const unsigned int pD, const unsigned int pNumNodes,
                const unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
                const unsigned int * const pNeighs = NULL );

        // Method refining all simplices accordingly
        int refine( const unsigned int pMaxNumNodes, std::vector<double> & pMaxDiam, const unsigned int * const pNumLevels, int (* transformationPtr)(double *, unsigned int) );
        // Method refining a simplex
        int refineSimplex( const unsigned int pChosenSimplex, const double pMaxDiam, const unsigned int pMaxNewSimplices = 1);
        // Overloaded member function for populating arrays from mesh
        int populateArrays( double * const pNodes, const unsigned int pD, const unsigned int pNumNodes,
            unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
            unsigned int * const pNeighs = NULL );

    private:

        // Function for updating ConstMesh pointers when FullMesh is changing
        inline void updateConstMeshPointers()
        {
            // Set pointers to data
            mNodes = (mFullNodes.size() == 0) ? NULL : mFullNodes.data();
            mSimplices = (mFullSimplices.size() == 0) ? NULL : mFullSimplices.data();
            mNeighs = (mFullNeighs.size() == 0) ? NULL : mFullNeighs.data();
            return;
        }

        // Stores simplices
        std::vector< unsigned int > mFullSimplices;
        // Store nodes
        std::vector< double > mFullNodes;
        // Store neigbors
        std::vector< unsigned int > mFullNeighs;

};

// Functor for comparison of sets of node indices to give a weak ordering
class CompareNodeSets
{
    public:
        CompareNodeSets( const unsigned int pD = 0 ) : mD(pD) {}

        bool operator() ( const std::set<unsigned int> pLhs, const std::set<unsigned int> pRhs )
        {
            // Initialize comparison as less than
            bool lLhsLessThanRhs = false;
            // Loop through set
            std::set<unsigned int>::const_iterator lIterRhs = pRhs.begin();
            for ( std::set<unsigned int>::const_iterator lIterLhs = pLhs.begin(); lIterLhs !=  pLhs.end();  )
            {
                // If current
                if ( *lIterLhs < *lIterRhs )
                {
                    // Set that Lhs is smaller than Rhs
                    lLhsLessThanRhs = true;
                    // Stop looping
                    break;
                }
                else
                    if ( *lIterLhs > *lIterRhs )
                        // Stop looping since rhs is apparently smaller
                        break;

                // Advance iterators
                ++lIterLhs;
                ++lIterRhs;
            }
            // Return result
            return lLhsLessThanRhs;
        }
        bool operator() ( const unsigned int * const pLhs, const unsigned int * const pRhs )
        {
            // Initialize comparison as less than
            bool lLhsLessThanRhs = false;
            // Loop through arrays
            for ( unsigned int lIter = 0; lIter < mD; lIter++  )
            {
                // If current
                if ( pLhs[lIter] < pRhs[lIter] )
                {
                    // Set that Lhs is smaller than Rhs
                    lLhsLessThanRhs = true;
                    // Stop looping
                    break;
                }
                else
                    if ( pLhs[lIter] > pRhs[lIter] )
                        // Stop looping since rhs is apparently smaller
                        break;
            }
            // Return result
            return lLhsLessThanRhs;
        }

    private:
        const unsigned int mD;
};

// Class representing boundaries of all simplices in a mesh
class SimplexEdges
{
    public:

        SimplexEdges() {}

        // Compute all simplex boundaries and which simplices they are boundaries of.
        int computeEdges(
            const unsigned int * const pSimplices, const unsigned int pTopD,
            const unsigned int pNumSimplices, const unsigned int pEdgeDim );
        // Populate edges array
        int populateEdges( unsigned int * pEdges, const unsigned int pNumEdges, const unsigned pNumNodes ) const;
        // Populate simplex array associated with edges array
        int populateEdgesSimplexList( unsigned int * const pSimplexList, const unsigned int pNumEdges,
            const unsigned pMaxNumSimplicesPerEdge, const unsigned int pNumSimplices ) const;
        // Populate map of all edges to each simplex
        int populateSimplexEdgesList( unsigned int * const pEdgeList, const unsigned int pNumSimplices,
            const unsigned int pNumEdgesPerSimplex ) const;

        // Acquire number of maximum simplices per edge
        inline const unsigned int & getMaxSimplicesPerEdge() const { return mMaxSimplicesPerEdge; }
        // Get number of edges
        inline unsigned int getNumEdges() const { return mEdges.size(); }
        // Get number of edges for each simplex
        inline const unsigned int & getNumEdgesPerSimplex() const { return mNumEdgesPerSimplex; }

        // Function for finding edge index from edge definition in logarithmic time
        static unsigned int findEdgeIndexGivenEdge( const unsigned int * const pEdge,
            const unsigned int * pEdges, const unsigned int pNumEdges, const unsigned int pEdgeDim );

        // Typedef of the pair of boundary node indices and simplices associated with boundary
        typedef std::pair< std::set<unsigned int>, std::set<unsigned int> > EdgeElement;

    private:

        // Functor for comparison of EdgeElements
        class CompareEdges
        {
            public:
            bool operator() ( const EdgeElement & pLhs, const EdgeElement & pRhs )
            {
                CompareNodeSets lTemp;
                return lTemp(pLhs.first, pRhs.first);
            }
        };

        // Store the maximum number of simplices for any edge
        unsigned int mMaxSimplicesPerEdge = 1;
        // Store the number of nodes in edges
        unsigned int mEdgeDim = 0;
        // Store number of edges per simplex
        unsigned int mNumEdgesPerSimplex = 0;
        // Store simplex boundaries
        std::set< EdgeElement, CompareEdges > mEdges;
};

// Class representing a mapping from original simplices on a manifold (embedded or not) to standard simplex coordinates
class MapToSimp
{
    public:
        // Constructor
        MapToSimp( const double * const pPoints, const unsigned int pD, const unsigned int pTopD );
        // Acquire determinant (of R1 if pTopD < pD)
        double getAbsDeterminant() const {return std::abs(mDeterminant);}
        // Solve
        Eigen::MatrixXd solve( const Eigen::MatrixXd & pVector ) const;
        // Solve transposed
        Eigen::MatrixXd solveTransposed( const Eigen::MatrixXd & pVector ) const;
        // Get length between hyperplane of simplex and vector
        double getOrthogonalLength( const Eigen::VectorXd & pVector ) const;
        // Get parameter value 't' of line parameterized as 'pLinePoint' + t*'pLineVector', for the point where line cuts hyperplane of simplex.
        double getLineIntersection( const Eigen::VectorXd & pLinePoint, const Eigen::VectorXd & pLineVector ) const;
        // Get standard coordinates of vector
        Eigen::MatrixXd getStandardCoord( const Eigen::MatrixXd & pVector ) const {return solve(pVector.colwise() - mPoint0);}
        // right multiplication of F with vector
        Eigen::MatrixXd multiplyWithF( const Eigen::MatrixXd & pVector ) const;
        // Map point from standard simplex space to original space
        Eigen::MatrixXd getOriginalCoord( const Eigen::MatrixXd & pVector ) const { return ( multiplyWithF( pVector ).colwise() + mPoint0 );  }

    private:

        const unsigned int mD;
        const unsigned int mTopD;
        double mDeterminant;
        Eigen::VectorXd mPoint0;
        Eigen::ColPivHouseholderQR< Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > mQR;

};

// Class aiding in extending mesh to new dimension
class ExtendMesh
{

    public:

        ExtendMesh( const std::vector<unsigned int> * const pEdges,
            const std::vector<unsigned int> * const pSimplexIdentity,
            const std::vector<unsigned int> * const pEdgeIdentity,
            const unsigned int * const pSimplices,
            const unsigned int pNumSimplices, const unsigned int pTopD,
            const unsigned int pNumNodes, const unsigned int pNumEdgesSimp ) :
            mEdges(pEdges), mSimplexIdentity(pSimplexIdentity), mEdgeIdentity(pEdgeIdentity),
            mOldSimplices(pSimplices), mNumSimplices(pNumSimplices), mTopD(pTopD), mNumNodes(pNumNodes), mNumEdgesSimp(pNumEdgesSimp)
            {
                // Create vector of which simplices that have been placed
                mPlacedSimplices = std::vector<bool>(mNumSimplices, false);
            }

        // Mark that simplices has been placed covering old simplex
        inline int markPlacement( const unsigned int pSimplexId, const bool pMarker = true )
        {
            if ( pSimplexId > mNumSimplices )
                return 1;
            mPlacedSimplices.at(pSimplexId) = pMarker;
            return 0;
        }
        // Get if simplex is placed
        inline bool isSimplexPlaced( const unsigned int pSimplexId ) const
        {
            if ( pSimplexId > mNumSimplices )
                return true;
            return mPlacedSimplices.at(pSimplexId);
        }
        // get all node index values to choose from for extensions of specific simplex
        inline std::set<unsigned int> allPossibleNodeInds( const unsigned int pSimplexInd ) const
        {
            // Get all nodes possible for sub simplices
            std::set<unsigned int> lPossibleNodeInds;
            for ( unsigned int lIterNodes = 0; lIterNodes < ( mTopD + 1 ); lIterNodes++ )
            {
                lPossibleNodeInds.insert( mOldSimplices[ pSimplexInd * (mTopD+1) + lIterNodes ] );
                lPossibleNodeInds.insert( mOldSimplices[ pSimplexInd * (mTopD+1) + lIterNodes ] + mNumNodes );
            }

            return lPossibleNodeInds;
        }
        // Get all edge indices of current simplex
        inline std::vector<unsigned int> edgesOfOldSimplex( const unsigned int pSimplexInd ) const
        {
            return std::vector< unsigned int >(
                mSimplexIdentity->begin() + pSimplexInd * mNumEdgesSimp,
                mSimplexIdentity->begin() + (pSimplexInd + 1) * mNumEdgesSimp );
        }
        // Is simplex on the border
        bool isSimplexOnBorder( const unsigned int pSimplexId ) const;
        // Get all old simplices sharing an edge with current simplex, as well as which edge are shared between them
        std::vector< std::pair< unsigned int, unsigned int> > neighSimps(
            const unsigned int pSimplexInd, const char pPlaced = 0 ) const;
        // Computes all new edges of old simplex that are fixed due to already placed new simplices
        std::list< std::set<unsigned int> > computeComplyEdgesFromPlaced(
            const unsigned int pSimplexInd, const std::vector<std::pair<unsigned int, unsigned int>> & pNeighs,
            const unsigned int * const pNewSimplices ) const;
        // Computes the simplex when projected onto old dimensionality
        std::set<unsigned int> projectNewOntoOldSimplex( std::set<unsigned int> & pNewSimplex ) const;
        // See if simplex is on edge of prism
        bool onPrismEdge( std::set<unsigned int> & pNewSimplex ) const;

        // Acquire vector of new simplices from an old simplex (taking into account placed neighbors)
        int computeNewSubSimplices(
            const unsigned int pSimplexInd, const unsigned int * const pNewSimplices, std::vector< std::set<unsigned int> > & pOut  ) const;
        // Acquire list of simplices which mark the path from given simplex to closest simplex that is not placed (or is an edge)
        unsigned int shortestPathToFreedom( const unsigned int pSimplexId, const unsigned int pPrevSimplexId, std::list<unsigned int> & pPath ) const;

    protected:

        const std::vector<unsigned int> * const mEdges;
        const std::vector<unsigned int> * const mSimplexIdentity;
        const std::vector<unsigned int> * const mEdgeIdentity;
        std::vector<bool> mPlacedSimplices;
        const unsigned int mTopD;
        const unsigned int mNumNodes;
        const unsigned int mNumSimplices;
        const unsigned int mNumEdgesSimp;
        const unsigned int * const mOldSimplices;

};

// Get edges of simplex
int mesh_getEdgesOfSimplex( std::vector< std::set<unsigned int> > & pOut, const unsigned int pNumCombinations,
    const unsigned int pEdgeDim, const unsigned int pTopologicalD, const unsigned int * const pSimplex );
// Acquire edges and edge identities from simplex mapping
int mesh_getEdgesAndRelations( std::vector<unsigned int> &pEdges, std::vector<unsigned int> &pSimplexIdentity, std::vector<unsigned int> &pEdgeIdentity,
    const unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD, const unsigned int pNumEdgesSimp = 0 );

extern "C"
{

    // Get observation matrix (only works in R^d, no embedded manifolds)
    int mesh_getObservationMatrix( double * const pData, unsigned int * const pRow, unsigned int * const pCol, const unsigned int pNumNonZeros,
        const double * const pPoints, const unsigned int pNumPoints,
        const double * const pNodes, const unsigned int pNumNodes,
        const unsigned int * const pMesh, const unsigned int pNumSimplices,
        const unsigned int pD, const unsigned int pTopD, const double pEmbTol = 0.0d,
        const double * const pCenterOfCurvature = NULL, const unsigned int pNumCentersOfCurvature = 0);

    // Maps triangle values to matrices
    int mesh_getGradientCoefficientMatrix( const unsigned int pNonNulls, double * const pData,
        unsigned int * const pRow, unsigned int * const pCol, unsigned int * const pDataIndex,
        const double * const pNodes, const unsigned int pNumNodes,
        const unsigned int * const pMesh, const unsigned int pNumSimplices,
        const unsigned int pD, const unsigned int pTopD,
        double * const pAreas = NULL );

    // Recurrent investigation of all edges (or sub-edges) [thread safe]
    int mesh_recurrentEdgeFinder( unsigned int * const pEdgeList, const unsigned int pAllocateSpace,
        const unsigned int pD, const unsigned int * const pCurPointConfig, const unsigned int pNumPointConfig );

    // Get edges of simplices
    int mesh_getEdgesOfSimplices( unsigned int * const pEdges, unsigned int * pEdgeIndex, unsigned int * const pSimplexIdentity, const unsigned int pEdgeDim,
        const unsigned int pTopologicalD, const unsigned int * const pSimplex, const unsigned int pNumSimplices );
    // Compute edges of chosen dimensionality and which simplices that are associated to them
    int mesh_computeEdges( const unsigned int pEdgeDim, const unsigned int * const pSimplices,
        const unsigned int pNumSimplices, const unsigned int pTopD,
        unsigned int * const pNumEdges, unsigned int * const pEdgeId, unsigned int * const pMaxSimplicesPerEdge );
    // Populate arrays of edges, associated simplices, and associated edges to each simplex
    int mesh_populateEdges( unsigned int * const pEdges, const unsigned int pEdgeDim, const unsigned int pNumEdges,
        const unsigned int pEdgeId,
        unsigned int * const pSimplicesForEdges, const unsigned int pMaxSimplicesPerEdge, const unsigned int pNumSimplices,
        unsigned int * const pEdgesForSimplices, const unsigned int pNumEdgesPerSimplex );
    // Clear saved edges
    int mesh_clearEdges( const unsigned int pEdgeId );

    // Get neighborhood of each simplex
    int mesh_getSimplexNeighborhood( const unsigned int pNumEdges, const unsigned int pNumSimplices,
        const unsigned int * const pSimplicesForEdges, const unsigned int pMaxSimplicesPerEdge,
        const unsigned int * const pEdgesForSimplices, const unsigned int pNumEdgesPerSimplex,
        unsigned int * const pNeighborhood
      );

    // Refines chosen simplices
    int mesh_refineMesh( const double * const pNodes, const unsigned int pNumNodes, const unsigned int pD,
        const unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
        unsigned int * const pNewNumNodes, unsigned int * const pNewNumSimplices, unsigned int * const pId,
        const unsigned int pMaxNumNodes, const double * const pMaxDiam, const unsigned int pNumMaxDiam,
        const unsigned int * const pNumLevels, int (* transformationPtr)(double *, unsigned int) );

    // Populate new mesh
    int mesh_acquireMesh( const unsigned int pId,
        double * const pNodes, const unsigned int pNumNodes, const unsigned int pD,
        unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD,
        unsigned int * const pNeighs = NULL );

    // Binomial coefficient
    inline unsigned int mesh_nchoosek( unsigned int n, unsigned int k );

    // Extend mesh
    int mesh_extendMesh( unsigned int * const pNewSimplices, const unsigned int pNewNumSimplices,
        const unsigned int * const pSimplices, const unsigned int pNumSimplices, const unsigned int pTopD, const unsigned int pNumNodes );

}

#endif // MESH_HXX


/** \file mean_atomic.hpp \brief Atomic (simple) parametric functions */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _MEAN_ATOMIC_HPP_
#define  _MEAN_ATOMIC_HPP__

#include <boost/numeric/ublas/vector_proxy.hpp>
#include "mean_functors.hpp"

namespace bayesopt
{

  /**\addtogroup ParametricFunctions
   * @{
   */

  /** \brief Abstract class for an atomic kernel */
  class AtomicFunction : public ParametricFunction
  {
  public:
    virtual int init(size_t input_dim)
    {
      n_inputs = input_dim;
      return 0;
    };
    void setParameters(const vectord &theta)
    {
      if(theta.size() != n_params)
	{
	  throw std::invalid_argument("Wrong number of mean function parameters");
	}

      mParameters = theta;
    };
    vectord getParameters() {return mParameters;};
    size_t nParameters() {return n_params;};
    size_t nFeatures() {return n_features;};

    virtual ~AtomicFunction(){};

  protected:
    size_t n_params;
    size_t n_features;
    vectord mParameters;
  };

  /** \brief Constant zero function */
  class ZeroFunction: public AtomicFunction
  {
  public:
    int init(size_t input_dim)
    {
      n_inputs = input_dim;
      n_params = 1;
      n_features = 1;
      return 0;
    };
    double getMean (const vectord& x) { return 0.0; };
    vectord getFeatures(const vectord& x) { return zvectord(1); };
  };

  /** \brief Constant one function */
  class OneFunction: public AtomicFunction
  {
  public:
    int init(size_t input_dim)
    {
      n_inputs = input_dim;
      n_params = 1;
      n_features = 1;
      return 0;
    };
    double getMean (const vectord& x) { return 1.0; };
    vectord getFeatures(const vectord& x) { return svectord(1,1.0); };
  };

  /** \brief Constant function.
      The first parameter indicates the constant value. */
  class ConstantFunction: public AtomicFunction
  {
  public:
    int init(size_t input_dim)
    {
      n_inputs = input_dim;
      n_params = 1;
      n_features = 1;
      return 0;
    };
    double getMean (const vectord& x) { return mParameters(0); };
    vectord getFeatures(const vectord& x) { return svectord(1,1.0); };
  };

  /** \brief Linear combination function.
      Each parameter indicates the coefficient of each dimension. */
  class LinearFunction: public AtomicFunction
  {
  public:
    int init(size_t input_dim)
    {
      n_inputs = input_dim;
      n_params = input_dim;
      n_features = input_dim;
      return 0;
    };
    double getMean (const vectord& x)
    { return boost::numeric::ublas::inner_prod(x,mParameters);  };
    vectord getFeatures(const vectord& x) { return x; };
  };

  /** \brief Linear combination plus constant function.
      The first parameter indicates the constant value. */
  class LinearPlusConstantFunction: public AtomicFunction
  {
  public:
    int init(size_t input_dim)
    {
      n_inputs = input_dim;
      n_params = input_dim + 1;
      n_features = input_dim + 1;
      return 0;
    };
    void setParameters(const vectord& params)
    {
      if(params.size() != n_params)
	{
	  throw std::invalid_argument("Wrong number of mean function parameters");
	}

      mConstParam = params(0);
      mParameters = boost::numeric::ublas::project(params,
						   boost::numeric::ublas::range(1, params.size()));
    };

    double getMean (const vectord& x)
    { return boost::numeric::ublas::inner_prod(x,mParameters) + mConstParam;  };

    vectord getFeatures(const vectord& x)
    {
      using boost::numeric::ublas::range;
      using boost::numeric::ublas::project;
      vectord res(x.size()+1);
      res(0) = 1;
      project(res,range(1,res.size())) = x;
      return res;
    };

  protected:
    double mConstParam;
  };

  //@}

} //namespace bayesopt

#endif


#ifndef ALEPH_CONTAINERS_DIMENSIONALITY_ESTIMATORS_HH__
#define ALEPH_CONTAINERS_DIMENSIONALITY_ESTIMATORS_HH__

#include <aleph/math/KahanSummation.hh>
#include <aleph/math/PrincipalComponentAnalysis.hh>

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

#include <iterator>
#include <limits>
#include <stdexcept>
#include <vector>

#include <cmath>

namespace aleph
{

namespace containers
{

/**
  Estimates local intrinsic dimensionality of a container using its
  nearest neighbours. The underlying assumption of the estimator is
  that points are locally uniformly distributed uniformly. Use this
  estimator with care when analysing unknown data.

  @param container Container to use for dimensionality estimation
  @param k         Number of nearest neighbours
  @param distance  Distance measure

  @returns Vector of local intrinsic dimensionality estimates. Note
           that the numbers are reported *without* rounding.
*/

template <
  class Distance,
  class Container,
  class Wrapper
> std::vector<double> estimateLocalDimensionalityNearestNeighbours( const Container& container,
                                                                    unsigned k,
                                                                    Distance /* distance */ = Distance() )
{
  using IndexType   = typename Wrapper::IndexType;
  using ElementType = typename Wrapper::ElementType;

  std::vector< std::vector<IndexType> > indices;
  std::vector< std::vector<ElementType> > distances;

  Wrapper nnWrapper( container );
  nnWrapper.neighbourSearch( k+1, indices, distances );

  auto n = container.size();

  std::vector<double> estimates;
  estimates.reserve( n );

  for( decltype(n) i = 0; i < n; i++ )
  {
    auto&& nnDistances = distances.at(i);
    auto r1            = aleph::math::accumulate_kahan( nnDistances.begin(), nnDistances.begin() + k,     0.0 ) / static_cast<double>(k  );
    auto r2            = aleph::math::accumulate_kahan( nnDistances.begin(), nnDistances.begin() + k + 1, 0.0 ) / static_cast<double>(k+1);

    estimates.push_back( r1 / ( (r2-r1)*k ) );
  }

  return estimates;
}

/**
  Estimates local intrinsic dimensionality of a container using its
  nearest neighbours. No assumptions about the distribution of data
  points are made. The function uses an iteration over a *range* of
  nearest neighbours and solves a regression problem.

  Please see the publication

    > An evaluation of intrinsic dimensionality estimators\n
    > Peter J. Verveer and Robert P. W. Duin\n
    > IEEE Transactions on Pattern Analysis and Machine Intelligence 17.1, pp. 81-86, 1985

  for more details.

  @param container Container to use for dimensionality estimation

  @param kMin      Minimum number of nearest neighbours to use in
                   computing local dimensionality estimates.

  @param kMax      Maximum number of nearest neighbours to use in
                   computing local dimensionality estimates. This
                   parameter influences performance.

  @param distance  Distance measure

  @returns Vector of local intrinsic dimensionality estimates. Note that
           the numbers are reported *without* rounding.
*/

template <
  class Distance,
  class Container,
  class Wrapper
> std::vector<double> estimateLocalDimensionalityNearestNeighbours( const Container& container,
                                                                    unsigned kMin,
                                                                    unsigned kMax,
                                                                    Distance /* distance */ = Distance() )

{
  if( kMin > kMax )
    std::swap( kMin, kMax );

  if( kMax == 0 || kMin == 0 )
    throw std::runtime_error( "Expecting non-zero number of nearest neighbours" );

  using IndexType   = typename Wrapper::IndexType;
  using ElementType = typename Wrapper::ElementType;

  std::vector< std::vector<IndexType> > indices;
  std::vector< std::vector<ElementType> > distances;

  Wrapper nnWrapper( container );
  nnWrapper.neighbourSearch( kMax, indices, distances );

  auto n = container.size();

  std::vector<double> estimates;
  estimates.reserve( n );

  for( decltype(n) i = 0; i < n; i++ )
  {
    auto&& nnDistances = distances.at(i);

    std::vector<double> localEstimates;
    localEstimates.reserve( kMax );

    for( unsigned k = kMin; k < kMax; k++ )
    {
      auto r = aleph::math::accumulate_kahan( nnDistances.begin(), nnDistances.begin() + k, 0.0 ) / static_cast<double>(k);
      localEstimates.emplace_back( r );
    }

    // The dimensionality estimates consist of two terms. The first term
    // is similar to the local biased dimensionality estimate.

    std::vector<double> firstTerms;
    firstTerms.reserve( kMax );

    std::vector<double> secondTerms;
    secondTerms.reserve( kMax );

    for( unsigned k = kMin; k < kMax - 1; k++ )
    {
      auto index = k - kMin;
      auto r1    = localEstimates.at(index);
      auto r2    = localEstimates.at(index+1);

      firstTerms.emplace_back ( ( (r2-r1) * r1 ) / k );
      secondTerms.emplace_back( ( (r2-r1) * (r2-r1) ) );
    }

    auto s = aleph::math::accumulate_kahan( firstTerms.begin() , firstTerms.end() , 0.0 );
    auto t = aleph::math::accumulate_kahan( secondTerms.begin(), secondTerms.end(), 0.0 );

    estimates.push_back( s / t );
  }

  return estimates;

}

/**
  Estimates local intrinsic dimensionality of a container using its
  nearest neighbours. No assumptions about the distribution of data
  points are made. The function uses *maximum likelihood estimates*
  for the dimensionality estimates.

  Please see the publication

    > Maximum Likelihood Estimation of Intrinsic Dimension\n
    > Elizaveta Levina and Peter J. Bickel\n
    > Advances in Neural Information Processing Systems, 2005

  for more details.

  @param container Container to use for dimensionality estimation

  @param kMin      Minimum number of nearest neighbours to use in
                   computing local dimensionality estimates.

  @param kMax      Maximum number of nearest neighbours to use in
                   computing local dimensionality estimates. This
                   parameter influences performance.

  @param distance  Distance measure

  @returns Vector of local intrinsic dimensionality estimates. Note that
           the numbers are reported *without* rounding.
*/

template <
  class Distance,
  class Container,
  class Wrapper
> std::vector<double> estimateLocalDimensionalityNearestNeighboursMLE( const Container& container,
                                                                       unsigned kMin,
                                                                       unsigned kMax,
                                                                       Distance /* distance */ = Distance() )
{
  if( kMin > kMax )
    std::swap( kMin, kMax );

  if( kMax == 0 || kMin == 0 )
    throw std::runtime_error( "Expecting non-zero number of nearest neighbours" );

  using IndexType   = typename Wrapper::IndexType;
  using ElementType = typename Wrapper::ElementType;

  std::vector< std::vector<IndexType> > indices;
  std::vector< std::vector<ElementType> > distances;

  Wrapper nnWrapper( container );
  nnWrapper.neighbourSearch( kMax, indices, distances );

  auto n = container.size();

  std::vector<double> estimates;
  estimates.reserve( n );

  for( decltype(n) i = 0; i < n; i++ )
  {
    auto&& nnDistances = distances.at(i);

    std::vector<double> localEstimates;
    localEstimates.reserve( kMax );

    // This follows the notation in the original paper. I dislike using
    // $T_k$ to denote distances, though.
    for( unsigned k = kMin - 1; k < kMax; k++ )
    {
      // Nothing to do here...
      if( k == 0 )
        continue;

      std::vector<double> logEstimates;
      logEstimates.reserve( k-1 );

      for( auto it = nnDistances.begin(); it != nnDistances.begin() + k; ++it )
      {
        if( *it > 0.0 && nnDistances.at(k) > 0.0 )
          logEstimates.push_back( std::log( nnDistances.at(k) / *it ) );

        // This defines log(0) = 0, as usually done in information
        // theory. The original paper does not handle this.
        else
          logEstimates.push_back( 0.0 );
      }

      auto mk = k > 1 ? 1.0 / (k-1) * aleph::math::accumulate_kahan( logEstimates.begin(), logEstimates.end(), 0.0 )
                      : 0.0;

      if( mk > 0.0 )
        mk = 1.0 / mk;
      else
        mk = 0.0;

      localEstimates.push_back( mk );
    }

    estimates.push_back( aleph::math::accumulate_kahan( localEstimates.begin(), localEstimates.end(), 0.0 ) / (kMax - kMin + 1) );
  }

  return estimates;
}

/**
  Estimates local intrinsic dimensionality of a container using its
  minimum spanning tree.

  @param container Container to use for dimensionality estimation
  @param distance  Distance measure

  @returns Vector of local intrinsic dimensionality estimates. Note
           that the numbers are reported *without* rounding.
*/

template <
  class Distance,
  class Container
> std::vector<double> estimateLocalDimensionalityNearestNeighbours( const Container& container,
                                                                    Distance distance = Distance() )
{
  using WeightedGraph
    = boost::adjacency_list<
        boost::vecS,
        boost::vecS,
        boost::undirectedS,
        boost::no_property,
        boost::property<boost::edge_weight_t, double> >;

  using VertexDescriptor = boost::graph_traits<WeightedGraph>::vertex_descriptor;
  using EdgeDescriptor   = boost::graph_traits<WeightedGraph>::edge_descriptor;

  auto n = container.size();
  auto d = container.dimension();

  WeightedGraph G( n );

  for( std::size_t i = 0; i < n; i++ )
  {
    auto&& p = container[i];

    for( std::size_t j = i+1; j < n; j++ )
    {
      auto&& q  = container[j];
      auto dist = distance( p.begin(), q.begin(), d );

      boost::add_edge( VertexDescriptor(i),
                       VertexDescriptor(j),
                       dist,
                       G );
    }
  }

  std::vector<EdgeDescriptor> mstEdges;
  boost::kruskal_minimum_spanning_tree( G,
                                        std::back_inserter( mstEdges ) );

  WeightedGraph MST( n );

  for( auto&& edge : mstEdges )
  {
    boost::add_edge( boost::source( edge, G ),
                     boost::target( edge, G ),
                     MST );
  }

  for( auto pair = boost::vertices( MST ); pair.first != pair.second; ++pair.first )
  {
  }

  return {};
}

/**
  Estimates local intrinsic dimensionality of a container using its
  local principal components. The basic premise is that the largest
  spectral gap in the eigenspectrum of a local PCA gives a suitable
  hint about the local dimensionality at the given set of points.

  @param container Container to use for dimensionality estimation
  @param distance  Distance measure

  @returns Vector of local intrinsic dimensionality estimates. The numbers are
           not rounded but taken directly from the estimation procedure.
*/

template <
  class Distance,
  class Container,
  class Wrapper
> std::vector<unsigned> estimateLocalDimensionalityPCA( const Container& container,
                                                        unsigned k,
                                                        Distance /* distance */ = Distance() )
{
  using IndexType   = typename Wrapper::IndexType;
  using ElementType = typename Wrapper::ElementType;

  std::vector< std::vector<IndexType> > indices;
  std::vector< std::vector<ElementType> > distances;

  Wrapper nnWrapper( container );
  nnWrapper.neighbourSearch( k+1, indices, distances );

  std::vector<unsigned> estimates;
  estimates.reserve( container.size() );

  for( auto&& localIndices : indices )
  {
    std::vector< std::vector<ElementType> > data( localIndices.size(), std::vector<ElementType>() );

    {
      std::size_t i = 0;
      for( auto&& index : localIndices )
      {
        auto&& p = container[index];

        data[i].assign( p.begin(), p.end() );

        i++;
      }
    }

    // Calculate a (local) principal component analysis and analyse the
    // resulting spectrum. The largest spectral gap is used to estimate
    // the local intrinsic dimensionality.
    aleph::math::PrincipalComponentAnalysis pca;
    auto result           = pca( data );
    auto&& singularValues = result.singularValues;

    if( singularValues.size() >= 2 )
    {
      ElementType spectralGap = std::numeric_limits<ElementType>::lowest();
      unsigned spectralIndex  = 0;

      auto prev = singularValues.begin();
      auto curr = std::next( prev );

      for( ; curr != singularValues.end(); ++prev, ++curr)
      {
        auto gap = std::abs( *prev - *curr );

        if( gap > spectralGap )
        {
          // Notice that I am using the *lower* bound of the index here
          // by specifying `prev` instead of current. This makes sense,
          // as a jumping between $i-1$ and $i$ indicates that $i-1$ is
          // sufficient to describe the data adequately.
          //
          // The additional offset of 1 is used because `std::distance`
          // uses zero-based indices.
          spectralGap   = gap;
          spectralIndex = static_cast<unsigned>( std::distance( singularValues.begin(), prev ) ) + 1;
        }
      }

      estimates.push_back( spectralIndex );
    }
  }

  return estimates;
}

} // namespace containers

} // namespace aleph

#endif


#include <limits>
#include <vector>
#include <unordered_map>

#include "controllers/sfm_aligner.h"
#include "estimators/similarity_transform.h"
#include "estimators/ransac_similarity.h"
#include "estimators/sim3.h"
#include "optim/bundle_adjustment.h"
#include "util/timer.h"
#include "math/util.h"

#include <glog/logging.h>
#include <Eigen/Geometry>
#include <ceres/rotation.h>

namespace GraphSfM {

double MeanReprojectionResiduals(const std::vector<double>& residuals)
{
    double mean_residual = 0.0;
    for (auto residual : residuals) {
        mean_residual += residual;
    }
    return mean_residual / residuals.size();
}

std::vector<double> ComputeReprojectionResiduals(const std::vector<Eigen::Vector3d>& src_points,
                                    const std::vector<Eigen::Vector3d>& ref_points,
                                    const Eigen::Matrix3x4d& alignment)
{
    std::vector<double> residuals;
    for (uint i = 0; i < src_points.size(); i++) {
        const Eigen::Vector3d& tsrc_point = alignment * src_points[i].homogeneous();
        const Eigen::Vector3d& ref_point = ref_points[i];

        double residual = (tsrc_point - ref_point).norm();
        residuals.push_back(residual);
    }
    return residuals;
}

SfMAligner::SfMAligner(const std::vector<Reconstruction*>& reconstructions,
                       const BundleAdjustmentOptions& ba_options)
     : reconstructions_(reconstructions),
       ba_options_(ba_options)
{
    // some logic or parameters check
    CHECK_GT(reconstructions.size(), 0);
    CHECK_GT(reconstructions_.size(), 0);

    for (uint i = 0; i < reconstructions_.size(); i++) {
        LOG(INFO) << "Node id: " << i;
        CHECK_NOTNULL(reconstructions_[i]);
        LOG(INFO) << "Total images number: " << reconstructions_[i]->NumImages();
    }
}

bool SfMAligner::Align()
{
    // 1. Constructing a graph from reconstructions,
    // each node is a reconstruction, edges represents the connections between
    // reconstructions (by the means of common images or common 3D points), the weight
    // of edge represents the mean reprojection error.
    LOG(INFO) << "Constructing Reconstructions Graph...";
    ConstructReconsGraph();
    recons_graph_.ShowInfo();
    CHECK_EQ(recons_graph_.GetNodesNum(), reconstructions_.size());

    // The reconstruction graph should be at least a spanning tree,
    // or we couldn't stitch all reconstructions together due to
    // too large alignment error or disconnected components.
    if (recons_graph_.GetEdgesNum() < recons_graph_.GetNodesNum() - 1) {
        LOG(ERROR) << "Can't align all reconstructions together due to "
                   << "too large alignment error or disconnected components";
        return false;
    }

    // 2. Constructing a minimum spanning tree, thus we can select the
    // most accurate n - 1 edges for accurate alignment.
    LOG(INFO) << "Finding Minimum Spanning Tree...";
    std::vector<Edge> mst_edges = recons_graph_.Kruskal();
    if (mst_edges.size() < recons_graph_.GetNodesNum() - 1) {
        return false;
    }

    Graph<Node, Edge> mst;
    for (auto node : recons_graph_.GetNodes()) {
        mst.AddNode(node.second);
    }
    for (const auto edge : mst_edges) {
        mst.AddEdge(edge);
    }
    mst.ShowInfo();

    // 3. Finding an anchor node, an anchor node is a reference reconstruction
    // that all other reconstructions should be aligned to.
    LOG(INFO) << "Finding Anchor Node...";
    FindAnchorNode(&mst);

    // 4. Compute the final transformation to anchor node for each cluster
    LOG(INFO) << "Computing Final Similarity Transformations...";
    for (uint i = 0; i < reconstructions_.size(); i++) {
        if (i != anchor_node_.id) {
            this->ComputePath(i, anchor_node_.id);
        }
    }
    sim3_to_anchor_[anchor_node_.id] = Sim3();

    // 5. Merging all other reconstructions to anchor node
    LOG(INFO) << "Merging Reconstructions...";
    this->MergeReconstructions();

    // 6. Final Bundle Adjustment
    LOG(INFO) << "Final Global Bundle Adjustment";
    this->AdjustGlobalBundle();

    return true;
}

Node SfMAligner::GetAnchorNode() const
{
    return anchor_node_;
}

std::vector<Sim3> SfMAligner::GetSim3ToAnchor() const
{
    return sim3_to_anchor_;
}

void SfMAligner::ConstructReconsGraph()
{
    // 1. Add nodes
    for (size_t i = 0; i < reconstructions_.size(); i++) {
        Node node(i);
        // node.recon = reconstructions_[i];
        recons_graph_.AddNode(node);
    }

    // 2. Add edges
    for (uint i = 0; i < reconstructions_.size(); i++) {
        for (uint j = i + 1; j < reconstructions_.size(); j++) {
            const double weight = ComputeEdgeWeight(i, j);
            LOG(INFO) << "weight: " << weight;
            if (weight != std::numeric_limits<double>::max()) {
                recons_graph_.AddEdge(Edge(i, j, (float)weight));
            }
        }
    }
}

double SfMAligner::ComputeEdgeWeight(const uint i, const uint j)
{
    const Reconstruction& recon1 = *reconstructions_[i];
    const Reconstruction& recon2 = *reconstructions_[j];
    double weight = std::numeric_limits<double>::max();

    Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity(3, 3),
                    R2 = Eigen::Matrix3d::Identity(3, 3);
    Eigen::Vector3d t1 = Eigen::Vector3d::Zero(),
                    t2 = Eigen::Vector3d::Zero();
    double s1 = 1.0, s2 = 1.0;

    // Find common registered images
    std::vector<image_t> common_reg_images = recon1.FindCommonRegImageIds(recon2);
    std::vector<Eigen::Vector3d> src_points, ref_points;
    std::vector<Eigen::Matrix3d> src_rotations, ref_rotations;
    for (const auto common_id : common_reg_images) {
        src_points.push_back(recon1.Image(common_id).ProjectionCenter());
        src_rotations.push_back(recon1.Image(common_id).RotationMatrix());
        ref_points.push_back(recon2.Image(common_id).ProjectionCenter());
        ref_rotations.push_back(recon2.Image(common_id).RotationMatrix());
    }
    LOG(INFO) << "Common registerd images number: " << common_reg_images.size();

    if (common_reg_images.size() < 2) {
        LOG(WARNING) << "Not found enough common registered images.";
        return std::numeric_limits<double>::max();
    } else if (common_reg_images.size() < 5) {
        ComputeSimilarityByCameraMotions(src_points, ref_points,
                                         src_rotations, ref_rotations, R1, t1, s1);
        ComputeSimilarityByCameraMotions(ref_points, src_points,
                                         ref_rotations, src_rotations, R2, t2, s2);

        double msd1 = CheckReprojError(src_points, ref_points, s1, R1, t1),
               msd2 = CheckReprojError(ref_points, src_points, s2, R2, t2);

        // angular residual should be considered
        const double angular_residual1 =
            CheckAngularResidual(src_rotations, ref_rotations, R1);
        const double angular_residual2 =
            CheckAngularResidual(ref_rotations, src_rotations, R2);

        weight = std::max(std::max(msd1, msd2),
                          std::max(angular_residual1, angular_residual2));

        if (weight != numeric_limits<double>::max()) {
            sim3_graph_[i][j] = Sim3(R1, t1, s1);
            sim3_graph_[j][i] = Sim3(R2, t2, s2);
        }
    } else {
        double msd1 = 0.0, msd2 = 0.0;
        FindSimilarityTransform(src_points, ref_points, R1, t1, s1, msd1);
        FindSimilarityTransform(ref_points, src_points, R2, t2, s2, msd2);

        // angular residual should be considered
        const double angular_residual1 =
            CheckAngularResidual(src_rotations, ref_rotations, R1);
        const double angular_residual2 =
            CheckAngularResidual(ref_rotations, src_rotations, R2);

        weight = std::max(std::max(msd1, msd2),
                          std::max(angular_residual1, angular_residual2));

        if (weight != numeric_limits<double>::max()) {
            sim3_graph_[i][j] = Sim3(R1, t1, s1);
            sim3_graph_[j][i] = Sim3(R2, t2, s2);
        }
    }

    return weight;
}

void SfMAligner::FindAnchorNode(Graph<Node, Edge>* graph)
{
    paths_.resize(recons_graph_.GetNodesNum());
    sim3_to_anchor_.resize(recons_graph_.GetNodesNum());

    // The anchor is found by merging all leaf nodes to their adjacent nodes,
    // until one node or two nodes left. If two nodes left, we choose the reconstruction
    // that has the largest size as the anchor.
    int layer = 1;
    uint anchor_index = 0;

    while (graph->GetNodesNum() > 1) {
        LOG(INFO) << "Merging the " << layer++ << "-th layer leaf nodes";

        graph->CountOutDegrees();
        graph->CountInDegrees();
        graph->CountDegrees();
        std::unordered_map<size_t, size_t> degrees = graph->GetDegrees();

        // Finding all leaf nodes. Leaf node in graph has degree equals to 1.
        std::vector<int> indexes;
        if (graph->GetNodesNum() == 2) {
            indexes.push_back(degrees.begin()->first);
        } else {
            for (auto it = degrees.begin(); it != degrees.end(); ++it) {
                LOG(INFO) << "node: " << it->first << ", "
                          << "degree: " << it->second;
                if (it->second == 1) indexes.push_back(it->first);
            }
        }
        if (indexes.empty()) break;

        for (auto idx : indexes) {
            if (idx == -1) break;
            const Edge& edge = graph->FindConnectedEdge(idx);

            LOG(INFO) << "Find node [degree = 1]: " << idx;
            LOG(INFO) << edge.src << "->" << edge.dst << ": " << edge.weight;

            // src is the node with degree = 1
            uint src = (idx == edge.src) ? edge.src : edge.dst;
            uint dst = (idx == edge.src) ? edge.dst : edge.src;
            Reconstruction* src_recon = reconstructions_[src];
            Reconstruction* dst_recon = reconstructions_[dst];

            LOG(INFO) << "Merge Clusters: " << src << "->" << dst << ": " << edge.weight;
            anchor_index = dst;
            const Sim3 sim = sim3_graph_[src][dst];
            paths_[src].insert(std::make_pair(dst, sim));

            graph->DeleteNode(src);
            graph->DeleteEdge(src, dst);
            graph->DeleteEdge(dst, src);
            graph->ShowInfo();
        }
    }

    anchor_node_.id = anchor_index;
}

void SfMAligner::ComputePath(int src, int dst)
{
    LOG(INFO) << "Computing Path: " << src << "->" << dst;
    std::queue<int> qu;
    qu.push(src);

    Eigen::Matrix3d r = Eigen::Matrix3d::Identity();
    Eigen::Vector3d t = Eigen::Vector3d::Zero();
    double s = 1.0;

    Sim3 sim(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Identity(), 1.0);
    LOG(INFO) << "v: " << src;
    while (!qu.empty()) {
        int u = qu.front(); qu.pop();
        auto it = paths_[u].begin();
        int v = it->first;
        LOG(INFO) << "v: " << v;
        s = it->second.s * s;
        r = it->second.R * r.eval();
        t = it->second.s * it->second.R * t.eval() + it->second.t;
        if (v == dst) {
            sim.s = s; sim.R = r; sim.t = t;
            sim3_to_anchor_[src] = sim;
            return;
        }
        else qu.push(v);
    }
    LOG(INFO) << "\n";
}

void SfMAligner::MergeReconstructions()
{
    for (uint i = 0; i < reconstructions_.size(); i++) {
        if (i == anchor_node_.id) { continue; }

        Sim3 sim3 = sim3_to_anchor_[i];
        Eigen::Matrix3x4d alignment;
        alignment.block(0, 0, 3, 3) = sim3.s * sim3.R;
        alignment.block(0, 3, 3, 1) = sim3.t;

        reconstructions_[anchor_node_.id]->Merge(*reconstructions_[i],
                                                 alignment);
    }
}

bool SfMAligner::AdjustGlobalBundle()
{
    Reconstruction* final_recon = reconstructions_[anchor_node_.id];
    CHECK_NOTNULL(final_recon);

    const std::vector<image_t>& reg_image_ids = final_recon->RegImageIds();

    CHECK_GE(reg_image_ids.size(), 2) << "At least two images must be "
                                         "registered for global bundle-adjustment";

    // Avoid degeneracies in bundle adjustment.
    final_recon->FilterObservationsWithNegativeDepth();

    // Configure bundle adjustment
    BundleAdjustmentConfig ba_config;
    for (const image_t image_id : reg_image_ids) {
        ba_config.AddImage(image_id);
    }

    // Fix 7-DOFs of the bundle adjustment problem.
    ba_config.SetConstantPose(reg_image_ids[0]);
    ba_config.SetConstantTvec(reg_image_ids[1], {0});

    // Run bundle adjustment.
    BundleAdjuster bundle_adjuster(ba_options_, ba_config);
    if (!bundle_adjuster.Solve(final_recon)) {
        return false;
    }

    // Normalize scene for numerical stability and
    // to avoid large scale changes in viewer.
    final_recon->Normalize();

    return true;
}

void FindSimilarityTransform(const std::vector<Eigen::Vector3d>& observations1,
                             const std::vector<Eigen::Vector3d>& observations2,
                             Eigen::Matrix3d& R,
                             Eigen::Vector3d& t,
                             double& scale,
                             double& msd)
{
    std::vector<Eigen::Vector3d> inliers1, inliers2;
    double threshold = 0.1; // TODO: (chenyu) make this parameter an user option
    double p = 0.99;

    if (observations1.size() > 5) {
        LOG(INFO) << "Finding Similarity by RANSAC";
        RansacSimilarity(observations1, observations2, inliers1, inliers2, R, t, scale, threshold, p);
        VLOG(2) << "inliers size: " << inliers1.size();
        // Re-compute similarity by inliers
        Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, inliers1.size()),
                        x2 = Eigen::MatrixXd::Zero(3, inliers2.size());
        for(int i = 0; i < inliers1.size(); i++) {
            x1.col(i) = inliers1[i];
            x2.col(i) = inliers2[i];
        }
        GraphSfM::FindRTS(x1, x2, &scale, &t, &R);
        // Optional non-linear refinement of the found parameters
        GraphSfM::Refine_RTS(x1, x2, &scale, &t, &R);

        if (inliers1.size() < 4) { msd = numeric_limits<double>::max(); return; }
        // else msd = CheckReprojError(inliers1, inliers2, scale, R, t);
    }

    if (observations1.size() <= 5 || inliers1.size() <= 5) {
        Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, observations1.size()),
                        x2 = Eigen::MatrixXd::Zero(3, observations2.size());
        for (int i = 0; i < observations1.size(); i++) {
            x1.col(i) = observations1[i];
            x2.col(i) = observations2[i];
        }
        GraphSfM::FindRTS(x1, x2, &scale, &t, &R);
        GraphSfM::Refine_RTS(x1, x2, &scale, &t, &R);

        // msd = CheckReprojError(observations1, observations2, scale, R, t);
    }

    msd = CheckReprojError(observations1, observations2, scale, R, t);

    // LOG(INFO) << "scale: " << scale;
    // LOG(INFO) << "rotation: ";
    // LOG(INFO) << R(0, 0) << " " << R(0, 1) << " " << R(0, 2);
    // LOG(INFO) << R(1, 0) << " " << R(1, 1) << " " << R(1, 2);
    // LOG(INFO) << R(2, 0) << " " << R(2, 1) << " " << R(2, 2);
    // LOG(INFO) << "translation: " << t[0] << ", " << t[1] << ", " << t[2];
}

double CheckReprojError(const vector<Eigen::Vector3d>& src_observations,
                        const vector<Eigen::Vector3d>& dst_observations,
                        const double& scale,
                        const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& t)
{
    double reproj_err = 0.0;
    const int size = src_observations.size();
    for (int i = 0; i < size; i++) {
        Eigen::Vector3d reproj_obv = scale * R * src_observations[i] + t;
        reproj_err += (reproj_obv - dst_observations[i]).norm();
    }

    LOG(INFO) << "Mean Reprojection Error: " << reproj_err / size
              << " (" << reproj_err << "/"
              << size << ")";
    return reproj_err / size;
}

double CheckAngularResidual(const std::vector<Eigen::Matrix3d>& src_rotations,
                           const std::vector<Eigen::Matrix3d>& dst_rotations,
                           const Eigen::Matrix3d& R)
{
    double angular_residual = 0.0;
    const int n = src_rotations.size();
    for (uint i = 0; i < n; i++) {
        Eigen::Matrix3d rel_rotation =
            dst_rotations[i].transpose() * src_rotations[i] * R.transpose();
        Eigen::Vector3d angle_axis;
        ceres::RotationMatrixToAngleAxis(rel_rotation.data(), angle_axis.data());
        angular_residual += RadToDeg(angle_axis.norm());
    }

    angular_residual /= n;
    LOG(INFO) << "Average Angular Residual: " << angular_residual << " degree.";
    return angular_residual;
}

bool ComputeSimilarityByCameraMotions(
    std::vector<Eigen::Vector3d>& camera_centers1,
    std::vector<Eigen::Vector3d>& camera_centers2,
    std::vector<Eigen::Matrix3d>& camera_rotations1,
    std::vector<Eigen::Matrix3d>& camera_rotations2,
    Eigen::Matrix3d& relative_r,
    Eigen::Vector3d& relative_t,
    double& scale)
{
    // my hybrid approach by combining "Divide and Conquer: Efficient Large-Scale
    // Structure from Motion Using Graph Partitioning" and RANSAC

    const int n = camera_centers1.size();
    std::vector<Eigen::Vector3d> ts1(n);
    std::vector<Eigen::Vector3d> ts2(n);

    for (uint i = 0; i < n; i++) {
        ts1[i] = -camera_rotations1[i] * camera_centers1[i];
        ts2[i] = -camera_rotations2[i] * camera_centers2[i];
    }

    // compute relative scale from a->b
    std::vector<double> scales;
    for (int i = 0; i < n; i++) {
        Eigen::Vector3d center_a1 = camera_centers1[i];
        Eigen::Vector3d center_b1 = camera_centers2[i];
        for (int j = i + 1; j < n; j++) {
            Eigen::Vector3d center_a2 = camera_centers1[j];
            Eigen::Vector3d center_b2 = camera_centers2[j];
            double scale_ab = (center_b1 - center_b2).norm() /
                             (center_a1 - center_a2).norm();
            scales.push_back(scale_ab);
        }
    }
    // retrieve the median of scales, according to
    // the equation (5) of the paper "Divide and Conquer: Efficient Large-Scale
    // Structure from Motion Using Graph Partitioning"
    std::sort(scales.begin(), scales.end());
    scale = scales[scales.size() / 2];

    // compute relative rotation & relative translation from a->b
    std::vector<Correspondence3D> corres3d;
    std::vector<CorrespondenceEuc> input_datas;
    for (int i = 0; i < camera_centers1.size() ;i++) {
        corres3d.emplace_back(camera_centers1[i], camera_centers2[i]);
        input_datas.push_back(make_pair(Euclidean3D(camera_rotations1[i], ts1[i]),
                                        Euclidean3D(camera_rotations2[i], ts2[i])));
    }
    EuclideanEstimator euc_estimator(scale, corres3d);

    Euclidean3D euc3d;
    RansacParameters params;
    params.rng = std::make_shared<RandomNumberGenerator>((unsigned int)time(NULL));
    params.error_thresh = 0.002;
    params.max_iterations = 1000;

    Prosac<EuclideanEstimator> prosac_euc3(params, euc_estimator);
    prosac_euc3.Initialize();
    RansacSummary summary;
    prosac_euc3.Estimate(input_datas, &euc3d, &summary);

    relative_r = euc3d.R;
    relative_t = euc3d.t;

    // LOG(INFO) << "scale: " << scale;
    // LOG(INFO) << "rotation: \n";
    // LOG(INFO) << relative_r(0, 0) << " " << relative_r(0, 1) << " " << relative_r(0, 2);
    // LOG(INFO) << relative_r(1, 0) << " " << relative_r(1, 1) << " " << relative_r(1, 2);
    // LOG(INFO) << relative_r(2, 0) << " " << relative_r(2, 1) << " " << relative_r(2, 2);
    // LOG(INFO) << "translation: "
            //   << relative_t[0] << ", " << relative_t[1] << ", " << relative_t[2];

    return true;
}

} // namespace GraphSfM


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/algorithms/detail/thomas_inverse.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class thomas
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline thomas()
        : m_spheroid()
    {}

    explicit inline thomas(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::thomas_inverse
                <
                    typename calculation_type<Point1, Point2>::type,
                    true, false
                >::apply(get_as_radian<0>(point1),
                         get_as_radian<1>(point1),
                         get_as_radian<0>(point2),
                         get_as_radian<1>(point2),
                         m_spheroid).distance;
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<thomas<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<thomas<Spheroid, CalculationType>, P1, P2>
    : thomas<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<thomas<Spheroid, CalculationType> >
{
    typedef thomas<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<thomas<Spheroid, CalculationType> >
{
    static inline thomas<Spheroid, CalculationType> apply(thomas<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<thomas<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<thomas<Spheroid, CalculationType>, P1, P2>::type
        apply(thomas<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP


//Source: The LLVM Compiler Infrastructure - lib/addsf3.c
//Modified to truncate result of addition

#include <limits.h>
#include <boost/cstdint.hpp>
#include "FpAddTruncate.h"
#include "BitManip.h"

typedef uint32 rep_t;
typedef int32 srep_t;
typedef float fp_t;
#define REP_C UINT32_C
#define significandBits 23

#define typeWidth       (sizeof(rep_t)*CHAR_BIT)
#define exponentBits    (typeWidth - significandBits - 1)
#define maxExponent     ((1 << exponentBits) - 1)
#define exponentBias    (maxExponent >> 1)

#define implicitBit     (REP_C(1) << significandBits)
#define significandMask (implicitBit - 1U)
#define signBit         (REP_C(1) << (significandBits + exponentBits))
#define absMask         (signBit - 1U)
#define exponentMask    (absMask ^ significandMask)
#define oneRep          ((rep_t)exponentBias << significandBits)
#define infRep          exponentMask
#define quietBit        (implicitBit >> 1)
#define qnanRep         (exponentMask | quietBit)

static inline int rep_clz(rep_t a) {
	return __builtin_clz(a);
}

uint32 FpAddTruncate(uint32 a, uint32 b)
{
    const rep_t aAbs = a & absMask;
    const rep_t bAbs = b & absMask;

    // Detect if a or b is zero, infinity, or NaN.
    if (aAbs - 1U >= infRep - 1U || bAbs - 1U >= infRep - 1U) {

        // NaN + anything = qNaN
        if (aAbs > infRep) return (a | quietBit);
        // anything + NaN = qNaN
        if (bAbs > infRep) return (b | quietBit);

        if (aAbs == infRep) {
            // +/-infinity + -/+infinity = qNaN
            if ((a ^ b) == signBit) return qnanRep;
            // +/-infinity + anything remaining = +/- infinity
            else return a;
        }

        // anything remaining + +/-infinity = +/-infinity
        if (bAbs == infRep) return b;

        // zero + anything = anything
        if (!aAbs) {
            // but we need to get the sign right for zero + zero
            if (!bAbs) return (a & b);
            else return b;
        }

        // anything + zero = anything
        if (!bAbs) return a;
    }

    // Swap a and b if necessary so that a has the larger absolute value.
    if (bAbs > aAbs) {
        const uint32 temp = a;
        a = b;
        b = temp;
    }

    // Extract the exponent and significand from the (possibly swapped) a and b.
    int aExponent = a >> significandBits & maxExponent;
    int bExponent = b >> significandBits & maxExponent;
    rep_t aSignificand = a & significandMask;
    rep_t bSignificand = b & significandMask;

    // Normalize any denormals, and adjust the exponent accordingly.
    //if (aExponent == 0) aExponent = normalize(&aSignificand);
    //if (bExponent == 0) bExponent = normalize(&bSignificand);

    // The sign of the result is the sign of the larger operand, a.  If they
    // have opposite signs, we are performing a subtraction; otherwise addition.
    const rep_t resultSign = a & signBit;
    const bool subtraction = (a ^ b) & signBit;

    // Shift the significands to give us round, guard and sticky, and or in the
    // implicit significand bit.  (If we fell through from the denormal path it
    // was already set by normalize( ), but setting it twice won't hurt
    // anything.)
    aSignificand = (aSignificand | implicitBit) << 3;
    bSignificand = (bSignificand | implicitBit) << 3;

    // Shift the significand of b by the difference in exponents, with a sticky
    // bottom bit to get rounding correct.
    const unsigned int align = aExponent - bExponent;
    if (align) {
        if (align < typeWidth) {
            //const bool sticky = bSignificand << (typeWidth - align);
            bSignificand = bSignificand >> align;
        } else {
            bSignificand = 0; // sticky; b is known to be non-zero.
        }
    }

    if (subtraction) {
        aSignificand -= bSignificand;

        // If a == -b, return +zero.
        if (aSignificand == 0) return 0;

        // If partial cancellation occured, we need to left-shift the result
        // and adjust the exponent:
        if (aSignificand < implicitBit << 3) {
            const int shift = rep_clz(aSignificand) - rep_clz(implicitBit << 3);
            aSignificand <<= shift;
            aExponent -= shift;
        }
    }

    else /* addition */ {
        aSignificand += bSignificand;

        // If the addition carried up, we need to right-shift the result and
        // adjust the exponent:
        if (aSignificand & implicitBit << 4) {
            const bool sticky = aSignificand & 1;
            aSignificand = aSignificand >> 1 | sticky;
            aExponent += 1;
        }
    }

    // If we have overflowed the type, return +/- infinity:
    if (aExponent >= maxExponent) return infRep | resultSign;

    if (aExponent <= 0) {
        // Result is denormal before rounding; the exponent is zero and we
        // need to shift the significand.
        const int shift = 1 - aExponent;
        const bool sticky = aSignificand << (typeWidth - shift);
        aSignificand = aSignificand >> shift | sticky;
        aExponent = 0;
    }

    // Low three bits are round, guard, and sticky.
    const int roundGuardSticky = aSignificand & 0x7;

    // Shift the significand into place, and mask off the implicit bit.
    rep_t result = aSignificand >> 3 & significandMask;

    // Insert the exponent and sign.
    result |= (rep_t)aExponent << significandBits;
    result |= resultSign;

    return result;
}