// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora
// =============================================================================

#include "chrono_modal/ChEigenvalueSolver.h"
#include "chrono/core/ChMathematics.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <numeric>

#include <Spectra/KrylovSchurGEigsSolver.h>
#include <Spectra/SymGEigsSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/MatOp/SparseGenMatProd.h>
#include <Spectra/MatOp/SparseRegularInverse.h>
#include <Spectra/GenEigsBase.h>

using namespace Spectra;

using Matrix = Eigen::MatrixXd;
using Vector = Eigen::VectorXd;
using SpMatrix = Eigen::SparseMatrix<double>;

namespace chrono {

namespace modal {

// This is an helper class for using Krylov-Schur eigen solver also with the shift&invert mode,
// because at the moment it is not yet available in Spectra.
template <typename OpType, typename BOpType>
class KrylovSchurGEigsShiftInvert :
    public KrylovSchurGEigsBase<SymGEigsShiftInvertOp<OpType, BOpType>, BOpType>
{
private:
    using Scalar = typename OpType::Scalar;
    using Index = Eigen::Index;
    using Array = Eigen::Array<Scalar, Eigen::Dynamic, 1>;

    using ModeMatOp = SymGEigsShiftInvertOp<OpType, BOpType>;
    using Base = KrylovSchurGEigsBase<ModeMatOp, BOpType>;
    using Base::m_nev;
    using Base::m_ritz_val;

    const Scalar m_sigma;

    // Set shift and forward
    static ModeMatOp set_shift_and_move(ModeMatOp&& op, const Scalar& sigma)
    {
        op.set_shift(sigma);
        return std::move(op);
    }

    // First transform back the Ritz values, and then sort
    void sort_ritzpair(SortRule sort_rule) override
    {
        // The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma)
        // So the eigenvalues of the original problem is lambda = 1 / nu + sigma
        m_ritz_val.head(m_nev).array() = Scalar(1) / m_ritz_val.head(m_nev).array() + m_sigma;
        Base::sort_ritzpair(sort_rule);
    }

public:
    KrylovSchurGEigsShiftInvert(OpType& op, BOpType& Bop, Index nev, Index ncv, const Scalar& sigma) :
        Base(set_shift_and_move(ModeMatOp(op, Bop), sigma), Bop, nev, ncv),
        m_sigma(sigma)
    {}
};

// Assembly a sparse matrix by bordering square H with rectangular Cq.
//    HCQ = [ H  Cq' ]
//          [ Cq  0  ]
void sparse_assembly_2x2symm(Eigen::SparseMatrix<double, Eigen::ColMajor, int>& HCQ,       ///< resulting square sparse matrix (column major)
	const ChSparseMatrix& H,   ///< square sparse H matrix, n_v x n_v
	const ChSparseMatrix& Cq)  ///< rectangular  sparse Cq  n_c x n_v
{
	int n_v   = H.rows();
	int n_c   = Cq.rows();
	HCQ.resize(n_v + n_c, n_v + n_c);
	HCQ.reserve(H.nonZeros() + 2 * Cq.nonZeros());
    HCQ.setZero();

	for (int k=0; k<H.outerSize(); ++k)
		for (ChSparseMatrix::InnerIterator it(H,k); it; ++it) {
			HCQ.insert(it.row(),it.col()) = it.value();
        }

	for (int k=0; k<Cq.outerSize(); ++k)
        for (ChSparseMatrix::InnerIterator it(Cq, k); it; ++it) {
            HCQ.insert(it.row() + n_v, it.col()) = it.value(); // insert Cq
            HCQ.insert(it.col(), it.row() + n_v) = it.value(); // insert Cq'
        }

    // This seems necessary in Release mode
    HCQ.makeCompressed();

    //***NOTE***
    // for some reason the HCQ matrix created via .insert() or .elementRef() or triplet insert, is
    // corrupt in Release mode, not in Debug mode. However, when doing a loop like the one below,
    // it repairs it.
    // ***TODO*** avoid this bad hack and find the cause of the release/debug difference.
    for (int k = 0; k < HCQ.rows(); ++k) {
        for (int j = 0; j < HCQ.cols(); ++j) {
            auto foo = HCQ.coeffRef(k, j);
            //GetLog() << HCQ.coeffRef(k,j) << " ";
        }
    }
}

bool ChGeneralizedEigenvalueSolverKrylovSchur::Solve(const ChSparseMatrix& M,  ///< input M matrix, n_v x n_v
        const ChSparseMatrix& K,  ///< input K matrix, n_v x n_v
        const ChSparseMatrix& Cq, ///< input Cq matrix of constraint jacobians, n_c x n_v
        ChMatrixDynamic<std::complex<double>>& V,    ///< output matrix n x n_v with eigenvectors as columns, will be resized
        ChVectorDynamic<std::complex<double>>& eig,  ///< output vector with n eigenvalues, will be resized.
        ChVectorDynamic<double>& freq,  ///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
        ChEigenvalueSolverSettings settings  ///< optional: settings for the solver, or n. of desired lower eigenvalues. If =0, return all eigenvalues.
) const
{
	// Assembly the A and B for the generalized constrained eigenvalue problem.
	// Note that those sparse matrices must be column-major for better compatibility with Spectra.
	int n_vars   = M.rows();
	int n_constr = Cq.rows();

	// A  =  [ -K   -Cq' ]
	//       [ -Cq    0  ]
	Eigen::SparseMatrix<double> A(n_vars + n_constr, n_vars + n_constr);
	sparse_assembly_2x2symm(A, -K, -Cq);

	// B  =  [  M     0  ]
	//       [  0     0  ]
	Eigen::SparseMatrix<double> B(n_vars + n_constr, n_vars + n_constr);
	ChSparseMatrix O(n_constr, n_vars); O.setZero();
	sparse_assembly_2x2symm(B, M, O);

	int m = 2 * settings.n_modes >= 30 ? 2 * settings.n_modes : 30;  // minimum subspace size   //**TO DO*** make parametric?
	if (m > n_vars + n_constr-1)
		m = n_vars + n_constr-1;
	if (m <= settings.n_modes)
		m = settings.n_modes+1;

	// Construct matrix operation objects using the wrapper classes
	using OpType = SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
    using BOpType = SparseSymMatProd<double>;
    OpType op(A, B);
    BOpType Bop(B);

	// Dump data for test. ***TODO*** remove when well tested
	if (false)
	{
		ChStreamOutAsciiFile fileA("dump_modal_A.dat");
		fileA.SetNumFormat("%.12g");
		StreamOUTdenseMatlabFormat(ChMatrixDynamic<>(A), fileA);
		ChStreamOutAsciiFile fileB("dump_modal_B.dat");
		fileB.SetNumFormat("%.12g");
		StreamOUTdenseMatlabFormat(ChMatrixDynamic<>(B), fileB);
	}

	// The Krylov-Schur solver, using the shift and invert mode:
 	KrylovSchurGEigsShiftInvert<OpType, BOpType> eigen_solver(op, Bop, settings.n_modes, m, settings.sigma);  //*** OK EIGVECTS, WRONG EIGVALS REQUIRE eigen_values(i) = (1.0 / eigen_values(i)) + sigma;

	eigen_solver.init();

	int nconv = eigen_solver.compute(SortRule::LargestMagn, settings.max_iterations, settings.tolerance);

	if (settings.verbose) {
		if (eigen_solver.info() != CompInfo::Successful)
		{
			GetLog() << "KrylovSchurGEigsSolver FAILED. \n";
			if (eigen_solver.info() == CompInfo::NotComputed) GetLog() << " Error: not computed. \n";
			if (eigen_solver.info() == CompInfo::NotConverging) GetLog() << " Error: not converging. \n";
			if (eigen_solver.info() == CompInfo::NumericalIssue) GetLog() << " Error: numerical issue. \n";
			GetLog() << " nconv  = " << nconv << "\n";
			GetLog() << " niter  = " << eigen_solver.num_iterations() << "\n";
			GetLog() << " nops   = " << eigen_solver.num_operations() << "\n";
			return false;
		}
		else
		{
			GetLog() << "KrylovSchurGEigsSolver successfull. \n";
			GetLog() << " nconv   = " << nconv << "\n";
			GetLog() << " niter   = " << eigen_solver.num_iterations() << "\n";
			GetLog() << " nops    = " << eigen_solver.num_operations() << "\n";
			GetLog() << " n_modes = " << settings.n_modes  << "\n";
			GetLog() << " n_vars  = " << n_vars << "\n";
			GetLog() << " n_constr= " << n_constr << "\n";
		}
	}
	Eigen::VectorXcd eigen_values = eigen_solver.eigenvalues();
	Eigen::MatrixXcd eigen_vectors = eigen_solver.eigenvectors();

	// ***HACK***
	// Correct eigenvals for shift-invert because KrylovSchurGEigsShiftInvert does not take care of it.
	// This should be automatically done by KrylovSchurGEigsShiftInvert::sort_ritz_pairs() at the end of compute(),
	// but at the moment such sort_ritz_pairs() is not called by the base KrylovSchurGEigsBase, differently from SymGEigsShiftSolver, for example.
	for (int i = 0; i < eigen_values.rows() ; ++i)
	{
		eigen_values(i) = (1.0 / eigen_values(i)) + settings.sigma;
	}

    // Return values
    V.setZero(M.rows(), settings.n_modes);
    eig.setZero(settings.n_modes);
	freq.setZero(settings.n_modes);

    for (int i = 0; i < settings.n_modes; i++) {
        V.col(i) = eigen_vectors.col(i).head(n_vars);  // store only displacement part of eigenvector, no constraint part
        V.col(i).normalize(); // check if necessary or already normalized
        eig(i) = eigen_values(i);
		freq(i) = (1.0 / CH_C_2PI) * sqrt(-eig(i).real());
    }

    return true;
}

bool ChGeneralizedEigenvalueSolverLanczos::Solve(const ChSparseMatrix& M,  ///< input M matrix, n_v x n_v
        const ChSparseMatrix& K,  ///< input K matrix, n_v x n_v
        const ChSparseMatrix& Cq, ///< input Cq matrix of constraint jacobians, n_c x n_v
        ChMatrixDynamic<std::complex<double>>& V,    ///< output matrix n x n_v with eigenvectors as columns, will be resized
        ChVectorDynamic<std::complex<double>>& eig,  ///< output vector with n eigenvalues, will be resized.
        ChVectorDynamic<double>& freq,  ///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
        ChEigenvalueSolverSettings settings ///< optional: settings for the solver, or n. of desired lower eigenvalues. If =0, return all eigenvalues.)
)  const
{
	// Assembly the A and B for the generalized constrained eigenvalue problem.
	// Note that those sparse matrices must be column-major for better compatibility with Spectra.
	int n_vars   = M.rows();
	int n_constr = Cq.rows();

	// A  =  [ -K   -Cq' ]
	//       [ -Cq    0  ]
	Eigen::SparseMatrix<double, Eigen::ColMajor, int> A(n_vars + n_constr, n_vars + n_constr);
	sparse_assembly_2x2symm(A, -K, -Cq);

	// B  =  [  M     0  ]
	//       [  0     0  ]
	Eigen::SparseMatrix<double, Eigen::ColMajor, int> B(n_vars + n_constr, n_vars + n_constr);
	ChSparseMatrix O(n_constr, n_vars); O.setZero();
	sparse_assembly_2x2symm(B, M, O);

	int m = 2 * settings.n_modes >= 20 ? 2 * settings.n_modes : 20;  // minimum subspace size
	if (m > n_vars + n_constr -1)
		m = n_vars + n_constr -1;
	if (m <= settings.n_modes)
		m = settings.n_modes+1;

	// Construct matrix operation objects using the wrapper classes
    using OpType = SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
    using BOpType = SparseSymMatProd<double>;
    OpType op(A, B);
    BOpType Bop(B);

	// The Lanczos solver, using the shift and invert mode
    SymGEigsShiftSolver<OpType, BOpType, GEigsMode::ShiftInvert> eigen_solver(op, Bop, settings.n_modes, m, settings.sigma);

	eigen_solver.init();

	int nconv = eigen_solver.compute(SortRule::LargestMagn, settings.max_iterations, settings.tolerance);

	if (settings.verbose) {
		if (eigen_solver.info() != CompInfo::Successful)
		{
			GetLog() << "Lanczos eigenvalue solver FAILED. \n";
			if (eigen_solver.info() == CompInfo::NotComputed) GetLog() << " Error: not computed. \n";
			if (eigen_solver.info() == CompInfo::NotConverging) GetLog() << " Error: not converging. \n";
			if (eigen_solver.info() == CompInfo::NumericalIssue) GetLog() << " Error: numerical issue. \n";
			GetLog() << " nconv  = " << nconv << "\n";
			GetLog() << " niter  = " << eigen_solver.num_iterations() << "\n";
			GetLog() << " nops   = " << eigen_solver.num_operations()  << "\n";
			return false;
		}
		else
		{
			GetLog() << "Lanczos eigenvalue solver successfull. \n";
			GetLog() << " nconv   = " << nconv << "\n";
			GetLog() << " niter   = " << eigen_solver.num_iterations() << "\n";
			GetLog() << " nops    = " << eigen_solver.num_operations() << "\n";
			GetLog() << " n_modes = " << settings.n_modes  << "\n";
			GetLog() << " n_vars  = " << n_vars << "\n";
			GetLog() << " n_constr= " << n_constr << "\n";
		}
	}
	Eigen::VectorXcd eigen_values = eigen_solver.eigenvalues();
	Eigen::MatrixXcd eigen_vectors = eigen_solver.eigenvectors();

	// Return values
    V.setZero(M.rows(), settings.n_modes);
    eig.setZero(settings.n_modes);
	freq.setZero(settings.n_modes);

    for (int i = 0; i < settings.n_modes; i++) {
        V.col(i) = eigen_vectors.col(i).head(n_vars);  // store only displacement part of eigenvector, no constraint part
        V.col(i).normalize(); // check if necessary or already normalized
        eig(i) = eigen_values(i);
		freq(i) = (1.0 / CH_C_2PI) * sqrt(-eig(i).real());
    }

    return true;
}

int ChModalSolveUndamped::Solve(
	const ChSparseMatrix& M,  ///< input M matrix, n_v x n_v
	const ChSparseMatrix& K,  ///< input K matrix, n_v x n_v
	const ChSparseMatrix& Cq, ///< input Cq matrix of constraint jacobians, n_c x n_v
	ChMatrixDynamic<std::complex<double>>& V,    ///< output matrix n x n_v with eigenvectors as columns, will be resized
	ChVectorDynamic<std::complex<double>>& eig,  ///< output vector with n eigenvalues, will be resized.
	ChVectorDynamic<double>& freq   ///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
) const
{
	int found_eigs = 0;
	V.resize(0, 0);
	eig.resize(0);
	freq.resize(0);

	// for each freq_spans finds the closest modes to i-th input frequency:
	for (int i = 0; i < this->freq_spans.size(); ++i) {

		int nmodes_goal_i = this->freq_spans[i].nmodes;
		double sigma_i = -pow(this->freq_spans[i].freq * CH_C_2PI, 2); // sigma for shift&invert, as lowest eigenvalue, from Hz info

		ChMatrixDynamic<std::complex<double>> V_i;
		ChVectorDynamic<std::complex<double>> eig_i;
		ChVectorDynamic<double> freq_i;

		V_i.setZero(M.rows(), nmodes_goal_i);
		eig_i.setZero(nmodes_goal_i);
		freq_i.setZero(nmodes_goal_i);

		ChEigenvalueSolverSettings settings_i (nmodes_goal_i, this->max_iterations, this->tolerance, this->verbose, sigma_i);

		if (!this->msolver.Solve(M, K, Cq, V_i, eig_i, freq_i, settings_i))
			return found_eigs;

		// append to list of results

		int nmodes_out_i = eig_i.size();

		// Sort modes by frequencies if not exactly in increasing order. Some solver sometime fail at this.
		std::vector<int> order(nmodes_out_i);
		std::iota(order.begin(), order.end(), 0);
		std::sort(order.begin(), order.end(), [&](int a, int b) {
			return freq_i[a] < freq_i[b];
		});
		Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm;
		perm.indices() =  Eigen::Map<Eigen::ArrayXi>(order.data(), order.size());
		V_i = V_i * perm;
		eig_i = perm * eig_i;
		freq_i = perm * freq_i;

		// avoid overlap when multiple shifts were used, and too close.. If is it may happen that the lowest eigvals of
		// some shift are smaller than the highest of the previous shift.
		int i_nodes_notoverlap = nmodes_out_i;
		if (freq.size() > 0) {
			double upper_freq = freq[freq.size()-1];
			for (int j = 0; j < nmodes_out_i; ++j)
				if (freq_i[j] < upper_freq)
					i_nodes_notoverlap--;
		}

		if (i_nodes_notoverlap) {
			V.conservativeResize(M.rows(), V.cols() + i_nodes_notoverlap);
			eig.conservativeResize(eig.size() + i_nodes_notoverlap);
			freq.conservativeResize(freq.size() + i_nodes_notoverlap);
			V.rightCols(i_nodes_notoverlap) = V_i;
			eig.tail(i_nodes_notoverlap) = eig_i;
			freq.tail(i_nodes_notoverlap) = freq_i;
			found_eigs = eig.size();
		}
	}
	return found_eigs;
}

//-------------------------------------------------------------------------------------------------------------------

bool ChQuadraticEigenvalueSolverNullspaceDirect::Solve(const ChSparseMatrix& M, const ChSparseMatrix& R, const ChSparseMatrix& K, const ChSparseMatrix& Cq,
                                                        ChMatrixDynamic<std::complex<double>>& V,    ///< output matrix with eigenvectors as columns, will be resized
                                                        ChVectorDynamic<std::complex<double>>& eig,  ///< output vector with eigenvalues (real part not zero if some damping), will be resized
														ChVectorDynamic<double>& freq,  ///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
														ChVectorDynamic<double>& damping_ratio,  ///< output vector with n damping rations r=damping/critical_damping.
                                                        ChEigenvalueSolverSettings settings ) const
{
    // The folowing is adapted from the former implementation of Peng Chao

	// Compute the null space of the Cq matrix
    ChMatrixDynamic<> Cq_full = Cq; // because the fullPivLu is not available for sparse matrix in Eigen
	Eigen::MatrixXd Cq_null_space = Cq_full.fullPivLu().kernel();
	Eigen::MatrixXd M_hat = Cq_null_space.transpose() * M * Cq_null_space;
	Eigen::MatrixXd K_hat = Cq_null_space.transpose() * K * Cq_null_space;
	Eigen::MatrixXd R_hat = Cq_null_space.transpose() * R * Cq_null_space;

	// frequency-shift，Solve the matrix singular problem - it is not needed now, you can set it to 0
    double freq_shift = 0; // shift value, can take any value
	Eigen::MatrixXd M_bar = M_hat;
	Eigen::MatrixXd K_bar = pow(freq_shift, 2) * M_hat + freq_shift * R_hat + K_hat;
	Eigen::MatrixXd R_bar = 2 * freq_shift * M_hat + R_hat;
	Eigen::MatrixXd M_bar_inv = M_bar.inverse();  // performance warning! dense inverse matrix! Only small sizes should be used.

	// Generate the A matrix of the state equation, whose eigenvalues are the modal frequencies
	int dim = M_bar.rows();
	Eigen::MatrixXd A_tilde(2 * dim, 2 * dim);
	A_tilde << Eigen::MatrixXd::Zero(dim, dim), Eigen::MatrixXd::Identity(dim, dim),
		       -M_bar_inv * K_bar             , -M_bar_inv * R_bar;

	// Call EIGEN3, dense matrix to directly solve the eigenvalues and eigenvectors
	// NOTE：EIGEN3 has a very fast calculation speed in release mode, and a very slow calculation speed in debug mode
	Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(A_tilde);

	Eigen::VectorXcd eigen_values = eigen_solver.eigenvalues() + freq_shift;
	Eigen::MatrixXcd eigen_vectors = eigen_solver.eigenvectors();

    class eig_vect_and_val
    {
    public:
        Eigen::VectorXcd eigen_vect;
        std::complex<double> eigen_val;

        bool operator < (const eig_vect_and_val& str) const {
            return (eigen_val.imag() < str.eigen_val.imag());
        }
    };

	std::vector<eig_vect_and_val> all_eigen_values_and_vectors;
	for (int i = 0; i < eigen_values.size(); i++) {
        eig_vect_and_val vec_and_val{ eigen_vectors.col(i), eigen_values(i) };
        all_eigen_values_and_vectors.push_back(vec_and_val);
	}

	// sort
	std::sort(all_eigen_values_and_vectors.begin(), all_eigen_values_and_vectors.end());   // sort by imaginary part

	// organize and return results
	int middle_number = (int)(all_eigen_values_and_vectors.size() / 2);  // The eigenvalues filtered out are conjugate complex roots, just take half
	int DOF_counts = (int)((all_eigen_values_and_vectors.at(0)).eigen_vect.rows()/2);  // The number of degrees of freedom in the model, used when extracting the mode shape.

    // Return values

	int nmodes = settings.n_modes;

	// if nmodes==0, then compute all eigs:
	if (nmodes == 0)
       nmodes = M.rows() - Cq.rows();

    // cannot use more modes than n. of dofs, if so, clamp
    nmodes = ChMin(nmodes, M.rows() - Cq.rows());

    V.setZero(M.rows(), nmodes);
    eig.setZero(nmodes);
	freq.setZero(nmodes);
	damping_ratio.setZero(nmodes);

    for (int i = 0; i < nmodes; i++) {
        int i_half = middle_number + i; // because the n.of eigenvalues is double (conjugate pairs), so just use the 2nd half after sorting
        auto mv = all_eigen_values_and_vectors.at(i_half).eigen_vect.head(DOF_counts);
        auto mvhns = Cq_null_space * mv;

        V.col(i) = mvhns;
        V.col(i).normalize(); // check if necessary or already normalized
        eig(i) = all_eigen_values_and_vectors.at(i_half).eigen_val;
		freq(i) = (1.0 / CH_C_2PI) * (std::abs(eig(i))); // undamped freq.
		damping_ratio(i) = -eig(i).real() / std::abs(eig(i));
    }

    return true;
}

//-------------------------------------------------------------------------------------------------------------------

bool ChQuadraticEigenvalueSolverKrylovSchur::Solve(const ChSparseMatrix& M, const ChSparseMatrix& R, const ChSparseMatrix& K, const ChSparseMatrix& Cq,
                                                        ChMatrixDynamic<std::complex<double>>& V,   ///< output matrix with eigenvectors as columns, will be resized
                                                        ChVectorDynamic<std::complex<double>>& eig, ///< output vector with eigenvalues (real part not zero if some damping), will be resized
														ChVectorDynamic<double>& freq,				///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
														ChVectorDynamic<double>& damping_ratio,		///< output vector with n damping rations r=damping/critical_damping.
                                                        ChEigenvalueSolverSettings settings) const
{
	//*****WARNING*****  THIS SHOULD NOT BE USED: KrylovSchurGEigsShiftInvert NOT YET WORKING FOR COMPLEX Re Im EIGENVALUES!
	throw (ChException("SOLVER ChQuadraticEigenvalueSolverKrylovSchur NOT YET WORKING. TO BE COMPLETED."));

	// Generate the A and B in state space
	int n_vars   = M.rows();
	int n_constr = Cq.rows();

	//***TO DO*** avoid Ad and Bd...
	// In fact here we create a temporary couple of _dense_ matrices Ad and Bd, just to exploit the easy << operator,
	// then later copied to sparse matrices A and B. But sparse matrices A and B should be used from the beginning
	// to avoid the dense Ad and Bd!!! (This is not done right now just because Eigen does not offer easy block-initialization for sparse matrices).
	ChMatrixDynamic<> Ad(2 * n_vars + n_constr, 2 * n_vars + n_constr);
	ChMatrixDynamic<> Bd(2 * n_vars + n_constr, 2 * n_vars + n_constr);
	ChMatrixDynamic<> Md = M;
	ChMatrixDynamic<> Rd = R;
	ChMatrixDynamic<> Kd = K;
	ChMatrixDynamic<> Cqd = Cq;

	// A  =  [  0     I     0 ]
	//       [ -K    -R  -Cq' ]
	//       [ -Cq    0     0 ]
	Ad << Eigen::MatrixXd::Zero    (n_vars,  n_vars), Eigen::MatrixXd::Identity(n_vars,  n_vars), Eigen::MatrixXd::Zero(n_vars,   n_constr),
		 -Kd                                        , -Rd                                       , -Cqd.transpose(),
		 -Cqd                                       , Eigen::MatrixXd::Zero(n_constr,n_vars)    , Eigen::MatrixXd::Zero(n_constr, n_constr);

	// B  =  [  I     0     0 ]
	//       [  0     M     0 ]
	//       [  0     0     0 ]
	Bd << Eigen::MatrixXd::Identity(n_vars,  n_vars), Eigen::MatrixXd::Zero(n_vars,  n_vars), Eigen::MatrixXd::Zero(n_vars,   n_constr),
		  Eigen::MatrixXd::Zero    (n_vars,  n_vars), Md                                    , Eigen::MatrixXd::Zero(n_vars,   n_constr),
		  Eigen::MatrixXd::Zero    (n_constr,n_vars), Eigen::MatrixXd::Zero(n_constr,n_vars), Eigen::MatrixXd::Zero(n_constr, n_constr);

	Eigen::SparseMatrix<double> A(2 * n_vars + n_constr, 2 * n_vars + n_constr);
	Eigen::SparseMatrix<double> B(2 * n_vars + n_constr, 2 * n_vars + n_constr);
	A = Ad.sparseView();
	B = Bd.sparseView();

	int n_computed_eigs = 2 * settings.n_modes;
	int m = 2 * n_computed_eigs >= 30 ? 2 * n_computed_eigs : 30;  // minimum subspace size   //**TO DO*** make parametric
	if (m > 2*n_vars + n_constr)
		m = 2*n_vars + n_constr;

	// Construct matrix operation objects using the wrapper classes
	//using OpType = SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
    //using BOpType = SparseSymMatProd<double>;
	using OpType = SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
    using BOpType = SparseSymMatProd<double>;
    OpType op(A, B);
    BOpType Bop(B);

	// The complex Krylov-Schur solver, using the shift and invert mode:
	KrylovSchurGEigsShiftInvert<OpType, BOpType> eigen_solver(op, Bop, n_computed_eigs, m, settings.sigma);   //***TODO**** wait that SPECTRA will have KrylovSchur implemented for the COMPLEX case

	eigen_solver.init();

	int nconv = eigen_solver.compute(SortRule::LargestMagn, settings.max_iterations, settings.tolerance);
    int niter = eigen_solver.num_iterations();
    int nops  = eigen_solver.num_operations();

	if (settings.verbose) {
		if (eigen_solver.info() != CompInfo::Successful)
		{
			GetLog() << "KrylovSchurGEigsShiftInvert FAILED. \n";
			if (eigen_solver.info() == CompInfo::NotComputed) GetLog() << " Error: not computed. \n";
			if (eigen_solver.info() == CompInfo::NotConverging) GetLog() << " Error: not converging. \n";
			if (eigen_solver.info() == CompInfo::NumericalIssue) GetLog() << " Error: numerical issue. \n";
			GetLog() << " nconv = " << nconv << "\n";
			GetLog() << " niter = " << niter << "\n";
			GetLog() << " nops  = " << nops  << "\n";

			return false;
		}
		else
		{
			GetLog() << "KrylovSchurGEigsShiftInvert successfull. \n";
			GetLog() << " nconv   = " << nconv << "\n";
			GetLog() << " niter   = " << niter << "\n";
			GetLog() << " nops    = " << nops  << "\n";
			GetLog() << " n_modes = " << settings.n_modes  << "\n";
			GetLog() << " n_eigs  = " << settings.n_modes*2 << "\n";
			GetLog() << " n_vars  = " << n_vars << "\n";
			GetLog() << " n_constr= " << n_constr << "\n";
		}
	}
	Eigen::VectorXcd eigen_values = eigen_solver.eigenvalues();
	Eigen::MatrixXcd eigen_vectors = eigen_solver.eigenvectors();

	// ***HACK***
	// Correct eigenvals for shift-invert because KrylovSchurGEigsShiftInvert does not take care of it.
	// This should be automatically done by KrylovSchurGEigsShiftInvert::sort_ritz_pairs() at the end of compute(),
	// but at the moment such sort_ritz_pairs() is not called by the base KrylovSchurGEigsBase, differently from SymGEigsShiftSolver, for example.
	for (int i = 0; i < eigen_values.rows() ; ++i)
	{
        eigen_values(i) = (1.0 / eigen_values(i)) + settings.sigma;
	}

    class eig_vect_and_val
    {
    public:
        Eigen::VectorXcd eigen_vect;
        std::complex<double> eigen_val;

        bool operator < (const eig_vect_and_val& str) const {
            return (eigen_val.imag() < str.eigen_val.imag());
        }
    };

	std::vector<eig_vect_and_val> all_eigen_values_and_vectors;
	for (int i = 0; i < eigen_values.size(); i++) {
        eig_vect_and_val vec_and_val{ eigen_vectors.col(i), eigen_values(i) };
        all_eigen_values_and_vectors.push_back(vec_and_val);
		GetLog() << " Eig " << i << "= " << eigen_values(i).real() << "  " << eigen_values(i).imag()
			<< "   freq= " << (1.0/CH_C_2PI)*std::abs(eigen_values(i)) << "\n";
	}

	// sort
	//std::sort(all_eigen_values_and_vectors.begin(), all_eigen_values_and_vectors.end());   // sort by imaginary part

	// organize and return results
	int middle_number = (int)(all_eigen_values_and_vectors.size() / 2);  // The eigenvalues filtered out are conjugate complex roots, just take half

    // Return values
    V.setZero(M.rows(), settings.n_modes);
    eig.setZero(settings.n_modes);
	freq.setZero(settings.n_modes);
	damping_ratio.setZero(settings.n_modes);

    for (int i = 0; i < settings.n_modes; i++) {
        //int i_half = middle_number + i; // because the n.of eigenvalues is double (conjugate pairs), so just use the 2nd half after sorting
		int i_half = i;

        V.col(i) = all_eigen_values_and_vectors.at(i_half).eigen_vect.head(n_vars);  // store only displacement part of eigenvector, no speed part, no constraint part
        V.col(i).normalize(); // check if necessary or already normalized
        eig(i) = all_eigen_values_and_vectors.at(i_half).eigen_val;
		freq(i) = (1.0 / CH_C_2PI) * (std::abs(eig(i))); // undamped freq.
		damping_ratio(i) = -eig(i).real() / std::abs(eig(i));
    }

    return true;
}

//-------------------------------------------------------------------------------------

int ChModalSolveDamped::Solve(
	const ChSparseMatrix& M,  ///< input M matrix, n_v x n_v
	const ChSparseMatrix& R,  ///< input R matrix, n_v x n_v
	const ChSparseMatrix& K,  ///< input K matrix, n_v x n_v
	const ChSparseMatrix& Cq, ///< input Cq matrix of constraint jacobians, n_c x n_v
	ChMatrixDynamic<std::complex<double>>& V,    ///< output matrix n x n_v with eigenvectors as columns, will be resized
	ChVectorDynamic<std::complex<double>>& eig,  ///< output vector with n eigenvalues, will be resized.
	ChVectorDynamic<double>& freq,   ///< output vector with n frequencies [Hz], as f=w/(2*PI), will be resized.
	ChVectorDynamic<double>& damp_ratios         ///< output vector with n damping ratios, will be resized.
) const
{
	int found_eigs = 0;
	V.resize(0, 0);
	eig.resize(0);
	freq.resize(0);
	damp_ratios.resize(0);

	// for each freq_spans finds the closest modes to i-th input frequency:
	for (int i = 0; i < this->freq_spans.size(); ++i) {

		int nmodes_goal_i = this->freq_spans[i].nmodes;
		double sigma_i = -pow(this->freq_spans[i].freq * CH_C_2PI, 2); // sigma for shift&invert, as lowest eigenvalue, from Hz info

		ChMatrixDynamic<std::complex<double>> V_i;
		ChVectorDynamic<std::complex<double>> eig_i;
		ChVectorDynamic<double> freq_i;
		ChVectorDynamic<double> damp_ratios_i;

		V_i.setZero(M.rows(), nmodes_goal_i);
		eig_i.setZero(nmodes_goal_i);
		freq_i.setZero(nmodes_goal_i);
		damp_ratios_i.setZero(nmodes_goal_i);

		ChEigenvalueSolverSettings settings_i (nmodes_goal_i, this->max_iterations, this->tolerance, this->verbose, sigma_i);

		if (!this->msolver.Solve(M, R, K, Cq, V_i, eig_i, freq_i, damp_ratios_i, settings_i))
			return found_eigs;

		// append to list of results

		int nmodes_out_i = eig_i.size();

		// Sort modes by eigenvalue imag part (or modulus(?)) if not exactly in increasing order. Some solver sometime fail at this.
		std::vector<int> order(nmodes_out_i);
		std::iota(order.begin(), order.end(), 0);
		std::sort(order.begin(), order.end(), [&](int a, int b) {
			//return std::abs(eig_i[a]) < std::abs(eig_i[b]);
			return eig_i[a].imag() < eig_i[b].imag();
		});
		Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm;
		perm.indices() =  Eigen::Map<Eigen::ArrayXi>(order.data(), order.size());
		V_i = V_i * perm;
		eig_i = perm * eig_i;
		freq_i = perm * freq_i;
		damp_ratios_i = perm * damp_ratios_i;

		// avoid overlap when multiple shifts were used, and too close.. If is it may happen that the lowest eigvals of
		// some shift are smaller than the highest of the previous shift.
		int i_nodes_notoverlap = nmodes_out_i;
		if (freq.size() > 0) {
			double upper_freq = freq[freq.size()-1];
			for (int j = 0; j < nmodes_out_i; ++j)
				if (freq_i[j] < upper_freq)
					i_nodes_notoverlap--;
		}

		if (i_nodes_notoverlap) {
			V.conservativeResize(M.rows(), V.cols() + i_nodes_notoverlap);
			eig.conservativeResize(eig.size() + i_nodes_notoverlap);
			freq.conservativeResize(freq.size() + i_nodes_notoverlap);
			damp_ratios.conservativeResize(damp_ratios.size() + i_nodes_notoverlap);
			V.rightCols(i_nodes_notoverlap) = V_i;
			eig.tail(i_nodes_notoverlap) = eig_i;
			freq.tail(i_nodes_notoverlap) = freq_i;
			damp_ratios.tail(i_nodes_notoverlap) = damp_ratios_i;
			found_eigs = eig.size();
		}
	}
	return found_eigs;
}

}  // end namespace modal

}  // end namespace chrono


