#include "util.hpp"

#include <Eigen/Geometry>
#include <vector>
#include <map>
#include <fstream>

#include <float.h>

#include <cvx/util/math/rng.hpp>
#include <cvx/util/geometry/kdtree.hpp>

using namespace Eigen ;
using namespace std ;
using namespace cvx::util ;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static uint closest_center(const Vector3f &p, const vector<Vector3f> &c, float &closest_dist)
{
    float min_dist = FLT_MAX ;
    uint best_idx ;

    for(uint i=0 ; i<c.size() ; i++ )
    {
        float dist = (c[i] - p).norm() ;
        if ( dist < min_dist ) {
            min_dist = dist ;
            best_idx = i ;
        }
    }

    closest_dist = min_dist ;

    return best_idx ;

}

static void pruneModes(vector<Vector3f> &centers, vector<float> &cweights, float threshold, vector<uint> &cassign )
{
    uint n = centers.size() ;

    if ( n == 0 ) return ;

    vector<Vector3f> prunned ;
    vector<uint> idxs ;
    vector<float> cwassign ;

    idxs.resize(n) ;

    prunned.push_back(centers[0]) ;
    cassign.push_back(1) ;
    cwassign.push_back(cweights[0]) ;
    idxs[0] = 0 ;

    for( uint j=1 ; j<n ; j++ )
    {
        float closest_dist ;
        uint closest = closest_center(centers[j], prunned, closest_dist) ;

        if ( closest_dist < threshold )
        {
            assert(closest < cwassign.size()) ;

            cassign[closest] ++ ;
            cwassign[closest] += cweights[j] ;
            idxs[j] = closest ;
        }
        else {
            prunned.push_back(centers[j]) ;
            cassign.push_back(1) ;
            cwassign.push_back(cweights[j]) ;
            idxs[j] = cassign.size() - 1 ;
        }
    }

    centers = prunned ;
    cweights = cwassign ;
}

struct WeightSorter
{
    WeightSorter(const vector<float> &weights): weights_(weights) {}

    bool operator () (const int a, const int b) { return weights_[a] >= weights_[b] ; }
    const vector<float> weights_ ;
};