// Copyright 2015-2017 Peter Dimov.
//
// 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

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/list.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <boost/config.hpp>
#include <boost/detail/workaround.hpp>
#include <type_traits>
#include <tuple>

struct X1 {};
struct X2 {};
struct X3 {};
struct X4 {};

template<class T1, class T2> struct F {};

using boost::mp11::mp_push_back;
using boost::mp11::mp_push_front;

template<class T, class L> using rev_push_back = mp_push_back<L, T>;
template<class T, class L> using rev_push_front = mp_push_front<L, T>;

using boost::mp11::mp_plus;

int main()
{
    using boost::mp11::mp_list;
    using boost::mp11::mp_reverse_fold;

    {
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<mp_list<>, void, F>, void>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<mp_list<X1>, void, F>, F<X1, void>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<mp_list<X1, X2>, void, F>, F<X1, F<X2, void>>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<mp_list<X1, X2, X3>, void, F>, F<X1, F<X2, F<X3, void>>>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<mp_list<X1, X2, X3, X4>, void, F>, F<X1, F<X2, F<X3, F<X4, void>>>>>));
    }

    {
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<>, void, F>, void>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1>, void, F>, F<X1, void>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1, X2>, void, F>, F<X1, F<X2, void>>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1, X2, X3>, void, F>, F<X1, F<X2, F<X3, void>>>>));
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1, X2, X3, X4>, void, F>, F<X1, F<X2, F<X3, F<X4, void>>>>>));
    }

    {
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1, X2, X3, X4>, mp_list<>, rev_push_back>, mp_list<X4, X3, X2, X1>>));
    }

    {
        BOOST_TEST_TRAIT_TRUE((std::is_same<mp_reverse_fold<std::tuple<X1, X2, X3, X4>, mp_list<>, rev_push_front>, mp_list<X1, X2, X3, X4>>));
    }

    using boost::mp11::mp_iota_c;
    using boost::mp11::mp_reverse;
    using boost::mp11::mp_size_t;

    {
        int const N = 37;

        using L = mp_iota_c<N>;

        using R1 = mp_reverse_fold<L, mp_list<>, rev_push_front>;
        BOOST_TEST_TRAIT_TRUE((std::is_same<R1, L>));

        using R2 = mp_reverse_fold<L, mp_list<>, rev_push_back>;
        BOOST_TEST_TRAIT_TRUE((std::is_same<R2, mp_reverse<L>>));

        using R3 = mp_reverse_fold<L, mp_size_t<0>, mp_plus>;
        BOOST_TEST_TRAIT_TRUE((std::is_same<R3, mp_size_t<N*(N-1)/2>>));
    }

    return boost::report_errors();
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file body_tail_sensor_test.hpp
 * \date August 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../../typecast.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

#include <fl/util/types.hpp>
#include <fl/model/sensor/body_tail_sensor.hpp>
#include <fl/model/sensor/linear_gaussian_sensor.hpp>
#include <fl/model/sensor/linear_decorrelated_gaussian_sensor.hpp>

template <typename TestType>
class BodyTailSensorTest:
    public testing::Test
{
public:
    enum : signed int
    {
        StateDim = TestType::Parameter::StateDim,
        ObsrvDim = TestType::Parameter::ObsrvDim,
        NoiseDim = TestType::Parameter::ObsrvDim + 1,

        StateSize = fl::TestSize<StateDim, TestType>::Value,
        ObsrvSize = fl::TestSize<ObsrvDim, TestType>::Value,
        NoiseSize = fl::TestSize<NoiseDim, TestType>::Value
    };

    typedef Eigen::Matrix<fl::Real, StateSize, 1> State;
    typedef Eigen::Matrix<fl::Real, ObsrvSize, 1> Obsrv;

    typedef fl::LinearGaussianSensor<Obsrv, State> BodyModel;
    typedef fl::LinearGaussianSensor<Obsrv, State> TailModel;
    typedef fl::BodyTailSensor<BodyModel, TailModel> BodyTailModel;

    typedef typename BodyTailModel::Noise Noise;

    BodyTailSensorTest()
        : body_model(create_body_model()),
          tail_model(create_tail_model()),
          body_tail_model(create_body_tail_model(body_model, tail_model))
    { }

    BodyModel create_body_model()
    {
        auto model = BodyModel(ObsrvDim, StateDim);

        auto noise_matrix = model.create_noise_matrix();
        noise_matrix *= fl::Real(10.0);
        model.noise_matrix(noise_matrix);

        return model;
    }

    TailModel create_tail_model()
    {
        auto model = TailModel(ObsrvDim, StateDim);

        auto noise_matrix = model.create_noise_matrix();
        noise_matrix *= fl::Real(0.1);
        model.noise_matrix(noise_matrix);

        return model;
    }

    BodyTailModel create_body_tail_model(const BodyModel& body_model,
                                         const TailModel& tail_model,
                                         fl::Real threshold = 0.5)
    {
        auto model = BodyTailModel(body_model, tail_model, threshold);

        return model;
    }

    void init_dimension_test()
    {
        EXPECT_EQ(body_tail_model.obsrv_dimension(), ObsrvDim);
        EXPECT_EQ(body_tail_model.noise_dimension(), ObsrvDim + 1);
        EXPECT_EQ(body_tail_model.state_dimension(), StateDim);

        EXPECT_EQ(body_tail_model.obsrv_dimension(),
                  body_model.obsrv_dimension());
        EXPECT_EQ(body_tail_model.state_dimension(),
                  body_model.state_dimension());

        EXPECT_EQ(body_tail_model.obsrv_dimension(),
                  tail_model.obsrv_dimension());
        EXPECT_EQ(body_tail_model.state_dimension(),
                  tail_model.state_dimension());

        EXPECT_EQ(body_tail_model.noise_dimension(),
                  1 + std::max(body_model.noise_dimension(),
                               tail_model.noise_dimension()));
    }

    void observation(fl::Real threshold)
    {
        auto x = State::Random(StateDim).eval();
        auto n = Noise::Random(NoiseDim).eval();

        n(body_tail_model.noise_dimension() - 1)
            = fl::uniform_to_normal(threshold);

        auto body_y = body_model.observation(
            x, n.topRows(body_model.noise_dimension()));
        auto tail_y = tail_model.observation(
            x, n.topRows(tail_model.noise_dimension()));
        auto body_tail_y = body_tail_model.observation(x, n);

        if (threshold > 0.5)
        {
            EXPECT_TRUE(fl::are_similar(body_y, body_tail_y));
            EXPECT_FALSE(fl::are_similar(tail_y, body_tail_y));
        }
        else
        {
            EXPECT_FALSE(fl::are_similar(body_y, body_tail_y));
            EXPECT_TRUE(fl::are_similar(tail_y, body_tail_y));
        }
    }

    void probability()
    {
        auto x = State::Random(StateDim).eval();
        auto y = Obsrv::Random(ObsrvDim).eval();

        auto body_prob = body_model.probability(y, x);
        auto tail_prob = tail_model.probability(y, x);
        auto body_tail_prob = body_tail_model.probability(y, x);

        ASSERT_DOUBLE_EQ((body_prob + tail_prob)/2., body_tail_prob);
    }

    void log_probability()
    {
        auto x = State::Random(StateDim).eval();
        auto y = Obsrv::Random(ObsrvDim).eval();

        auto body_prob = body_model.probability(y, x);
        auto tail_prob = tail_model.probability(y, x);
        auto body_tail_log_prob = body_tail_model.log_probability(y, x);

        ASSERT_DOUBLE_EQ(std::log((body_prob + tail_prob)/2.),
                         body_tail_log_prob);
    }

protected:
    BodyModel body_model;
    TailModel tail_model;
    BodyTailModel body_tail_model;
};