void mean_shift(RNG &g, const std::vector<Vector3f> &pts, Vector3f &mode,  float &weight, float sigma,
                    unsigned int maxIter, float seed_perc, uint min_seeds)
{
    uint n = pts.size() ;

    uint n_seed = std::max((uint)(n * seed_perc), std::min(n, min_seeds)) ;

    vector<uint> seed_idx ;

    for( uint i=0 ; i<n ; i++ )
        seed_idx.push_back(i) ;

    g.shuffle(seed_idx) ;

    Vector3f best_center ;
    float max_weight = -FLT_MAX ;

    for(uint r=0 ; r<n_seed ; r++)
    {
        const Vector3f &seed = pts[seed_idx[r]] ;

        Vector3f center = seed ;
        double center_dist ;
        uint iter = 0 ;
        float weight ;

        // shift center until convergence

        do {

            Vector3f new_center(0, 0, 0) ;
            double denom = 0.0 ;

            for(uint i=0 ; i<n ; i++ )
            {
                float sqd = (center - pts[i]).squaredNorm() ;
                double ep = exp(-sqd/ (2.0 * sigma * sigma));

                denom += ep ;

                new_center += ep * pts[i] ;
            }

            new_center /= denom ;

            center_dist = (new_center - center).norm() ;

            ++iter ;

            center = new_center ;

            weight = denom ;

        } while ( center_dist > 1.0e-7  && iter < maxIter ) ;

        if ( weight > max_weight ) {
            max_weight = weight ;
            best_center = center ;
        }
    }

    mode = best_center ;
    weight = max_weight ;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// rigid pose estimation

Isometry3f find_rigid(const Matrix3Xf &P, const Matrix3Xf &Q) {

    // Default output
    Isometry3f A;
    A.linear() = Matrix3f::Identity(3, 3);
    A.translation() = Vector3f::Zero();

    if (P.cols() != Q.cols())
        throw "Find3DAffineTransform(): input data mis-match";

    // Center the data
    Vector3f p = P.rowwise().mean();
    Vector3f q = Q.rowwise().mean();

    Matrix3Xf X = P.colwise() - p;
    Matrix3Xf Y = Q.colwise() - q;

    // SVD
    MatrixXf Cov = X*Y.transpose();
    JacobiSVD<MatrixXf> svd(Cov, ComputeThinU | ComputeThinV);

    // Find the rotation, and prevent reflections
    Matrix3f I = Matrix3f::Identity(3, 3);
    double d = (svd.matrixV()*svd.matrixU().transpose()).determinant();
    (d > 0.0) ? d = 1.0 : d = -1.0;
    I(2, 2) = d;

    Matrix3f R = svd.matrixV()*I*svd.matrixU().transpose();

    // The final transform
    A.linear() = R;
    A.translation() = q - R*p;

    return A;
}

Isometry3f find_rigid(const vector<Vector3f> &src, const vector<Vector3f> &dst) {

    assert( src.size() == dst.size() ) ;

    Eigen::Map<Matrix3Xf> m_src((float *)src.data(), 3, src.size())  ;
    Eigen::Map<Matrix3Xf> m_dst((float *)dst.data(), 3, dst.size())  ;

    return find_rigid(m_src, m_dst) ;
}

//////////////////////////////////////////////////////////////

Vector3f back_project(const cv::Mat &depth, const PinholeCamera &cam, const cv::Point &pt) {
    float z = depth.at<ushort>(pt)/1000.0 ;
    return cam.backProject(pt.x, pt.y, z) ;
}

void save_cloud_obj(const string &file_name, const std::vector<Vector3f> &cloud)
{
    ofstream strm(file_name.c_str()) ;

    for( uint i=0 ; i<cloud.size() ; i++ )
        strm << "v " << cloud[i].adjoint() << endl ;
}


#pragma once

#include <Eigen/Geometry>

namespace iNav {

class IMUData {
public:
  double timestamp;
  Eigen::Vector3d gyro;
  Eigen::Vector3d acc;
};

class IMUParam {
public:
  double ARW;
  double VRW;
  double gyro_bias_std;
  double T_gyro_bias;
  double acc_bias_std;
  double T_acc_bias;
  double gyro_scalar_std;
  double T_gyro_scalar;
  double acc_scalar_std;
  double T_acc_scalar;
};

class GnssData {
public:
  double timestamp;
  Eigen::Vector3d pos;
  Eigen::Vector3d pos_std;
};

class NavData {
public:
  double timestamp;
  Eigen::Vector3d pos;
  Eigen::Vector3d pos_std;
  Eigen::Vector3d vel;
  Eigen::Vector3d vel_std;
  Eigen::Vector3d att;
  Eigen::Vector3d att_std;
};

}  // namespace iNav


/*
* 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


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::exponential::model.hpp                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under 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_STATISTICS_SURVIVAL_MODEL_MODELS_EXPONENTIAL_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_MODEL_MODELS_EXPONENTIAL_MODEL_HPP_ER_2009
#include <numeric>
#include <boost/mpl/assert.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/type_traits/is_scalar.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/range.hpp>

namespace boost{
namespace statistics{

namespace survival{
namespace model{
namespace exponential{

    template<typename T>
    class model{
        public:
        typedef T value_type;

        model();

        template<typename X,typename B>
        static typename boost::enable_if<boost::is_scalar<X>,T>::type
        log_rate(const X& x,const B& b);

        template<typename X,typename B>
        static typename boost::disable_if<boost::is_scalar<X>,T>::type
        log_rate(const X& x,const B& b);

        protected:
        friend class boost::serialization::access;
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version){
            // no member variables
        }

    };

    // Implementation //

    template<typename T>
    model<T>::model(){}

    template<typename T>
    template<typename X,typename B>
    typename boost::enable_if<boost::is_scalar<X>,T>::type
    model<T>::log_rate(const X& x,const B& b){
        BOOST_MPL_ASSERT((
            is_scalar<B>
        ));
        return ( static_cast<T>( x ) * static_cast<T>( b ) );
    }

    template<typename T>
    template<typename X,typename B>
    typename boost::disable_if<is_scalar<X>,T>::type
    model<T>::log_rate(const X& x,const B& b){
        BOOST_ASSERT(size(x) == size(b));
        return std::inner_product(
            boost::begin(x),
            boost::end(x),
            boost::begin(b),
            static_cast<T>(0)
        );
    }

}// exponential
}// model
}// survival
}// statistics
}// boost

#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


// Copyright (C) 2017 Vicente J. Botet Escriba
//
//  Distributed under 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)

// <experimental/strong_type.hpp>

/// This example tries to see how close we can define money using strong_type
/// money should be close to strong_counter as it is close to duration.
/// However, there are some issues.
///     There is not a common currency (until we decide to have it)
///         Adding money with different currencies needs the currency result, and so needs to be lazy
///     Having a common currency means to do two operations to convert from two currencies.
///         The compiler could optimize it if the currency is know at compile time, but in general this is not the case.
///     Should we have implicit or explicit conversions between currencies?

#include <iostream>
#include <functional>
#include <type_traits>
#include "currency.hpp"
#include <experimental/strong_counter.hpp>
#include <experimental/fundamental/v2/config.hpp>

namespace stdex = std::experimental;

template <class M>
using currency_t = typename M::domain::currency_type;

template<class Currency>
struct money_domain
{
    using currency_type =  Currency;
    template <class T, class C, class U
          , typename std::enable_if <
              std::conjunction <
                std::is_convertible<U, T>, // no overflow
                std::disjunction<
                      std::is_floating_point<T>,
                      std::conjunction<
                          //std::integral_constant<bool, std::ratio_divide<P, Period>::den == 1>,
                          std::true_type,
                          std::negation< std::is_floating_point<U> >
                      >
                  >
              >::value
          >::type* = nullptr
    >
    static T inter_domain_convert(money_domain<C>, U const& u)
    {
        return T(currency::convert<C,Currency>(u));
    }

    template <class T, class C, class U>
    static T inter_domain_cast(money_domain<C>, U const& u)
    {
        return T(currency::convert<C,Currency>(u));
    }

    template <class T, class U
          , typename std::enable_if <
              std::conjunction <
                std::is_convertible<U, T>,
                std::disjunction<
                      std::is_floating_point<T>,
                      std::negation< std::is_floating_point<U> >
                >
              >::value
          >::type* = nullptr
          >
    static T intra_domain_convert(U const& u)
    {
        return T(u);
    }
};

namespace std
{

namespace experimental
{
inline  namespace fundamental_v3 {
namespace mixin {
  template <class Currency>
  struct is_compatible_with<money_domain<Currency>, money_domain<Currency>> : std::true_type {};
}
}

template <class Currency>
struct domain_converter<money_domain<Currency>> : money_domain<Currency>  {  };
}

}

// fixme: Should be Rep, Currency?
// fixme: Should we add a Period parameter to count in Million Dollars, or in cents?
//      Don't having a period, forces almost to work with double.
//      Having a period allows to work with integers most of the time

template <class Currency, class Rep=double>
using money = stdex::strong_counter<money_domain<Currency>, Rep>;

namespace money_expr
{
/// helper function that converts a money to a specific currency using the conversion_factor customization point
template <class Currency, class C, class R>
double to_currency(money<C, R> const& m)
{
    return currency::convert<C, Currency>(m.count());
}

template <class M1, class M2, class Op>
struct binary
{
    M1 m1;
    M2 m2;

    // fixme:: shouldn't this require that the representation is convertible from the common_type?
    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>() {
        return money<C,R>( Op{}(to_currency<C>(m1), to_currency<C>(m2)));
    }
};

template <class M1, class M2>
struct add
{
    M1 m1;
    M2 m2;

    // fixme:: shouldn't this requires that the representation is convertible from the common_type?
    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( to_currency<C>(m1) + to_currency<C>(m2));
    }
};
template <class M1, class M2>
struct substract
{
    M1 m1;
    M2 m2;

    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( to_currency<C>(m1) - to_currency<C>(m2));
    }
};

template <class C, class M1, class M2>
struct divide
{
    M1 m1;
    M2 m2;

    template <class R>
    JASEL_CXX14_CONSTEXPR operator R()
    {
        return money<C,R>( to_currency<C>(m1) / to_currency<C>(m2));
    }
};

template <class M1, class M2>
struct modulo
{
    M1 m1;
    M2 m2;

    template <class C, class R>
    JASEL_CXX14_CONSTEXPR operator money<C,R>()
    {
        return money<C,R>( to_currency<C>(m1) % to_currency<C>(m2));
    }
};

}

template <class C1, class R1, class C2, class R2, typename = std::enable_if_t< ! std::is_same<C1, C2>::value>>
constexpr auto operator+(money<C1,R1> m1, money<C2,R2> m2) noexcept
-> money_expr::add<money<C1,R1>, money<C2,R2>>
{
    return money_expr::add<money<C1,R1>, money<C2,R2>>{m1, m2} ;
    //return money_expr::binary<money<C1,R1>, money<C2,R2>, std::plus>{m1, m2} ;
}

template <class C1, class R1, class C2, class R2, typename = std::enable_if_t< ! std::is_same<C1, C2>::value>>
constexpr auto operator-(money<C1,R1> m1, money<C2,R2> m2) noexcept
-> money_expr::substract<money<C1,R1>, money<C2,R2>>
{
    return money_expr::substract<money<C1,R1>, money<C2,R2>>{m1, m2} ;
}

template <class OSTREAM, class C, class R>
OSTREAM& operator<<(OSTREAM& os, money<C, R> m)
{
    os<< m.count() << C{}.symbol();
    return os;
}

using dollars = money<currency::dollar>;
using euros = money<currency::euro>;

#include <boost/detail/lightweight_test.hpp>
#include <iostream>

int main()
{
    {
        dollars d(5);
        BOOST_TEST(long(d.count()) == 5);
    }
    {
        dollars d;
        d = dollars(5);
        BOOST_TEST(long(d.count()) == 5);
    }
    {
        dollars d;
        d = dollars(5);
        BOOST_TEST(d == dollars(5));
    }
    {
        dollars d1(5), d2(10);
        BOOST_TEST(d1+d2 == dollars(15));
    }
    {
        dollars d(5);
        BOOST_TEST(+d  == d);
    }
    {
        dollars d1(5), d2(10);
        BOOST_TEST(d1-d2 == dollars(-5));
    }
    {
        dollars d(5);
        BOOST_TEST(-d  == dollars(-5));
    }
    {
        dollars d(5);
        BOOST_TEST(d * 3  == dollars(15));
    }
    {
        dollars d(5);
        BOOST_TEST(3 * d  == dollars(15));
    }
    {
        dollars d(15);
        BOOST_TEST(d / 3  == dollars(5));
    }
    {
        using idollars = money<currency::dollar, int>;
        idollars d(16);
        BOOST_TEST(d % 3  == idollars(1));
    }
//    {
//        using CR = std::common_type_t<double,int>;
//        double d(16);
//        BOOST_TEST(CR(d) % 3  == 1);
//    }
    {
        dollars d(10);
        euros e(5);
        euros x = d + e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(25));
    }
    {
        euros e1(5);
        dollars d(10);
        euros e2(5);
        euros x = e1 + d + e2;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(30));
    }
    {
        dollars d(10);
        euros e(5);
        e =  d;
        BOOST_TEST(e == euros(20));
    }
    {
        dollars d(10);
        euros e(5);
        euros x = d - e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(15));
    }
    {
        dollars d(10);
        euros e(5);
        euros x = d - e;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(15));
    }
    {
        euros e1(5);
        dollars d(10);
        euros e2(5);
        euros x = e1 - d - e2;
        std::cout <<x << std::endl;
        BOOST_TEST(x == euros(-20));
    }
    return ::boost::report_errors();
}


//=======================================================================
// Copyright (c)
//
// Distributed under 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)
//=======================================================================
/**
 * @file scheduling_jobs_long_test.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2013-11-19
 */
#include "test_utils/scheduling.hpp"
#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/utils/irange.hpp"
#include "paal/greedy/scheduling_jobs/scheduling_jobs.hpp"

#include <boost/test/unit_test.hpp>

#include <vector>
#include <utility>

BOOST_AUTO_TEST_CASE(testSchedulingJobs) {
    typedef long long Time;
    typedef std::pair<int, long long> Machine;
    typedef Time Job;

    const unsigned seed = 68;
    const Time optTime = 10000;
    const long min_machines = 10;
    const long max_machines = 100000;
    const long step_machines = 10;
    const long maxMachineSpeed = 100;
    const double min_jobs_on_machine_start = 1.0;
    const double min_jobs_on_machine_end = 5.0;
    const double min_jobs_on_machine_step = 0.33;

    std::srand(seed);
    for (int numberOfMachines = min_machines; numberOfMachines <= max_machines;
         numberOfMachines *= step_machines) {
        for (double minJobsOnMachine = min_jobs_on_machine_start;
             minJobsOnMachine < min_jobs_on_machine_end;
             minJobsOnMachine += min_jobs_on_machine_step) {
            LOGLN("machines: " << numberOfMachines);
            std::vector<Machine> machines(numberOfMachines);
            for (auto machineID : paal::irange(numberOfMachines)) {
                machines[machineID] =
                    std::make_pair(machineID, rand() % maxMachineSpeed + 1);
            }
            auto getSpeed = [](Machine machine) { return machine.second; };

            std::vector<Job> jobs = generate_job_loads(
                machines, minJobsOnMachine, optTime, getSpeed);
            LOGLN("jobs: " << jobs.size());

            typedef std::vector<std::pair<decltype(machines) ::iterator,
                                          decltype(jobs) ::iterator>> Result;
            Result resultRandomized, resultDeterministic;

            paal::greedy::schedule_randomized(
                machines.begin(), machines.end(), jobs.begin(), jobs.end(),
                back_inserter(resultRandomized), getSpeed,
                paal::utils::identity_functor());
            paal::greedy::schedule_deterministic(
                machines.begin(), machines.end(), jobs.begin(), jobs.end(),
                back_inserter(resultDeterministic), getSpeed,
                paal::utils::identity_functor());

            auto checkAndPrint = [&](const Result & result) {
                check_jobs(result, jobs);
                double max_time = get_max_time(result, getSpeed);
                check_result(max_time, double(optTime), 2);
            };

            checkAndPrint(resultRandomized);
            checkAndPrint(resultDeterministic);
        }
    }
}


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include "pgcpp/common_concepts.h"
#include "pgcpp/fractions.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <doctest/doctest.h>
// #include <iostream>

using namespace fun;

TEST_CASE("undefined behavior")
{
    int a = 125;
    int c = 32;
    [[maybe_unused]] int b = a >> c; // see if your tool can catch the problem
    // std::cout << "125 >> 32 = " << b << "\n";
}

TEST_CASE("Fraction")
{
    using boost::multiprecision::cpp_int;
    static_assert(Integral<cpp_int>);

    const auto a = cpp_int {3};
    const auto b = cpp_int {4};
    const auto c = cpp_int {5};
    const auto d = cpp_int {6};
    const auto f = cpp_int {-30};
    const auto g = cpp_int {40};
    const auto z = cpp_int {0};
    const auto h = cpp_int {-g};

    const auto p = Fraction {a, b};
    // std::cout << p << '\n';
    const auto q = Fraction {c, d};

    CHECK(p == Fraction(30, 40));
    CHECK(p + q == Fraction(19, 12));
    CHECK(p - q == Fraction(-1, 12));
    CHECK(p != 0);
}

TEST_CASE("Fraction Special Cases")
{
    const auto p = Fraction {3, 4};
    const auto inf = Fraction {1, 0};
    const auto nan = Fraction {0, 0};
    const auto zero = Fraction {0, 1};

    CHECK(-inf < zero);
    CHECK(zero < inf);
    CHECK(-inf < p);
    CHECK(p < inf);
    CHECK(inf == inf);
    CHECK(-inf < inf);
    CHECK(inf == inf * p);
    CHECK(inf == inf * inf);
    CHECK(inf == p / zero);
    CHECK(inf == inf / zero);
    CHECK(nan == nan);
    CHECK(nan == inf * zero);
    CHECK(nan == -inf * zero);
    CHECK(nan == inf / inf);
    CHECK(nan == nan * zero);
    CHECK(nan == nan * nan);
    CHECK(inf == inf + inf);
    CHECK(nan == inf - inf);
    // CHECK( inf + p == nan ); // ???
    // CHECK( -inf + p == nan ); // ???
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/cgold.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/five.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check cgold behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::cgold;
  using boost::simd::Cgold;

  STF_TYPE_IS(decltype(Cgold<T>()), T);
  STF_EQUAL(Cgold<T>(), T(0));
  STF_EQUAL(cgold( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check cgold behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::cgold;
  using boost::simd::Cgold;

  STF_TYPE_IS(decltype(Cgold<T>()), T);
  STF_ULP_EQUAL(Cgold<T>()+T(1), Cgold<T>()*Cgold<T>(), 1);
  STF_LESS(  Cgold<T>(), T(1));
  auto z = cgold( as(T{}));
  STF_ULP_EQUAL((z+T(1)),z*z, 1);
                                              }


/*
 * ConstantNode.cpp
 *
 *  Created on: 2013/06/23
 *      Author: kryozahiro
 */

#include "ConstantNode.h"

#include <boost/lexical_cast.hpp>
using namespace std;

ConstantNode::ConstantNode(pair<double, double> minmax) : minmax(minmax), constant(minmax.first) {
}

void ConstantNode::randomize(const ProgramType& programType, mt19937_64& randomEngine) {
	uniform_real_distribution<double> dist(minmax.first, minmax.second);
	constant = dist(randomEngine);
}

double ConstantNode::operator()(const vector<double>& input) {
	return constant;
}

int ConstantNode::getArity() const {
	return 0;
}

string ConstantNode::getName() const {
	return boost::lexical_cast<string>(constant);
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"

#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
#include "timestepping/limexWithoutJens.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;

#include "mygrid.hh"
#include "integrate.hh"
#include "movingsource.hh"

struct InitialValue
{
  using Scalar = double;
  static int const components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
                  Dune::FieldVector<typename Cell::ctype,Cell::dimension> const& localCoordinate) const
  {
    Dune::FieldVector<typename Cell::ctype,Cell::dimensionworld> x = cell.geometry().global(localCoordinate);
    x -= 0.5;
    return 0.0;
  }

private:
  int component;
};

int main(int argc, char *argv[])
  {
    int const dim = 2;
    int refinements = 5, order = 2, extrapolOrder = 2, maxSteps = 100,
      verbosity=1;
    double dt = 0.1, maxDT = 1.0, T = 1.0, rTolT = 1.0e-3, aTolT = 1.0e-3, rTolX = 2.0e-5, aTolX = 2.0e-5, writeInterval = 1.0;

    std::cout << "Start moving source tutorial program" << std::endl;

    using Grid = Dune::UGGrid<dim>;
    using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<double,Grid::LeafGridView> >;
    using Spaces = boost::fusion::vector<H1Space const*>;
    using VariableDescriptions = boost::fusion::vector<VariableDescription<0,1,0> >;
    using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
    using Equation = MovingSourceEquation<double,VariableSet>;

    GridManager<Grid> gridManager(RefineGrid<Grid>(refinements));
    std::cout << "Grid: " << gridManager.grid().size(0) << " " << gridManager.grid().size(1) << " "
              << gridManager.grid().size(2) << std::endl;

// construct involved spaces.

  H1Space temperatureSpace(gridManager,gridManager.grid().leafView(),order);

    Spaces spaces(&temperatureSpace);

    std::string varNames[1] = { "u" };

    VariableSet variableSet(spaces,varNames);

    Equation Eq;

  std::vector<VariableSet::VariableSet> solutions;
  // std::vector<VariableSet::VariableSet> devnull;

  Eq.time(0);
  VariableSet::VariableSet x(variableSet);
  Eq.scaleInitialValue<0>(InitialValue(0),x);

  x = integrate(gridManager,Eq,variableSet,spaces,
          dt,maxDT,T,maxSteps,rTolT,aTolT,rTolX,aTolX,extrapolOrder,
          std::back_inserter(solutions),writeInterval,x,DirectType::SUPERLU,verbosity);

    std::cout << "End moving source tutorial program" << std::endl;

  }


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/SVD>
#include <Eigen/LU>

template<typename MatrixType, unsigned int Options> void svd(const MatrixType& m = MatrixType(), bool pickrandom = true)
{
  typedef typename MatrixType::Index Index;
  Index rows = m.rows();
  Index cols = m.cols();

  enum {
    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
    ColsAtCompileTime = MatrixType::ColsAtCompileTime
  };

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime> MatrixUType;
  typedef Matrix<Scalar, ColsAtCompileTime, ColsAtCompileTime> MatrixVType;
  typedef Matrix<Scalar, RowsAtCompileTime, 1> ColVectorType;
  typedef Matrix<Scalar, ColsAtCompileTime, 1> InputVectorType;

  MatrixType a;
  if(pickrandom) a = MatrixType::Random(rows,cols);
  else a = m;

  JacobiSVD<MatrixType,Options> svd(a);
  MatrixType sigma = MatrixType::Zero(rows,cols);
  sigma.diagonal() = svd.singularValues().template cast<Scalar>();
  MatrixUType u = svd.matrixU();
  MatrixVType v = svd.matrixV();

  //std::cout << "a\n" << a << std::endl;
  //std::cout << "b\n" << u * sigma * v.adjoint() << std::endl;

  VERIFY_IS_APPROX(a, u * sigma * v.adjoint());
  VERIFY_IS_UNITARY(u);
  VERIFY_IS_UNITARY(v);
}

template<typename MatrixType> void svd_verify_assert()
{
  MatrixType tmp;

  SVD<MatrixType> svd;
  //VERIFY_RAISES_ASSERT(svd.solve(tmp, &tmp))
  VERIFY_RAISES_ASSERT(svd.matrixU())
  VERIFY_RAISES_ASSERT(svd.singularValues())
  VERIFY_RAISES_ASSERT(svd.matrixV())
  /*VERIFY_RAISES_ASSERT(svd.computeUnitaryPositive(&tmp,&tmp))
  VERIFY_RAISES_ASSERT(svd.computePositiveUnitary(&tmp,&tmp))
  VERIFY_RAISES_ASSERT(svd.computeRotationScaling(&tmp,&tmp))
  VERIFY_RAISES_ASSERT(svd.computeScalingRotation(&tmp,&tmp))*/
}

void test_jacobisvd()
{
  for(int i = 0; i < g_repeat; i++) {
    Matrix2cd m;
    m << 0, 1,
         0, 1;
    CALL_SUBTEST_1(( svd<Matrix2cd,0>(m, false) ));
    m << 1, 0,
         1, 0;
    CALL_SUBTEST_1(( svd<Matrix2cd,0>(m, false) ));
    Matrix2d n;
    n << 1, 1,
         1, -1;
    CALL_SUBTEST_2(( svd<Matrix2d,0>(n, false) ));
    CALL_SUBTEST_3(( svd<Matrix3f,0>() ));
    CALL_SUBTEST_4(( svd<Matrix4d,Square>() ));
    CALL_SUBTEST_5(( svd<Matrix<float,3,5> , AtLeastAsManyColsAsRows>() ));
    CALL_SUBTEST_6(( svd<Matrix<double,Dynamic,2> , AtLeastAsManyRowsAsCols>(Matrix<double,Dynamic,2>(10,2)) ));

    CALL_SUBTEST_7(( svd<MatrixXf,Square>(MatrixXf(50,50)) ));
    CALL_SUBTEST_8(( svd<MatrixXcd,AtLeastAsManyRowsAsCols>(MatrixXcd(14,7)) ));
  }
  CALL_SUBTEST_9(( svd<MatrixXf,0>(MatrixXf(300,200)) ));
  CALL_SUBTEST_10(( svd<MatrixXcd,AtLeastAsManyColsAsRows>(MatrixXcd(100,150)) ));

  CALL_SUBTEST_3(( svd_verify_assert<Matrix3f>() ));
  CALL_SUBTEST_3(( svd_verify_assert<Matrix3d>() ));
  CALL_SUBTEST_9(( svd_verify_assert<MatrixXf>() ));
  CALL_SUBTEST_11(( svd_verify_assert<MatrixXd>() ));

  // Test problem size constructors
  CALL_SUBTEST_12( JacobiSVD<MatrixXf>(10, 20) );
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE fourcenter_test

// Third party includes
#include <boost/test/unit_test.hpp>

#include <votca/tools/eigenio_matrixmarket.h>

// Local VOTCA includes
#include "votca/xtp/aobasis.h"
#include "votca/xtp/fourcenter.h"
#include "votca/xtp/qmmolecule.h"

using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(fourcenter_test)

BOOST_AUTO_TEST_CASE(small_l_test) {

  QMMolecule mol(" ", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/fourcenter/molecule.xyz");

  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/fourcenter/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);

  const AOShell& shell0 = aobasis.getShell(2);
  const AOShell& shell1 = aobasis.getShell(3);
  const AOShell& shell2 = aobasis.getShell(4);
  const AOShell& shell4 = aobasis.getShell(5);
  FCMatrix fcenter;
  Eigen::Tensor<double, 4> block(shell0.getNumFunc(), shell4.getNumFunc(),
                                 shell2.getNumFunc(), shell1.getNumFunc());
  block.setZero();
  fcenter.FillFourCenterRepBlock(block, shell0, shell4, shell2, shell1);

  Eigen::Map<Eigen::VectorXd> mapped_result(block.data(), block.size());
  Eigen::VectorXd ref = Eigen::VectorXd::Zero(block.size());
  ref << 0.021578, 0.0112696, 0.0112696, 0.0112696, 0.021578, 0.0112696,
      0.0112696, 0.0112696, 0.021578;
  Eigen::TensorMap<Eigen::Tensor<double, 4> > ref_block(
      ref.data(), shell0.getNumFunc(), shell4.getNumFunc(), shell2.getNumFunc(),
      shell1.getNumFunc());

  bool check = mapped_result.isApprox(ref, 0.0001);
  BOOST_CHECK_EQUAL(check, 1);
  if (!check) {
    cout << "ref" << endl;
    cout << ref_block << endl;
    cout << "result" << endl;
    cout << block << endl;
  }
}

BOOST_AUTO_TEST_CASE(large_l_test) {

  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) + "/fourcenter/C2.xyz");

  BasisSet basisset;
  basisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/fourcenter/G.xml");

  AOBasis dftbasis;
  dftbasis.Fill(basisset, mol);

  FCMatrix fcenter;
  Eigen::Tensor<double, 4> block(
      dftbasis.getShell(0).getNumFunc(), dftbasis.getShell(1).getNumFunc(),
      dftbasis.getShell(0).getNumFunc(), dftbasis.getShell(1).getNumFunc());
  block.setZero();
  fcenter.FillFourCenterRepBlock(block, dftbasis.getShell(0),
                                 dftbasis.getShell(1), dftbasis.getShell(0),
                                 dftbasis.getShell(1));
  // we only check the first and last 600 values because this gets silly quite
  // quickly
  Eigen::Map<Eigen::VectorXd> mapped_result(block.data(), block.size());
  Eigen::VectorXd ref_head = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/fourcenter/largeLbegin.mm");

  Eigen::VectorXd ref_tail = votca::tools::EigenIO_MatrixMarket::ReadVector(
      std::string(XTP_TEST_DATA_FOLDER) + "/fourcenter/largeLend.mm");

  bool check_head = mapped_result.head<600>().isApprox(ref_head, 0.0001);
  BOOST_CHECK_EQUAL(check_head, 1);
  if (!check_head) {
    cout << "ref" << endl;
    cout << ref_head.transpose() << endl;
    cout << "result" << endl;
    cout << mapped_result.head<600>().transpose() << endl;
  }

  bool check_tail = mapped_result.tail<600>().isApprox(ref_tail, 0.0001);
  BOOST_CHECK_EQUAL(check_tail, 1);
  if (!check_tail) {
    cout << "ref" << endl;
    cout << ref_tail.transpose() << endl;
    cout << "result" << endl;
    cout << mapped_result.tail<600>().transpose() << endl;
  }
}

BOOST_AUTO_TEST_SUITE_END()


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integer_list.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/type_list.hpp>

#include <type_traits>
using namespace boost::hana;
using namespace literals;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
        return x % 2_c != 0_c;
    };

    constexpr auto types = type_list<int, char, long, short, char, double>;
    constexpr auto ints = integer_list<int, 1, 2, 3>;

    BOOST_HANA_CONSTANT_ASSERT(count(ints, odd) == 2_c);

    BOOST_HANA_CONSTANT_ASSERT(count(types, trait<std::is_floating_point>) == 1_c);
    BOOST_HANA_CONSTANT_ASSERT(count(types, _ == type<char>) == 2_c);
    BOOST_HANA_CONSTANT_ASSERT(count(types, _ == type<void>) == 0_c);
    //! [main]
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 the Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      130301    D. Dirkx          Migrated from personal code.
 *      130308    E.D. Brandon      Minor changes.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Springer, 2000.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/format.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/geodeticCoordinateConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_geodetic_coordinate_conversions )