template <int ObsrvDimension, int StateDimension>
struct Dimensions
{
    enum: signed int
    {
        ObsrvDim = ObsrvDimension,
        StateDim = StateDimension
    };
};

typedef ::testing::Types<
            fl::StaticTest<Dimensions<2, 1>>,
            fl::StaticTest<Dimensions<2, 2>>,
            fl::StaticTest<Dimensions<3, 3>>,
            fl::StaticTest<Dimensions<10, 10>>,
            fl::DynamicTest<Dimensions<2, 1>>,
            fl::DynamicTest<Dimensions<2, 2>>,
            fl::DynamicTest<Dimensions<3, 3>>,
            fl::DynamicTest<Dimensions<10, 10>>
        > TestTypes;

TYPED_TEST_CASE(BodyTailSensorTest, TestTypes);

TYPED_TEST(BodyTailSensorTest, init_dimension)
{
    TestFixture::init_dimension_test();
}

TYPED_TEST(BodyTailSensorTest, observation_select_body)
{
    TestFixture::observation(0.6);
}

TYPED_TEST(BodyTailSensorTest, observation_select_tail)
{
    TestFixture::observation(0.4);
}

TYPED_TEST(BodyTailSensorTest, observation_select_randomly)
{
    for (int i = 0; i < 1000; ++i)
    {
        TestFixture::observation(fl::Real(rand()) / fl::Real(RAND_MAX));
    }
}

TYPED_TEST(BodyTailSensorTest, probability)
{
    for (int i = 0; i < 1000; ++i)
    {
        TestFixture::probability();
    }
}

TYPED_TEST(BodyTailSensorTest, log_probability)
{
    for (int i = 0; i < 1000; ++i)
    {
        TestFixture::log_probability();
    }
}

TYPED_TEST(BodyTailSensorTest, wrong_threshlold_exception)
{
    EXPECT_THROW(TestFixture::create_body_tail_model(
                     TestFixture::create_body_model(),
                     TestFixture::create_tail_model(),
                     fl::Real(-0.1)),
                 fl::Exception);

    EXPECT_THROW(TestFixture::create_body_tail_model(
                     TestFixture::create_body_model(),
                     TestFixture::create_tail_model(),
                     fl::Real(1.1)),
                 fl::Exception);
}