BOOST_AUTO_TEST_CASE( testGeodeticCoordinateConversions )
{
    using namespace coordinate_conversions;
    using namespace unit_conversions;

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testCartesianPosition( 1917032.190, 6029782.349, -801376.113 );

    // Expected Cartesian state, Montenbruck & Gill (2000) Exercise 5.3.
    const Eigen::Vector3d testGeodeticPosition( -63.667,
                                                convertDegreesToRadians( -7.26654999 ),
                                                convertDegreesToRadians( 72.36312094 ) );

    // Central body characteristics (WGS84 Earth ellipsoid).
    const double flattening = 1.0 / 298.257223563;
    const double equatorialRadius = 6378137.0;

    // Test conversion to geodetic coordinates.
    {
        // Calculate geodetic position.
        const Eigen::Vector3d calculatedGeodeticPosition =
                convertCartesianToGeodeticCoordinates(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );

        // Compare per coefficients (different tolerances).
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.x( ) - testGeodeticPosition.x( ), 1.0E-4 );
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.y( ) - testGeodeticPosition.y( ), 1.0E-10 );
        BOOST_CHECK_SMALL( calculatedGeodeticPosition.z( ) - testGeodeticPosition.z( ), 1.0E-10 );
    }

    // Test separate functions for altitude and geodetic latitude.
    {
        // Calculate altitude and geodetic latitude using dedicated functions.
        const double directAltitude = calculateAltitudeOverOblateSpheroid(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );
        const double directGeodeticLatitude = calculateGeodeticLatitude(
                    testCartesianPosition, equatorialRadius, flattening, 1.0E-4 );

        // Compare values.
        BOOST_CHECK_SMALL( directAltitude - testGeodeticPosition.x( ), 1.0E-4 );
        BOOST_CHECK_SMALL( directGeodeticLatitude - testGeodeticPosition.y( ), 1.0E-10 );
    }

    // Test conversions from geodetic coordinates to cartesian position.
    {
        const Eigen::Vector3d calculateCartesianPosition =
                convertGeodeticToCartesianCoordinates(
                    testGeodeticPosition, equatorialRadius, flattening  );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    calculateCartesianPosition, testCartesianPosition, 1.0E-9 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef __FOLDING_UTILS__
#define __FOLDING_UTILS__

#include <math.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <stdexcept>

namespace generic_control_toolbox
{
class MatrixParser
{
 public:
  MatrixParser();
  ~MatrixParser();

  /**
    Initialize a nxn matrix with values obtained from the ros parameter
    server.

    @param M The matrix to be initialized
    @param param_name The parameter server location
    @param n The ros nodehandle used to query the parameter server

    @throw logic_error in case vals does not have square dimensions.
    @return True for success, False if parameter is not available.
  **/
  static bool parseMatrixData(Eigen::MatrixXd &M, const std::string param_name,
                              const ros::NodeHandle &n);

  /**
    Computed the skew-symmetric matrix of a 3-dimensional vector.

    @param v The 3-dimensional vector
    @return The skew-symmetric matrix
  **/
  static Eigen::Matrix3d computeSkewSymmetric(const Eigen::Vector3d &v);

 private:
  /**
    Fill in a nxn matrix with the given values.

    @param M The matrix to be filled in. Will be set to the size nxn.
    @param vals A vector with the values to fill in
    @throw logic_error in case vals does not have square dimensions.
  **/
  static void initializeEigenMatrix(Eigen::MatrixXd &M,
                                    const std::vector<double> &vals);
};
}  // namespace generic_control_toolbox

#endif