TYPED_TEST(BodyTailSensorTest, correct_threshlold)
{
    EXPECT_NO_THROW(TestFixture::create_body_tail_model(
                     TestFixture::create_body_model(),
                     TestFixture::create_tail_model(),
                     fl::Real(0.99999999)));

    EXPECT_NO_THROW(TestFixture::create_body_tail_model(
                     TestFixture::create_body_model(),
                     TestFixture::create_tail_model(),
                     fl::Real(0.000000001)));
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_CONSTANTS_MINLOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_CONSTANTS_MINLOG_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
      @brief Minlog generic tag

      Represents the Minlog constant in generic contexts.

      @par Models:
      Hierarchy
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Minlog, double
                                , 0, 0xc2b0c0a5UL
                                , 0xc086232bdd7abcd2ULL
                                )
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::Minlog, Site> dispatching_Minlog(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::Minlog, Site>();
    }
    template<class... Args>
    struct impl_Minlog;
  }
  /*!
    Generates constant Minlog used in logarithm/exponential computations
    nt2::log(x) return  0 if x is less than Minlog (underflow)

    @par Semantic:

    @code
    T r = Minlog<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = -708.3964185322641;
    else if T is float
      r = -88.3762626647949
    @endcode

    @see @{Maxlog}

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Minlog, Minlog);
}

#endif


/*!
@file
Defines `boost::hana::detail::minimal::Group`.

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

#ifndef BOOST_HANA_DETAIL_MINIMAL_GROUP_HPP
#define BOOST_HANA_DETAIL_MINIMAL_GROUP_HPP

#include <boost/hana/comparable/equal_mcd.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/group/minus_mcd.hpp>
#include <boost/hana/group/negate_mcd.hpp>
#include <boost/hana/monoid/mcd.hpp>

namespace boost { namespace hana {
namespace detail { namespace minimal {
    template <template <typename ...> class mcd = hana::Group::minus_mcd>
    struct Group { };

    template <template <typename ...> class mcd,
        typename = operators<hana::Group, hana::Monoid, hana::Comparable>>
    struct group_impl {
        int value;
        using hana_datatype = Group<mcd>;
    };

    template <template <typename ...> class mcd>
    struct make_group_impl {
        constexpr auto operator()(int i) const
        { return group_impl<mcd>{i}; }
    };

    template <template <typename ...> class mcd = hana::Group::minus_mcd>
    constexpr make_group_impl<mcd> group{};
}} // end namespace detail::minimal

// Provided for convenience in the unit tests.
template <template <typename ...> class mcd>
struct Comparable::instance<
    detail::minimal::Group<mcd>,
    detail::minimal::Group<mcd>
> : Comparable::equal_mcd
{
    static constexpr bool equal_impl(
        detail::minimal::group_impl<mcd> x,
        detail::minimal::group_impl<mcd> y
    )
    { return x.value == y.value; }
};

template <template <typename ...> class Mcd>
struct Monoid::instance<
    detail::minimal::Group<Mcd>,
    detail::minimal::Group<Mcd>
> : Monoid::mcd
{
    static constexpr auto plus_impl(
        detail::minimal::group_impl<Mcd> x,
        detail::minimal::group_impl<Mcd> y
    )
    { return detail::minimal::group<Mcd>(x.value + y.value); }

    static constexpr auto zero_impl()
    { return detail::minimal::group<Mcd>(0); }
};

template <>
struct Group::instance<
    detail::minimal::Group<Group::minus_mcd>,
    detail::minimal::Group<Group::minus_mcd>
> : Group::minus_mcd<
        detail::minimal::Group<Group::minus_mcd>,
        detail::minimal::Group<Group::minus_mcd>
    >
{
    static constexpr auto minus_impl(
        detail::minimal::group_impl<Group::minus_mcd> x,
        detail::minimal::group_impl<Group::minus_mcd> y
    )
    { return detail::minimal::group<Group::minus_mcd>(x.value - y.value); }
};

template <>
struct Group::instance<
    detail::minimal::Group<Group::negate_mcd>,
    detail::minimal::Group<Group::negate_mcd>
> : Group::negate_mcd<
    detail::minimal::Group<Group::negate_mcd>,
    detail::minimal::Group<Group::negate_mcd>
>
{
    static constexpr auto negate_impl(
        detail::minimal::group_impl<Group::negate_mcd> x
    )
    { return detail::minimal::group<Group::negate_mcd>(-x.value); }
};
}} // end namespace boost::hana

#endif // !BOOST_HANA_DETAIL_MINIMAL_GROUP_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    Notes
 *      This unit test does not include testing with the function testFunctionWithZeroRoot, because
 *      this test function is not suited for the Bisection root finder. The Bisection method
 *      requires that the interval brackets the solution, and the functions values on both sides of
 *      the root have an opposite sign. Although the interval of the test function [-3,3] brackets
 *      the root, the function values of the lower bound and the upper bound are both positive,
 *      hence the root finder throws a runtime error.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/make_shared.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/math/root_finders/bisection.h"
#include "tudat/math/root_finders/testFunction1.h"
#include "tudat/math/root_finders/testFunction2.h"
#include "tudat/math/root_finders/testFunction3.h"
#include "tudat/math/root_finders/testFunctionWithLargeRootDifference.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( testsuite_rootfinders )

using namespace tudat;
using namespace root_finders;

//! Check if Bisection method converges on test function #1 (TestFunction1).
BOOST_AUTO_TEST_CASE( test_bisection_testFunction1 )
{
    // Create object containing the test function.
    std::shared_ptr< TestFunction1 > testFunction = std::make_shared< TestFunction1 >( 1 );

    // The termination condition.
    Bisection< >::TerminationFunction terminationConditionFunction =
            createTerminationConditionFunction( TUDAT_NAN, testFunction->getTrueRootAccuracy( ) );

    // Test Bisection object.
    Bisection< > bisection( terminationConditionFunction,
                         testFunction->getLowerBound( ), testFunction->getUpperBound( ) );

    // Let the Bisection method search for the root.
    const double root = bisection.execute( testFunction );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Bisection method converges on test function #2 (TestFunction2).
BOOST_AUTO_TEST_CASE( test_bisection_testFunction2 )
{
    // Create object containing the test function.
    std::shared_ptr< TestFunction2 > testFunction = std::make_shared< TestFunction2 >( 1 );

    // The termination condition.
    Bisection< >::TerminationFunction terminationConditionFunction =
            createTerminationConditionFunction( TUDAT_NAN, testFunction->getTrueRootAccuracy( ) );

    // Test Bisection object.
    Bisection< > bisection( terminationConditionFunction,
                         testFunction->getLowerBound( ), testFunction->getUpperBound( ) );

    // Let the Bisection method search for the root.
    const double root = bisection.execute( testFunction );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Bisection method converges on test function #3 (TestFunction3).
BOOST_AUTO_TEST_CASE( test_bisection_testFunction3 )
{
    // Create object containing the test function.
    std::shared_ptr< TestFunction3 > testFunction = std::make_shared< TestFunction3 >( 1 );

    // The termination condition.
    Bisection< >::TerminationFunction terminationConditionFunction =
            createTerminationConditionFunction( TUDAT_NAN, testFunction->getTrueRootAccuracy( ) );

    // Test Bisection object.
    Bisection< > bisection( terminationConditionFunction,
                         testFunction->getLowerBound( ), testFunction->getUpperBound( ) );

    // Let the Bisection method search for the root.
    const double root = bisection.execute( testFunction );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Bisection method converges on function with large root difference
//! (testFunctionWithLargeRootDifference).
// Not the best test case. Inheritance from old code.
BOOST_AUTO_TEST_CASE( test_bisection_testFunctionWithLargeRootDifference )
{
    // Declare tolerance.
    const double tolerance = 1.0e-10;

    // Declare expected roots.
    const double expectedRootLowCase = 1.00000000793634;
    const double expectedRootHighCase = 7937.3386333591;

    // Declare interval for the bisection method.
    const double lowerBound = 1.0;
    const double upperBound = 10000.0;

    // Create objects containing the test functions. Values were obtained during a limit case
    // gravity assist calculation (while evaluating Cassini-1 trajectory).
    std::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionLowCase =
            std::make_shared< TestFunctionWithLargeRootDifference >
            ( 1, -3.24859999867635e18, -3248600.0, 1.5707963267949 );
    std::shared_ptr< TestFunctionWithLargeRootDifference > testFunctionHighCase =
            std::make_shared< TestFunctionWithLargeRootDifference >
            ( 1, -3248600.0, -3.24859999867635e18, 1.5707963267949 );

    // The termination condition.
    Bisection< >::TerminationFunction terminationConditionFunction =
            createTerminationConditionFunction( 1.0E-10 );

    // Test Bisection object, per case.
    Bisection< > bisectionLowCase( terminationConditionFunction, lowerBound, upperBound );
    Bisection< > bisectionHighCase( terminationConditionFunction, lowerBound, upperBound );

    // Let the Bisection method search for the root.
    const double rootLowCase = bisectionLowCase.execute( testFunctionLowCase );
    const double rootHighCase = bisectionHighCase.execute( testFunctionHighCase );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( rootLowCase, expectedRootLowCase, tolerance );
    BOOST_CHECK_CLOSE_FRACTION( rootHighCase, expectedRootHighCase, tolerance );
}

//! Check expected functionality. Test interval that does not bracket the root.
BOOST_AUTO_TEST_CASE( test_bisection_wrongBracket )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction1 > testFunction = std::make_shared< TestFunction1 >( 1 );

    // The termination condition.
    Bisection< >::TerminationFunction terminationConditionFunction =
            createTerminationConditionFunction( TUDAT_NAN, testFunction->getTrueRootAccuracy( ) );

    // Test Bisection object. The input interval does not bracket the solution.
    Bisection< > bisection( terminationConditionFunction,
                         testFunction->getLowerBound( ), 0.0 );

    // Check if a runtime error is thrown if the root finder is executed.
    BOOST_CHECK_THROW( bisection.execute( testFunction ),
                       std::runtime_error );
}

BOOST_AUTO_TEST_SUITE_END( ) // testsuite_rootfinders

} // namespace unit_tests
} // namespace tudat


#include <iostream>
#include <string>
#include <sstream>
#include <stdexcept>
#include <boost/rational.hpp>
#include <cassert>

void true_assert(bool q){
	if (!q)
		throw std::runtime_error("assertion failed");
}

enum NumberFormat{
	Invalid = 0,
	IntU8,
	IntU16,
	IntU24,
	IntU32,
	IntS8,
	IntS16,
	IntS24,
	IntS32,
	Float32,
	Float64,
	COUNT,
};

const char *to_string(NumberFormat f){
	static const char * const table[] = {
		"",
		"IntU8",
		"IntU16",
		"IntU24",
		"IntU32",
		"IntS8",
		"IntS16",
		"IntS24",
		"IntS32",
		"Float32",
		"Float64",
	};
	return table[f];
}

const char *to_type(NumberFormat f){
	static const char * const table[] = {
		"",
		"std::uint8_t",
		"std::uint16_t",
		"std::uint32_t",
		"std::uint32_t",
		"std::int8_t",
		"std::int16_t",
		"std::int32_t",
		"std::int32_t",
		"float",
		"double",
	};
	return table[f];
}

int size_of_type(NumberFormat f){
	static const int table[] = {
		0,
		1,
		2,
		3,
		4,
		1,
		2,
		3,
		4,
		4,
		8,
	};
	return table[f];
}

NumberFormat intermediate_type(NumberFormat f){
	static const NumberFormat table[] = {
		Invalid,
		IntU32,
		IntU32,
		IntU32,
		IntU32,
		IntU32,
		IntU32,
		IntU32,
		IntU32,
		Float32,
		Float64,
	};
	return table[f];
}

NumberFormat to_unsigned(NumberFormat f){
	static const NumberFormat table[] = {
		Invalid,
		IntU8,
		IntU16,
		IntU24,
		IntU32,
		IntU8,
		IntU16,
		IntU24,
		IntU32,
		Invalid,
		Invalid,
	};
	return table[f];
}

NumberFormat to_signed(NumberFormat f){
	static const NumberFormat table[] = {
		Invalid,
		IntS8,
		IntS16,
		IntS24,
		IntS32,
		IntS8,
		IntS16,
		IntS24,
		IntS32,
		Invalid,
		Invalid,
	};
	return table[f];
}

const char *limit_max(NumberFormat f){
	static const char * const table[] = {
		"",
		"0x000000FF",
		"0x0000FFFF",
		"0x00FFFFFF",
		"0xFFFFFFFF",
		"0x0000007F",
		"0x00007FFF",
		"0x007FFFFF",
		"0x7FFFFFFF",
		"",
		"",
	};
	return table[f];
}

const char *signedness_flipping_mask(NumberFormat f){
	static const char * const table[] = {
		"",
		"0x80",
		"0x8000",
		"0x800000",
		"0x80000000",
		"0x80",
		"0x8000",
		"0x800000",
		"0x80000000",
		"",
		"",
	};
	return table[f];
}

const char *unit(NumberFormat f){
	static const char * const table[] = {
		"",
		"1",
		"1",
		"1",
		"1",
		"1",
		"1",
		"1",
		"1",
		"1.f",
		"1.0",
	};
	return table[f];
}

int bits(NumberFormat f){
	return size_of_type(f) * 8;
}

std::string generate_load(NumberFormat src){
	std::string temp = to_type(IntU32);
	switch (src){
		case IntU8:
		case IntS8:
			return "(" + temp + ")src[0]";
		case IntU16:
		case IntS16:
			return "(" + temp + ")src[0] | ((" + temp + ")src[1] << 8)";
		case IntU24:
		case IntS24:
			return "(" + temp + ")src[0] | ((" + temp + ")src[1] << 8) | ((" + temp + ")src[2] << 16)";
		case IntU32:
		case IntS32:
			return "(" + temp + ")src[0] | ((" + temp + ")src[1] << 8) | ((" + temp + ")src[2] << 16) | ((" + temp + ")src[2] << 24)";
		case Float32:
		case Float64:
			return (std::string)"*(" + to_type(src) + " *)src";
	}
}

#define C(x, y) ((int)x | ((int)y << 8))

typedef std::pair<std::string, NumberFormat> expression_t;

const char *signed_upshift_mask(NumberFormat to, NumberFormat from){
	static const char * const table[] = {
		"0x80",
		"0x8080",
		"0x808080",
		"0x8000",
	};
	auto s1 = size_of_type(to);
	auto s2 = size_of_type(from);
	true_assert(s1 > s2);
	auto g = (boost::gcd(s1, s2) - 1) * 2;
	return table[s1 - s2 - 1 + g];
}

#define E "(" + e.first + ")"

expression_t convert_to(const expression_t &e, NumberFormat from, NumberFormat to);

expression_t convert_to(const expression_t &e, NumberFormat to){
	auto from = e.second;
	switch (C(to, from)){

		//Unsigned upshifts

		case C(IntU16,IntU8):
			return expression_t("(" E " << 8) | " E, IntU16);
		case C(IntU24,IntU8):
			return expression_t("(" E " << 16) | (" E " << 8) | " E, IntU24);
		case C(IntU32,IntU8):
			return expression_t("(" E " << 24) | (" E " << 16) | (" E " << 8) | " E, IntU32);
		case C(IntU24,IntU16):
			return expression_t("(" E " << 8) | (" E " >> 8)", IntU24);
		case C(IntU32,IntU16):
			return expression_t("(" E " << 16) | " E, IntU32);
		case C(IntU32,IntU24):
			return expression_t("(" E " << 8) | (" E " >> 16)", IntU32);

		//Right shifts

		case C(IntU8,IntU16):
		case C(IntS8,IntS16):
		case C(IntU16,IntU24):
		case C(IntS16,IntS24):
		case C(IntU24,IntU32):
		case C(IntS24,IntS32):
		case C(IntU8,IntU24):
		case C(IntS8,IntS24):
		case C(IntU16,IntU32):
		case C(IntS16,IntS32):
		case C(IntU8,IntU32):
		case C(IntS8,IntS32):
			{
				std::stringstream stream;
				stream << (E " >> ") << bits(from) - bits(to);
				return expression_t(stream.str(), to);
			}

		//Signedness flipping

		case C(IntU8,IntS8):
		case C(IntS8,IntU8):
		case C(IntU16,IntS16):
		case C(IntS16,IntU16):
		case C(IntU24,IntS24):
		case C(IntS24,IntU24):
		case C(IntU32,IntS32):
		case C(IntS32,IntU32):
			return expression_t(E " ^ " + signedness_flipping_mask(to), to);

		//Float to unsigned

		case C(IntU8,Float32):
		case C(IntU8,Float64):
		case C(IntU16,Float32):
		case C(IntU16,Float64):
		case C(IntU24,Float32):
		case C(IntU24,Float64):
		case C(IntU32,Float32):
		case C(IntU32,Float64):
			return expression_t("(std::uint32_t)((saturate(" E ") + 1) * ((" + to_type(from) + ")" + limit_max(to) + " / 2))", to);

		//Unsigned to float

		case C(Float32,IntU8):
		case C(Float32,IntU16):
		case C(Float32,IntU24):
		case C(Float32,IntU32):
		case C(Float64,IntU8):
		case C(Float64,IntU16):
		case C(Float64,IntU24):
		case C(Float64,IntU32):
			return expression_t(E " * (" + unit(to) + " * 2 / " + limit_max(from) + ") - 1", to);

		//Signed upshifts

		case C(IntS16,IntS8):
		case C(IntS24,IntS8):
		case C(IntS32,IntS8):
		case C(IntS24,IntS16):
		case C(IntS32,IntS16):
		case C(IntS32,IntS24):
			return expression_t("(" + convert_to(e, to_unsigned(from), to_unsigned(to)).first + ") ^ " + signed_upshift_mask(to, from), to);

		//Signed upshifts with sign flipping
		case C(IntU16,IntS8):
		case C(IntU24,IntS8):
		case C(IntU32,IntS8):
		case C(IntU24,IntS16):
		case C(IntU32,IntS16):
		case C(IntU32,IntS24):
			return expression_t("(" + convert_to(e, to_unsigned(from), to).first + ") ^ (" + signed_upshift_mask(to, from) + "|" + signedness_flipping_mask(to) + ")", to);

		//Float casts

		case C(Float32,Float64):
			return expression_t("(float)" E, Float32);

		case C(Float64,Float32):
			return expression_t("(double)" E, Float64);

		//Compounded conversions (size first)

		case C(IntU16,IntS24):
		case C(IntU16,IntS32):
		case C(IntU24,IntS32):
			return convert_to(convert_to(e, to_signed(to)), to);
		case C(IntS8,IntU16):
		case C(IntS8,IntU24):
		case C(IntS8,IntU32):
		case C(IntS8,Float32):
		case C(IntS8,Float64):
		case C(IntS16,IntU8):
		case C(IntS16,IntU24):
		case C(IntS16,IntU32):
		case C(IntS16,Float32):
		case C(IntS16,Float64):
		case C(IntS24,IntU8):
		case C(IntS24,IntU16):
		case C(IntS24,IntU32):
		case C(IntS24,Float32):
		case C(IntS24,Float64):
		case C(IntS32,IntU8):
		case C(IntS32,IntU16):
		case C(IntS32,IntU24):
		case C(IntS32,Float32):
		case C(IntS32,Float64):
			return convert_to(convert_to(e, to_unsigned(to)), to);

		//Compounded conversions (signedness first)

		case C(IntU8,IntS16):
		case C(IntU8,IntS24):
		case C(IntU8,IntS32):
		case C(Float32,IntS8):
		case C(Float64,IntS8):
		case C(Float32,IntS16):
		case C(Float64,IntS16):
		case C(Float32,IntS24):
		case C(Float64,IntS24):
		case C(Float32,IntS32):
		case C(Float64,IntS32):
			return convert_to(convert_to(e, to_unsigned(from)), to);

		default:
			throw std::exception();
	}
}

expression_t convert_to(const expression_t &e, NumberFormat from, NumberFormat to){
	return convert_to(expression_t(e.first, from), to);
}

#if 0
std::string generate_converter(NumberFormat dst, NumberFormat src){
	auto S = intermediate_type(src);
	auto D = intermediate_type(dst);
	std::stringstream stream;
	stream <<
			"static read_buffer_t convert_" << to_string(dst) << "_" << to_string(src) << "(read_buffer_t &&buffer, size_t bytes_per_sample, size_t channels){\n"
			"    read_buffer_t ret = allocate_buffer(buffer->sample_count * bytes_per_sample);\n"
			"    auto src = buffer->data;\n"
			"    auto dst = ret->data;\n"
			"    auto elements = buffer->sample_count * channels;\n"
			"    for (size_t i = 0; i < elements; i++){\n"
			"        " << to_type(unsigned_type(src)) << " src_val = " << generate_load(src) << ";\n";
	if (is_signed(src)){
		if (src != IntS32)
			stream <<
			"        src_val = src_val | ((src_val > " << limit_min(src) << ") * " << sign_mask(src) << ");\n";
		stream <<
			"        src_val += " << limit_min(src) << ";\n";
	}
	if (!is_float(src) && !is_float(dst))
		stream <<
			"        auto low_byte = src_val & 0xFF;\n";
	stream <<
			"        " << to_type(D) << " dst_val = " << generate_convert(D, S, dst, src, size_of_type(dst) - size_of_type(src)) << ";\n"
			"        helper<D, S>::f(dst, src);\n"
			"    }\n"
			"    return ret;\n"
			"}\n"
	return stream.str();
}
#endif

int main(){
	try{
		for (NumberFormat source = IntU8; source < COUNT; source = (NumberFormat)(source + 1)){
			for (NumberFormat dest = IntU8; dest < COUNT; dest = (NumberFormat)(dest + 1)){
				if (source == dest){

					continue;
				}

				std::cout << "// " << to_string(source) << " -> " << to_string(dest) << "\n\n";

				auto temp = convert_to(expression_t("x", source), dest);
				true_assert(temp.second == dest);

				std::cout <<
					"template <>\n"
					"struct converter<" << to_string(source) << ", " << to_string(dest) << ">{\n"
					"\tstatic " << to_type(intermediate_type(dest)) << " f(" << to_type(intermediate_type(source)) << " x){\n"
					"\t\treturn " << temp.first << ";\n"
					"\t}\n"
					"};\n"
					"\n"
					;
			}
		}
		for (NumberFormat i = IntU8; i < COUNT; i = (NumberFormat)(i + 1)){
			std::cout << "// " << to_string(i) << " -> " << to_string(i) << "\n\n";
			std::cout <<
				"template <>\n"
				"struct converter<" << to_string(i) << ", " << to_string(i) << ">{\n"
				"\tstatic " << to_type(intermediate_type(i)) << " f(" << to_type(intermediate_type(i)) << " x){\n"
				"\t\treturn x;\n"
				"\t}\n"
				"};\n"
				"\n"
				;
		}
	}catch (std::exception &e){
		std::cerr << e.what() << std::endl;
		return -1;
	}
	return 0;
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [range]
#include <boost/simd/range/range.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/literal.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>
#include <vector>

using namespace boost::simd::literal;

int main()
{
  std::vector<float> x{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};

  auto pic = boost::simd::range(x.begin(),x.end(), 8_c);
  for(auto&& e : pic) std::cout << e;
  std::cout << std::endl;

  boost::simd::pack<float> q{0};
  auto pr = boost::simd::range(x);
  for(auto&& e : pr) q += e;

  std::cout << "Sum of [" << x.front() << " ... " << x.back() << "] is " << boost::simd::sum(q) << std::endl;

  return 0;
}
//! [range]


//
//  Copyright © 2015 Claus Christmann <hcc |ä| gatech.edu>.
//
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at
//
//      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.
//

#include "MVC/BlipModel/positionitem.h"
#include "MVC/poditem.h"
#include <boost/graph/graph_concepts.hpp>
#include <QtCore/QObject> // for tr()
#include <amg.hpp>

PositionItem::PositionItem( const AMG::Vector& vector, const QString & name
  , const QString & comment, TreeItem* parent )
  :TreeItem(parent)
  ,position(vector)
  ,name(name)
  ,comment(comment)
  ,type()
{
  x = new PodItem<const double>(position.coords(0),"x","m", QObject::tr("North (wrt. Datum)"));
  appendChild(x);

  y = new PodItem<const double>(position.coords(1),"y","m", QObject::tr("East (wrt. Datum)"));
  appendChild(y);

  z = new PodItem<const double>(position.coords(2),"z","m", QObject::tr("Down (wrt. Datum)"));
  appendChild(z);
}

PositionItem::~PositionItem()
{
  delete x;
  x = nullptr;

  delete y;
  y = nullptr;

  delete z;
  z = nullptr;
}

QVariant PositionItem::data ( int column, int role ) const
{
  if( role == Qt::DisplayRole )
  {
    switch( column )
    {
      case static_cast<int>(DataAspect::name) : return name;
      case static_cast<int>(DataAspect::value) : return QVariant(relativePositionString());
      case static_cast<int>(DataAspect::comment) : return comment;
      case static_cast<int>(DataAspect::type) : return QVariant();
      default: return QVariant("(default)");
    }
  }
  //else
  return QVariant();
}

QString PositionItem::geodeticPositionString() const
{
  return QString(AMG::GIS::geodeticPositionString(position).c_str());
}

QString PositionItem::relativePositionString() const
{
  AMG::Vector posVector = position;
  const double & x = posVector.coords(0);
  const double & y = posVector.coords(1);

  double distance2d_xy = std::sqrt(x*x+y*y);

  double heading_rad = std::atan2(y,x); // heading_rad is in (-pi,pi]
  double heading_deg = AMG::Units::radian2degree(heading_rad); // heading_deg is in (-180,180]

  QString compassPoint = AMG::GIS::convertToString(AMG::GIS::headingToCompassPoint(heading_deg)).c_str();

  return QString("%1 m %2 (%3°)")
          .arg(distance2d_xy,0,'f',0)
          .arg(compassPoint)
          .arg(heading_deg,0,'f',0);

}


//=======================================================================
// Copyright 2015-2018 Clemson University
// Authors: Bradley S. Meyer
//
// 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)
//=======================================================================

// This test first creates a complete graph or multigraph with n vertices.
// Each arc is assigned a random integer, double, or defined class
// weight.  A root vertex is added and arcs with zero weight
// are added from the root vertex to each of the other vertices.  The test
// then constructs all potential branchings and discards those
// that have indegree for any vertex greater than one or contain
// a cycle.  The valid branchings are then stored in a priority
// queue.  The test then
// uses the rank_spanning_branchings.hpp routine to construct all
// branchings, in order, and places them in a vector.  The vector
// and the priority queue are then compared, allowing for the possibility
// of equal weight branchings.

#include <iostream>
#include <vector>
#include <complex>

#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/heap/fibonacci_heap.hpp>
#include <boost/heap/pairing_heap.hpp>
#include <boost/heap/skew_heap.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/assert.hpp>

#include "../include/rank_spanning_branchings.hpp"

using namespace boost;

typedef random::mt19937 base_generator_type;

template<typename T, typename U>
class my_type
{
  public:

    my_type(){}
    my_type( T _t, U _u ) : t( _t ), u( _u ) {}

    my_type& operator+=( const my_type& rhs )
    {
      this->t += rhs.t;
      this->u += rhs.u;
      return *this;
    }

    friend my_type operator+( my_type lhs, const my_type& rhs )
    {
      lhs += rhs;
      return lhs;
    }

    my_type& operator-=( const my_type& rhs )
    {
      this->t -= rhs.t;
      this->u -= rhs.u;
      return *this;
    }

    friend my_type operator-( my_type lhs, const my_type& rhs )
    {
      lhs -= rhs;
      return lhs;
    }

    bool operator!=( const my_type& rhs )
    {
      return !( ( this->t == rhs.t ) && ( this->u == rhs.u ) );
    }

    bool operator<( const my_type& rhs ) const
    {
      if( this->t == rhs.t )
       return this->u < rhs.u;
      else
       return this->t < rhs.t;
    }

    friend std::ostream& operator<<( std::ostream& os, const my_type& p )
    {
      os << "(" << p.t << ", " << p.u << ")";
      return os;
    }

  private:
    T t;
    U u;

};

template<class Graph, class Edge>
struct Branching
{
  typedef
    typename
    property_traits<
      typename property_map<Graph, edge_weight_t>::const_type
    >::value_type weight_t;
  weight_t weight;
  std::set<Edge>  v_edges;
  Branching( weight_t & w, const std::set<Edge>& v_e) :
    weight( w ), v_edges( v_e ){}
};

template<class Graph, class Edge, class Compare>
struct compare_branchings
{
  Compare comp;
  compare_branchings( Compare _comp ) : comp( _comp ){}
  bool operator()(
    const Branching<Graph,Edge>& b1,
    const Branching<Graph,Edge>& b2 ) const
  {
    return comp( b1.weight, b2.weight );
  }
};

template<class Graph, class Edge>
struct set_rank_vector
{

  std::vector<Branching<Graph,Edge> >& rank_vector;
  set_rank_vector(
    std::vector<Branching<Graph,Edge> >& rv
  ) : rank_vector( rv ) {}

  template<class BranchingGraph>
  bool operator()( BranchingGraph& bg )
  {

    std::set<Edge> branching;

    typedef
      typename
        property_map<BranchingGraph, edge_weight_t>::const_type weight_map_t;
    weight_map_t w;
    typedef typename property_traits<weight_map_t>::value_type weight_t;

    scoped_ptr<weight_t> weight;
    weight.reset();

    BGL_FORALL_EDGES_T( e, bg, BranchingGraph )
    {
      if( !weight )
      {
        weight.reset( new weight_t );
        *weight = get( w, e );
      }
      else
      {
        *weight += get( w, e );
      }
      branching.insert( e );
    }

    rank_vector.push_back(Branching<Graph,Edge>( *weight, branching ) );

    return true;

  }

};

template<class Graph, class Edge, class Rank, class Parent>
void
set_branching(
  Graph& g,
  const std::vector<Edge>& v_edges,
  const std::vector<size_t>& combination,
  Rank rank,
  Parent parent,
  std::vector<Branching<Graph,Edge> >& branchings
)
{
  disjoint_sets<Rank, Parent> W( rank, parent );
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  std::set<Vertex> in_vertex_set;
  typedef typename property_map<Graph, edge_weight_t>::const_type weight_map_t;
  typedef typename property_traits<weight_map_t>::value_type weight_t;
  std::set<Edge> branching_edges;

  BGL_FORALL_VERTICES_T( v, g, Graph )
  {
    W.make_set( v );
  }

  BOOST_FOREACH( size_t i, combination )
  {
    Edge e = v_edges[i];
    if( in_vertex_set.find( target( e, g ) ) != in_vertex_set.end() ) return;
    if( W.find_set( source( e, g ) ) == W.find_set( target( e, g ) ) ) return;
    W.union_set( source( e, g ), target( e, g ) );
    in_vertex_set.insert( target( e, g ) );
  }

  scoped_ptr<weight_t> weight;
  weight.reset();

  BOOST_FOREACH( size_t i, combination )
  {
    Edge e = v_edges[i];
    if( !weight )
    {
      weight.reset( new weight_t );
      *weight = get( edge_weight, g, e );
    }
    else
    {
      *weight += get( edge_weight, g, e );
    }
    branching_edges.insert( e );
  }
  branchings.push_back( Branching<Graph,Edge>( *weight, branching_edges ) );
}

template<class Graph, class Edge>
void
loop(
  size_t offset,
  size_t k,
  Graph& g,
  const std::vector<Edge>& v_edges,
  const std::vector<size_t>& indices,
  std::vector<size_t>& combination,
  std::vector<Branching<Graph,Edge> >& branchings
)
{

  typedef typename graph_traits<Graph>::vertices_size_type size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  size_type n = num_vertices( g );
  std::vector<size_type> rank_map( n );
  std::vector<vertex_t> pred_map( n );

  if( k == 0 )
  {
    set_branching(
      g,
      v_edges,
      combination,
      make_iterator_property_map(
        rank_map.begin(),
        get(vertex_index, g),
        rank_map[0]
      ),
      make_iterator_property_map(
        pred_map.begin(),
        get(vertex_index, g),
        pred_map[0]
      ),
      branchings
    );
    return;
  }
  for (size_t i = offset; i <= indices.size() - k; ++i) {
    combination.push_back(indices[i]);
    loop(
      i+1,
      k-1,
      g,
      v_edges,
      indices,
      combination,
      branchings
    );
    combination.pop_back();
  }
}

template<class Graph, class Edge, class Compare>
bool
compare_heap_and_vector(
  Graph& g,
  heap::fibonacci_heap<
    Branching<Graph,Edge>,
    heap::compare<Compare>
  >& branching_heap,
  std::vector<Branching<Graph,Edge> >& rank_vector
)
{

  // Compare branchings in heap and ranked vector.  Allow for the possibility
  // that two branchings might have the same weight (hence the second loop).

  typename property_map<Graph, vertex_index_t>::type index_map =
    get( vertex_index, g );

  bool found;

  std::set<size_t> check;

  for( size_t i = 0; i < rank_vector.size(); i++ )
  {
    check.insert( i );
  }

  while( !branching_heap.empty() )
  {

    for(
      std::set<size_t>::iterator it = check.begin();
      it != check.end();
      it++
    )
    {

      found = true;

      if( rank_vector[*it].weight != branching_heap.top().weight )
      {
        found = false;
        break;
      }

      BOOST_FOREACH( Edge e, branching_heap.top().v_edges )
      {
        if(
          rank_vector[*it].v_edges.find( e ) == rank_vector[*it].v_edges.end()
        )
        {
          found = false;
        }
      }

      if( found )
      {
        check.erase( it );
        break;
      }

    }

    if( !found )
    {
      std::cerr << "Error: " << std::endl;
      BOOST_FOREACH( Edge e, branching_heap.top().v_edges )
      {
        std::cerr <<
        " (" << index_map[source( e, g )] <<
        "," << index_map[target( e, g )] << ") ";
      }
      std::cerr << " Weight: " << branching_heap.top().weight << std::endl;
        std::cerr << std::endl;
      return false;
    }
    branching_heap.pop();
  }

  return true;

}

template<class Compare>
void test_int_graph( base_generator_type& gen, size_t n, Compare comp )
{

  typedef adjacency_list < listS, setS, directedS,
     property < vertex_index_t, size_t >, property < edge_weight_t, int > >
     Graph;
  typedef property_map < Graph, vertex_index_t >::type index_map_t;
  typedef graph_traits <Graph>::edge_descriptor Edge;

  std::vector<size_t> indices;
  std::vector<size_t> combination;
  std::vector<Edge> v_edges;
  std::vector<Branching<Graph,Edge> > branchings, rank_vector;

  typedef
    heap::fibonacci_heap<
      Branching<Graph,Edge>,
      heap::compare<compare_branchings<Graph,Edge,Compare> >
    > branching_heap_t;

  branching_heap_t branching_heap( comp );

  random::uniform_int_distribution<> tdist( -10, 10 );

  variate_generator<
    base_generator_type&, random::uniform_int_distribution<>
  > rvt( gen, tdist );

  Graph g;

  for( size_t i = 0; i <= n; i++ )
  {
    add_vertex( g );
  }

  // Set index map.

  index_map_t index_map = get( vertex_index, g );

  size_t j = 0;
  BGL_FORALL_VERTICES( v, g, Graph )
  {
    put( index_map, v, j++ );
  }

  // Add edges.

  BGL_FORALL_VERTICES( v, g, Graph )
  {
    BGL_FORALL_VERTICES( u, g, Graph )
    {
      if( u != v )
      {
        if( index_map[v] != 0 )
        {
          if( index_map[u] != 0 )
          {
            add_edge( v, u, rvt(), g );
          }
        }
        else
        {
          add_edge( v, u, 0, g );
        }
      }
    }
  }

  // Create vectors.

  BGL_FORALL_EDGES( e, g, Graph )
  {
    v_edges.push_back( e );
  }

  for( size_t i = 0; i < v_edges.size(); ++i )
  {
    indices.push_back(i);
  }

  loop(
    0,
    num_vertices(g) - 1,
    g,
    v_edges,
    indices,
    combination,
    branchings
  );

  for( size_t i = 0; i < branchings.size(); i++ )
  {
    branching_heap.push( branchings[i] );
  }

  rsb::rank_spanning_branchings<boost::heap::pairing_heap>(
    g,
    set_rank_vector<Graph,Edge>( rank_vector ),
    distance_compare( comp )
  );

  // Check that number of branchings found is correct.

  assert(
    pow(
      static_cast<double>( n + 1 ),
      static_cast<double>( n - 1 )
    )
    ==
    rank_vector.size()
  );

  assert( branching_heap.size() == rank_vector.size() );

  // Compare branchings found by both methods.

  if( !compare_heap_and_vector( g, branching_heap, rank_vector ) )
  {
    exit( EXIT_FAILURE );
  }

}

template<class Compare>
void test_real_graph( base_generator_type& gen, size_t n, Compare comp )
{

  typedef adjacency_list < vecS, vecS, directedS,
     no_property, property < edge_weight_t, double > > Graph;
  typedef graph_traits <Graph>::edge_descriptor Edge;

  std::vector<size_t> indices;
  std::vector<size_t> combination;
  std::vector<Edge> v_edges;
  std::vector<Branching<Graph,Edge> > branchings, rank_vector;

  typedef
    heap::fibonacci_heap<
      Branching<Graph,Edge>,
      heap::compare<compare_branchings<Graph,Edge,Compare> >
    > branching_heap_t;

  branching_heap_t branching_heap( comp );

  boost::random::uniform_real_distribution<> tdist( -10, 10 );

  boost::variate_generator<
    base_generator_type&, boost::random::uniform_real_distribution<>
  > rvt( gen, tdist );

  Graph g;

  for( size_t i = 0; i < n; i++ )
  {
    for( size_t j = 0; j < n; j++ )
    {
      if( i != j )
      {
        add_edge( i, j, rvt(), g );
        add_edge( i, j, rvt(), g );    // A parallel edge.
      }
    }
    add_edge( n, i, 0, g );
  }

  BGL_FORALL_EDGES( e, g, Graph )
  {
    v_edges.push_back( e );
  }

  for( size_t i = 0; i < v_edges.size(); ++i )
  {
    indices.push_back(i);
  }

  loop(
    0,
    num_vertices(g) - 1,
    g,
    v_edges,
    indices,
    combination,
    branchings
  );

  for( size_t i = 0; i < branchings.size(); i++ )
  {
    branching_heap.push( branchings[i] );
  }

  // Call explicitly with named parameters to test.  Also use a skew_heap
  // in routine.

  rsb::rank_spanning_branchings<boost::heap::skew_heap>(
    g,
    set_rank_vector<Graph, Edge>( rank_vector ),
    distance_compare( comp ).
    weight_map( get( edge_weight, g ) ).
    vertex_index_map( get( vertex_index, g ) )
  );

  // Check that number of branchings found is correct.  The formula
  // accounts for the parallel edges.

  assert(
    pow(
      static_cast<double>( 2*n + 1 ),
      static_cast<double>( n - 1 )
    )
    ==
    rank_vector.size()
  );

  assert( branchings.size() == rank_vector.size() );

  // Compare branchings found by both methods.

  if( !compare_heap_and_vector( g, branching_heap, rank_vector ) )
  {
    exit( EXIT_FAILURE );
  }

}

template<class Compare>
void test_class_graph( base_generator_type& gen, size_t n, Compare comp )
{

  typedef adjacency_list < vecS, vecS, directedS,
     no_property, property < edge_weight_t, my_type<double, double > >
  > Graph;
  typedef graph_traits <Graph>::edge_descriptor Edge;

  std::vector<size_t> indices;
  std::vector<size_t> combination;
  std::vector<Edge> v_edges;
  std::vector<Branching<Graph,Edge> > branchings, rank_vector;

  typedef
    heap::fibonacci_heap<
      Branching<Graph,Edge>,
      heap::compare<compare_branchings<Graph,Edge,Compare> >
    > branching_heap_t;

  branching_heap_t branching_heap( comp );

  boost::random::uniform_real_distribution<> tdist( -10, 10 );

  boost::variate_generator<
    base_generator_type&, boost::random::uniform_real_distribution<>
  > rvt( gen, tdist );

  Graph g;

  for( size_t i = 0; i < n; i++ )
  {
    for( size_t j = 0; j < n; j++ )
    {
      if( i != j )
      {
        add_edge( i, j, my_type<double, double>( rvt(), rvt() ), g );
      }
    }
    add_edge( n, i, my_type<double, double>( 0, 0 ), g );
  }

  BGL_FORALL_EDGES( e, g, Graph )
  {
    v_edges.push_back( e );
  }

  for( size_t i = 0; i < v_edges.size(); ++i )
  {
    indices.push_back(i);
  }

  loop(
    0,
    num_vertices(g) - 1,
    g,
    v_edges,
    indices,
    combination,
    branchings
  );

  for( size_t i = 0; i < branchings.size(); i++ )
  {
    branching_heap.push( branchings[i] );
  }

  // Call explicitly with named parameters to test.

  rsb::rank_spanning_branchings(
    g,
    set_rank_vector<Graph, Edge>( rank_vector ),
    distance_compare( comp ).
    weight_map( get( edge_weight, g ) ).
    vertex_index_map( get( vertex_index, g ) )
  );

  // Check that number of branchings found is correct.

  assert(
    pow(
      static_cast<double>( n + 1 ),
      static_cast<double>( n - 1 )
    )
    ==
    rank_vector.size()
  );

  assert( branchings.size() == rank_vector.size() );

  // Compare branchings found by both methods.

  if( !compare_heap_and_vector( g, branching_heap, rank_vector ) )
  {
    exit( EXIT_FAILURE );
  }

}

int main( int argc, char **argv )
{

  if( argc != 3 )
  {
    std::cerr << std::endl;
    std::cerr << "  Usage: " << argv[0] << " n" << std::endl << std::endl;
    std::cerr << "    n = number of vertices in complete graph" <<
                 std::endl << std::endl;
    std::cerr << "    k = number of trials" << std::endl << std::endl;
    return EXIT_FAILURE;
  }

  base_generator_type gen;
  gen.seed( (unsigned int) time( NULL ) );

  for( size_t i = 0; i < lexical_cast<size_t>( argv[2] ); i++ )
  {
    test_int_graph(
      gen, lexical_cast<size_t>( argv[1] ) + 1, std::greater<int>()
    );
    test_real_graph(
      gen, lexical_cast<size_t>( argv[1] ), std::less<double>()
    );
    test_class_graph(
      gen,
      lexical_cast<size_t>( argv[1] ) + 1,
      std::less<my_type<double, double> >()
    );
  }

  std::cout << "tests passed" << std::endl;

  return EXIT_SUCCESS;

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

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

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

#include <test_common/test_point.hpp>

int test_main(int, char* [])
{
    using namespace bg::strategy::distance;

    bg::model::ll::point<bg::degree> paris;
    paris.lat(bg::dms<bg::north>(48, 52, 0));
    paris.lon(bg::dms<bg::east>(2, 19, 59));

    bg::model::ll::point<bg::degree> amsterdam;
    amsterdam.lat(bg::dms<bg::north>(52, 22, 23));
    amsterdam.lon(bg::dms<bg::east>(4, 53, 32));

    bg::model::ll::point<bg::radian> paris_rad, amsterdam_rad;
    transform(amsterdam, amsterdam_rad);
    transform(paris, paris_rad);

    double d1 = 0.001 * distance(paris, amsterdam);
    double d2 = 0.001 * distance(paris_rad, amsterdam_rad);

    double d3 = 0.001 * distance(paris, amsterdam_rad);
    double d4 = 0.001 * distance(paris_rad, amsterdam);
    std::cout << "Distances: " << d1 << " == " << d2 << " == " << d3 << " == " << d4 << std::endl;

    double d5 = 0.001 * distance(paris, amsterdam, vincenty<bg::model::ll::point<bg::degree>, bg::model::ll::point<bg::degree> >());
    double d6 = 0.001 * distance(paris_rad, amsterdam_rad, vincenty<bg::model::ll::point<bg::radian>, bg::model::ll::point<bg::radian> >());
    double d7 = 0.001 * distance(paris, amsterdam_rad, vincenty<bg::model::ll::point<bg::degree>, bg::model::ll::point<bg::radian> >());
    double d8 = 0.001 * bg::distance(paris_rad, amsterdam, vincenty<bg::model::ll::point<bg::radian>, bg::model::ll::point<bg::degree> >());
    std::cout << "Distances: " << d5 << " == " << d6 << " == " << d7 << " == " << d8 << std::endl;

    bg::model::ll::point<bg::degree> barcelona(
        bg::latitude<>(bg::dms<bg::north>(41, 23)),
        bg::longitude<>(bg::dms<bg::east>(2, 11))
        );

    // Now declare a line in latlong and calculate the distance, this MUST reverse...
    bg::model::linestring<bg::model::ll::point<bg::degree> > ab;
    ab.push_back(amsterdam);
    ab.push_back(barcelona);

    double d9 = 0.001 * distance(ab, paris);
    double d10 = 0.001 * distance(paris, ab);
    double d11 = 0.001 * distance(paris, ab, vincenty<bg::model::ll::point<bg::degree>, bg::model::ll::point<bg::degree> >());
    double d12 = 0.001 * distance(ab, paris, vincenty<bg::model::ll::point<bg::degree>, bg::model::ll::point<bg::degree> >());
    std::cout << "Distances: " << d9 << " == " << d10 << " == " << d11 << " == " << d12 << std::endl;

    // TODO: solve this case, it is reversed -> strategy should be reversed as well
    // For this, first implement that linestring can have other coor.sys then point type...
    //double d13 = 0.001 * distance(ab, paris_rad, vincenty<ll::point<bg::degree>, ll::point<bg::radian> >());

    return 0;
}


#include "hash_functions.h"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <openssl/md5.h>
#include <iostream>
// #include <boost/iostreams/device/mapped_file.hpp>
unsigned char result[MD5_DIGEST_LENGTH];

using namespace std;
string get_file_hash(string file_path) {
  std::ifstream file(file_path, std::ifstream::binary);
  MD5_CTX md5Context;
  MD5_Init(&md5Context);
  char buf[1024 * 16];
  while (file.good()) {
      file.read(buf, sizeof(buf));
      MD5_Update(&md5Context, buf, file.gcount());
  }
  unsigned char result[MD5_DIGEST_LENGTH];
  MD5_Final(result, &md5Context);

  std::stringstream md5string;
  md5string << std::hex << std::uppercase << std::setfill('0');
  for (const auto &byte: result)
    md5string << std::setw(2) << (int)byte;

  return md5string.str();
}


#ifndef XML_HPP
#define XML_HPP

#include <iostream>
#include <armadillo>
#include <string>
#include <vector>
#include <memory>

#include "rapidxml.hpp"
#include "Reader.hpp"
#include "debug.hpp"

class XML : public Reader {
  public:
  // protected:
    std::string fileName;

    double lx;
    double ly;
    double lz;
    arma::Mat<int> mol;
    arma::Mat<float> pos;
    arma::Mat<float> vel;
    arma::Mat<int> img;
    std::vector<std::string> type;
    int natoms;
    int frame;

  // public:
    typedef std::shared_ptr<XML> ptr;

    // derived virtuals
    void readFrame(int frame) {
        std::cout << "XML File is read in constructor! Nothing to read!" << std::endl;
    };
    void printFileInfo();
    arma::Mat<float> getPositions() {return pos;};
    double getLx() {return lx;};
    double getLy() {return ly;};
    double getLz() {return lz;};
    // std::vector<std::string> getTypes() {return type;};

    XML(std::string) ;
    void read();
    void parse();
    void setFilename(std::string);
    int parse_int(char const*,char const*);
    double parse_double(char const*,char const*);
    float parse_float(char const*,char const*);

    std::shared_ptr<char> xmlCharArray;
    bool has_read;
    rapidxml::xml_document<> xmlFile;

    struct IsEmptyStr {
      bool operator()(const std::string& s) {
        return s.find("", 0) != std::string::npos;
      }
    };

    template<typename T>
    void parse_node(std::string& data_str, arma::Mat<T>& array, std::string type) {
      char const* delims = "\n ";
      int offset =0;
      if (data_str[0]=='\n') { offset = 1; }
      char const* val_start = data_str.c_str()+offset;
      char const* val_end = strpbrk(val_start+1, delims);
      int row=0; int col=0;
      for( ;val_end!= NULL; val_end=strpbrk(val_start,delims)) {
        if (row>=array.n_rows) {
          std::cerr << "Attempting to insert data past array size!" << std::endl;
          std::cerr << "Array  size: " << array.n_rows << std::endl;
          LOC();
          exit(EXIT_FAILURE);
        }

        if (type=="double") {
          array(row,col) = parse_double(val_start,val_end);
        } else if (type=="float") {
          array(row,col) = parse_float(val_start,val_end);
        } else if (type=="int") {
          array(row,col) = parse_int(val_start,val_end);
        } else {
          std::cerr << "Type specification for conversion not recognized!" << std::endl;
          std::cerr << "Type  spec: " << type << std::endl;
          LOC();
          exit(EXIT_FAILURE);
        }

        // std::cout << "(row,col): (" << row << "," << col << ") ";
        // std::cout << "val: " << array(row,col) << std::endl;
        val_start = val_end+1;
        col+=1;
        if (col>=array.n_cols) { col=0; row+=1; }
      }
    }
};
#endif


#include <string>     // to use std::string, std::to_string() and "+" operator acting on strings
#include <iostream>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <fstream>

#include <z3++.h>

using namespace z3;

struct GraphData { std::string Name; };
struct VertexProperty { std::string Name; };
struct EdgeProperty { std::string Name; };

using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS, VertexProperty, EdgeProperty, GraphData>;

Graph readGraph(std::string file_name);
void solveMinimum(Graph g);

int main (int argc, char * const argv[]) {

  Graph g = readGraph("graph.xml");

  solveMinimum(g);

	return 0;
}

void solveMinimum(Graph g) {
    context c;
    optimize opt(c);
    params p(c);
    p.set("priority",c.str_symbol("pareto"));
    opt.set(p);

    std::vector<expr> colors = {};
    expr minColor = c.int_const("minColor");
    optimize::handle handleMinColor = opt.minimize(minColor);

    auto name = get(&VertexProperty::Name, g);
    auto vs = boost::vertices(g);

    for (auto vit = vs.first; vit != vs.second; ++vit) {
        std::string colorName = std::string(name[*vit]) + "_color";
        char* charColorName = new char [colorName.length()+1];
        std::strcpy (charColorName, colorName.c_str());

        expr color = c.int_const(charColorName);
        colors.push_back(color);

        opt.add(color >= 0);
        opt.add(color < minColor);
    }

    for (auto vit = vs.first; vit != vs.second; ++vit) {

        expr colorNode = colors[*vit];

        auto neighbors = boost::adjacent_vertices(*vit, g);
        for (auto nit = neighbors.first; nit != neighbors.second; ++nit){
            expr colorNeighbor = colors[*nit];
            opt.add(colorNode != colorNeighbor);
        }
    }

    if (sat == opt.check()) {
        std::cout << minColor << ": " << opt.lower(handleMinColor) << std::endl;
    }
}

Graph readGraph(std::string file_name){

  // attach an input stream to the wanted file
  std::ifstream input_stream(file_name);

  // check stream status
  if (!input_stream) std::cerr << "Can't open input file!";

  Graph graph;
  boost::dynamic_properties dp(boost::ignore_other_properties);
  dp.property("Name", boost::get(&VertexProperty::Name, graph));

  boost::read_graphml(input_stream, graph, dp);

  return graph;
}


#include <armadillo>

#include "ema/Ema.h"
#include "ema-modify/ApplyModifications.h"

#include "settings.h"
#include "Tracker.h"
#include "Weights.h"
#include "mesh/MeshIO.h"
#include "mesh/Mesh.h"
#include "model/ModelReader.h"

int main(int argc, char* argv[]) {

  Settings settings(argc, argv);

  Ema ema;

  ema.read().from(settings.inputFileName);

  // reduce ema data to selected coil subset
  ema.reduce_coil_set().to(settings.channels);

  if(settings.applyModifications == true) {

    emaModify::ApplyModifications(ema).apply(settings.emaModifications);

  }

  ModelReader modelReader(settings.model);
  Model model = modelReader.get_model();
  TrackerData trackerData(model, settings);
  trackerData.sourceIds = settings.sourceIds;

  Tracker tracker(trackerData);
  tracker.update().for_source_ids();

  // fix speaker and create PCA model if desired
  if( settings.fixSpeaker ) {

    const arma::vec speakerWeights = Weights::read(settings.fixedSpeakerWeights);
    tracker.update().to_fixed_speaker(speakerWeights);

  }

  for(int i = 0; i < ema.info().sample_amount(); ++i) {

    // update data for current time frame
    tracker.data().target = ema.point_cloud().from(settings.channels, i);
    tracker.data().currentTime = ema.info().time_stamps().at(i);

    tracker.fitting().fit();

  }

  Json::Value loggedData = tracker.logger().get_logged_data();

  std::ofstream outFile(settings.outputFileName);

  outFile << loggedData << std::endl;

  outFile.close();

  return 0;

}