/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <algorithm>
#include <iostream>

#include <boost/random/normal_distribution.hpp>
#include <boost/math/special_functions/bessel.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/sphere.hpp>

#define LOG_2 0.69314718055994529
#define LOG_PI 1.1447298858494002
#define LOG_2PI 1.8378770664093453
#define LOG_4PI 2.5310242469692907

using namespace Eigen;
using std::cout;
using std::endl;
using std::min;

template<typename T>
inline T logBesselI(T nu, T x)
{
  // for large values of x besselI \approx exp(x)/sqrt(2 PI x)
  if(x>100.)  return x - 0.5*LOG_2PI -0.5*log(x);
  return log(boost::math::cyl_bessel_i(nu,x));

};

template<typename T>
inline T logxOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 0.;
  else
    return log(x)-log(sinh(x));
}
template<typename T>
inline T xOverSinhX(T x) {
  if (fabs(x) < 1e-9)
    return 1.;
  else
    return x/sinh(x);
}
template<typename T>
inline T xOverTanPiHalfX(T x) {
  if (fabs(x) < 1e-9)
    return 2./M_PI;
  else
    return x/tan(x*M_PI*0.5);
}

template <typename T>
inline T MLEstimateTau(const Eigen::Matrix<T,3,1>& xSum, const
    Eigen::Matrix<T,3,1>& mu, T count) {
  // Need double precision to achive convergence; single is not enough.
  double tau = 1.0;
  double prevTau = 0.;
  double eps = 1e-8;
  double R = xSum.norm()/count;
  while (fabs(tau - prevTau) > eps) {
//    std::cout << "tau " << tau << " R " << R << std::endl;
    double inv_tanh_tau = 1./tanh(tau);
    double inv_tau = 1./tau;
    double f = -inv_tau + inv_tanh_tau - R;
    double df = inv_tau*inv_tau - inv_tanh_tau*inv_tanh_tau + 1.;
    prevTau = tau;
    tau -= f/df;
  }
  return tau;
};

/* von-Mises-Fisher distribution in D=3 dimensions
 */
template<typename T>
class vMF : public Distribution<T>
{
public:
  uint32_t  D_;

  vMF(const Matrix<T,Dynamic,1>& mu, T tau, boost::mt19937 *pRndGen);
  vMF(const vMF<T>& vmf);
  ~vMF();

  T logPdf(const Matrix<T,Dynamic,1>& x) const;

  Matrix<T,Dynamic,1> sample();

  void print() const;

  const Matrix<T,Dynamic,1>& mu() const {return mu_;};
  void mu(const Matrix<T,Dynamic,1>& mu) const {mu_ = mu;};

  T tau() const {return tau_;};
  void tau(const T tau) {tau_ = tau;};

  Matrix<T,Dynamic,1> mu_;
  T tau_;
private:

// Gaussian as a proposal distribution
  boost::mt19937 *pRndGen_;
  boost::uniform_01<> unif_;
  boost::normal_distribution<> gauss_;
};

typedef vMF<double> vMFd;
typedef vMF<float> vMFf;

template<typename T>
vMF<T>::vMF(const Matrix<T,Dynamic,1>& mu, T tau, boost::mt19937 *pRndGen)
  : Distribution<T>(pRndGen), D_(mu.rows()), mu_(mu), tau_(tau),
  pRndGen_(pRndGen), gauss_(0,1)
{};

template<typename T>
vMF<T>::vMF(const vMF<T>& vmf)
  : Distribution<T>(vmf.pRndGen_), D_(vmf.D_), mu_(vmf.mu()), tau_(vmf.tau()),
    pRndGen_(vmf.pRndGen_), gauss_(0,1)
{};

template<typename T>
vMF<T>::~vMF()
{};

template<typename T>
T vMF<T>::logPdf(const Matrix<T,Dynamic,1>& x) const
{
  // modified bessel function of the first kind
  // http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/mbessel.html
  //
  const T d = static_cast<T>(D_);
  if (tau_ < 1e-9) {
    // TODO insert general formula here (this currently works only
    // for D=3
    assert(D_==3);
    return -LOG_4PI;
  } else {
    if (D_ == 3) {
      return -LOG_2PI + log(tau_) + tau_*(mu_.dot(x)-1.) -
        log(1.-exp(-2.*tau_));
    }else {
      return (d/2. -1.)*log(tau_) - (d/2.)*LOG_2PI
        - logBesselI<T>(d/2. -1.,tau_) + tau_*mu_.dot(x);
    }
  }
};

template<typename T>
Matrix<T,Dynamic,1> vMF<T>::sample()
{
  assert(D_==3);
  if (tau_ < 1e-10) {
//    Eigen::VectorXf x;
//    x << gauss_(*pRndGen_), gauss_(*pRndGen_), gauss_(*pRndGen_);
//    return x.normalized();
    return Eigen::Matrix<T,3,1>(gauss_(*pRndGen_), gauss_(*pRndGen_), gauss_(*pRndGen_)).normalized();
  }
  // https://www.mitsuba-renderer.org/~wenzel/files/vmf.pdf
  // sample around (0,0,1)
  Eigen::Matrix<T,2,1> v(gauss_(*pRndGen_), gauss_(*pRndGen_));
  v.normalize();
  const T u = unif_(*pRndGen_);
  const T w = 1. + log(u+(1.-u)*exp(-2.*tau_))/tau_;
  const T a = sqrtf(1.-w*w);
  Eigen::Matrix<T,3,1> x(a*v(0), a*v(1), w);

  // rotate to mu
  Eigen::Matrix<T,3,1> axis = Eigen::Matrix<T,3,1>(0,0,1).cross(Eigen::Matrix<T,3,1>(mu_(0),mu_(1),mu_(2)));
  T angle = acos(mu_[2]);

  if (fabs(angle) <1e-9)
    return x;

  Eigen::Quaternion<T> q(cos(angle*0.5),
      sin(angle*0.5)*axis(0)/axis.norm(),
      sin(angle*0.5)*axis(1)/axis.norm(),
      sin(angle*0.5)*axis(2)/axis.norm());
  Eigen::Matrix<T,Eigen::Dynamic,1> xx = q._transformVector(x);
  return xx;
};

template<typename T>
void vMF<T>::print() const
{
  cout<<"mu = "<<mu_.transpose()<<"\t tau = "<<tau_<<endl;
};


#ifndef MLT_MODELS_LINEAR_MODEL_HPP
#define MLT_MODELS_LINEAR_MODEL_HPP

#include <Eigen/Core>

#include "base.hpp"
#include "../utils/linear_algebra.hpp"

namespace mlt {
namespace models {
	using namespace utils::linear_algebra;

	template <class BaseModelType>
	class LinearModel : public BaseModelType {
	public:
		inline auto fit_intercept() const { return _fit_intercept; }

		inline const auto coefficients() const { assert(_fitted); return _fit_intercept ? _coefficients.leftCols(_coefficients.cols() - 1).eval() : _coefficients; }

		inline const auto intercepts() const { assert(_fitted && _fit_intercept); return _coefficients.rightCols<1>().eval(); }

		inline const auto all_coefficients() const { assert(_fitted); return _coefficients; }

	protected:
		LinearModel(bool fit_intercept) : _fit_intercept(fit_intercept) {}

		inline void _set_coefficients(MatrixXdRef coefficients) {
			_coefficients = coefficients;
			_fitted = true;
		}

		inline auto _apply_linear_transformation(Features input) const {
			assert(_fitted);

			if (_fit_intercept) {
				return linear_transformation(input, _coefficients.leftCols(_coefficients.cols() - 1), _coefficients.rightCols<1>());
			}

			return linear_transformation(input, _coefficients);
		}

		inline auto _apply_linear_transformation(Features input, MatrixXdRef coeffs) const {
			if (_fit_intercept) {
				return linear_transformation(input, coeffs.leftCols(coeffs.cols() - 1), coeffs.rightCols<1>());
			}

			return linear_transformation(input, coeffs);
		}

		const bool _fit_intercept;

	private:
		MatrixXd _coefficients;
	};
}
}

#endif


#pragma once

#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <cmath>

namespace LRR
{
  namespace Math
  {
    using namespace Eigen;

    template <typename T>
    T ToRadians(T deg) { return deg * T(0.01745329251f); }

    template <typename T>
    T ToDegrees(T rad) { return rad * T(57.2957795131f); }

    Matrix4f PerspectiveProjection(float fovy, float aspect, float zNear, float zFar);

    Matrix4f RotationMatrix(AngleAxisf const &rotation);
  }
}


#include <stdlib.h>     // srand, rand
#include <unistd.h>	// sleep
#include <armadillo>
#include <boost/math/special_functions/digamma.hpp>

#include "Sto_IHT.h"
#include "Functions.h"

using namespace arma;
using namespace std;

vec expected_ln_beta_dist(const vec pos_count, const vec neg_count){
// The logarithm of the geometric mean GX of a distribution with random variable X is the arithmetic mean of ln(X)
//https://en.wikipedia.org/wiki/Beta_distribution#Geometric_mean
vec ln_GX(pos_count.n_elem,fill::zeros);
for (unsigned int n = 0; n < ln_GX.n_elem; n++){
	ln_GX(n) = boost::math::digamma(pos_count(n)) - boost::math::digamma(pos_count(n) + neg_count(n));

}
return ln_GX;
}

double expected_ln_beta_dist(const double pos_count, const double neg_count){
// The logarithm of the geometric mean GX of a distribution with random variable X is the arithmetic mean of ln(X)
//https://en.wikipedia.org/wiki/Beta_distribution#Geometric_mean
double ln_GX = boost::math::digamma(pos_count) - boost::math::digamma(pos_count + neg_count);

return ln_GX;
}

ivec generate_support(const uvec &curr_supp,const uvec &prev_supp,const int sig_dim){
	ivec supp_data(sig_dim,fill::ones);
	supp_data *= -1;

	uvec mask_old(sig_dim,fill::zeros);	// mask for the previous support
	mask_old(prev_supp) = ones<uvec>(prev_supp.n_elem);

	uvec mask_new(sig_dim,fill::zeros);	// mask for the current support
	mask_new(curr_supp) = ones<uvec>(curr_supp.n_elem);

	const uvec zero_indices = find(mask_old%(1 - mask_new));
	const uvec one_indices  = find(mask_new%(1 - mask_old)) ;
	supp_data.elem( zero_indices ).zeros();
	supp_data.elem( one_indices ).ones();

	return supp_data;
}

void update_tally_bayesian(vec &pos_count, vec &neg_count, double &reliability_pos,
	double &reliability_neg, vec &expected_u, const ivec &support_data,
	const ivec &prev_support_data, const double P_rand, const unsigned int global_iters,
	const unsigned int local_iters){
	// initialization
	const unsigned int sig_dim = pos_count.n_elem;

	uvec old_zero_indices = find(prev_support_data ==0);
	uvec old_one_indices = find(prev_support_data ==1);
	vec  old_expected_u  = expected_u;

	uvec zero_indices = find(support_data ==0);
	uvec one_indices = find(support_data ==1);
	const uvec obs_indices = find(support_data !=-1);

	// #### Update U  #####
	mat Ln_Q(sig_dim,2,fill::zeros);
	uvec U(1);
	// evaluate update rules for U = 0
	U.zeros();
	//prior term
	Ln_Q(obs_indices,U).fill(expected_ln_beta_dist(reliability_neg,reliability_pos)) ;

	//liklihood term
	Ln_Q(zero_indices,U) += expected_ln_beta_dist(pos_count(zero_indices),neg_count(zero_indices));
	Ln_Q(one_indices,U)  += expected_ln_beta_dist(neg_count(one_indices),pos_count(one_indices));

	// evaluate update rules for U = 1
	U.ones();
	// prior term
	Ln_Q(obs_indices,U).fill( expected_ln_beta_dist(reliability_pos,reliability_neg) );
	// liklihood term
	Ln_Q(zero_indices,U) += expected_ln_beta_dist(neg_count(zero_indices),pos_count(zero_indices));
	Ln_Q(one_indices,U) += expected_ln_beta_dist(pos_count(one_indices),neg_count(one_indices));

	mat Q = exp(Ln_Q);		// posterior mass function (not normalized)
	expected_u(obs_indices) = Q(obs_indices,U)/sum(Q.rows(obs_indices),1);

	// #### Update R  #####
	// using coefficient reliability
	// prior term (XXX: comment tu use the prevoius value as prior)
	reliability_pos = 1;
	reliability_neg = 1;
	// liklihood term (coefficient reliability)
	reliability_pos += sum(expected_u(obs_indices));
	reliability_neg += sum(1 - expected_u(obs_indices));
	// liklihood term (number of iterations)
	reliability_pos += local_iters;
	reliability_neg += global_iters - local_iters;

	// #### Update Phi  #####
	#pragma omp critical
	{
	pos_count(one_indices) += expected_u(one_indices);
	neg_count(zero_indices) += expected_u(zero_indices);
	if (any(prev_support_data!=-1)){
		pos_count(old_one_indices) -= old_expected_u(old_one_indices);
		neg_count(old_zero_indices) -= old_expected_u(old_zero_indices);
	}
	}

	return;
}

vec bayesian_Sto_IHT(const mat &A, const vec &y, const int sparsity, const vec prob_vec,
		const unsigned int max_iter, const double gamma,const double tol,
		unsigned int &num_iters, const simulation_parameters simulation_params){
	uvec slow_cores;
	set_slow_cores(slow_cores, simulation_params);

	const unsigned int sig_dim = A.n_cols;
	// initialization of variables that are SHARED among cores
	uvec updated_indices;
	vec pos_count(sig_dim,fill::ones);	// positive count for tally score
	vec neg_count(sig_dim,fill::ones);	// negative count for tally score
	vec x_hat_total(sig_dim,fill::zeros);	// estimation of the signal
	bool done = false;			// flag to check the convergence criteria
	unsigned int i = 0;			// total number of iterations
	const double P_rand = sparsity/sig_dim;	//  probability of unreliable '1' measurement
	// parallel section of the code starts here
	#pragma omp parallel num_threads(simulation_params.num_cores)
	{

	// initializaiotn of variables that are LOCAL to each core
	uvec updated_indices;
	vec x_hat_local(sig_dim,fill::zeros);  	// this is local to each core
	unsigned int iter_local = 0;		// number of iteration for this core
	ivec support_data(sig_dim,fill::zeros);	// data on estimated support
	ivec prev_support_data(sig_dim);	// previous estimated support
	prev_support_data.fill(-1);
	double reliability_pos = 1;		// positive count for core reliability
	double reliability_neg = 1;		// negative count for core reliability
	vec expected_u(sig_dim);	// expected coeffcient reliability (vote)
	expected_u.fill(1);

	// iterations to find the solutions
	while(!done){
		vec tally= pos_count/ (pos_count + neg_count);
		// master thread uses the tally vector to check the convergence criteria
		if (omp_get_thread_num() == 0){
			const uvec sorted_ind = sort_index(abs(tally),"descend");
			const uvec est_supp = sorted_ind(span(0,sparsity - 1));
			const mat A_supp = A.cols(est_supp);
			x_hat_local.zeros();
			x_hat_local(est_supp) = solve(A_supp,y);
			if (norm (y - A*x_hat_local) < tol || i >= max_iter){
				x_hat_total = x_hat_local;
				done = true;
			}
		}

		//slow cores sleep for  simulation_params.sleep_slow_cores microseconds
		if (any( slow_cores == omp_get_thread_num()) ){
			usleep(simulation_params.sleep_slow_cores);
		}

		i++;
		iter_local++;

		// update the local estimate of the support
		uvec est_supp_local;
        est_supp_local = Sto_IHT_async_iteration(x_hat_local, tally, A, y,
            sparsity, prob_vec, gamma);

		// generate support data
		support_data.fill(-1);support_data.elem(est_supp_local).ones();
		update_tally_bayesian(pos_count, neg_count, reliability_pos, reliability_neg, expected_u, support_data, prev_support_data, P_rand, i , iter_local );

		prev_support_data = support_data;
	}
	}
	// parallel section of the code ends here

	num_iters = i;
	return x_hat_total;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file squarerootclvmodel.hpp
    \brief CLV model with a square root kernel process
*/

#ifndef quantlib_square_root_clv_model_hpp
#define quantlib_square_root_clv_model_hpp

#include <ql/time/date.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/math/interpolations/lagrangeinterpolation.hpp>
#include <ql/math/matrix.hpp>
#include <ql/experimental/math/gaussiannoncentralchisquaredpolynomial.hpp>

#include <boost/function.hpp>
#include <map>

namespace QuantLib {

    class GBSMRNDCalculator;
    class SquareRootProcess;
    class GeneralizedBlackScholesProcess;

    class SquareRootCLVModel : public LazyObject {
      public:
        SquareRootCLVModel(
            const boost::shared_ptr<GeneralizedBlackScholesProcess>& bsProcess,
            const boost::shared_ptr<SquareRootProcess>& sqrtProcess,
            const std::vector<Date>& maturityDates,
            Size lagrangeOrder,
            Real pMax = Null<Real>(),
            Real pMin = Null<Real>());

        // cumulative distribution function of the BS process
        Real cdf(const Date& d, Real x) const;

        // inverse cumulative distribution function of the BS process
        Real invCDF(const Date& d, Real q) const;

        // collocation points of the square root process
        Disposable<Array> collocationPointsX(const Date& d) const;

        // collocation points for the underlying Y
        Disposable<Array> collocationPointsY(const Date& d) const;

        // CLV mapping function
        boost::function<Real(Time, Real)> g() const;

      protected:
        void performCalculations() const;

      private:
        class MappingFunction : public std::binary_function<Time, Real, Real> {
          public:
            explicit MappingFunction(const SquareRootCLVModel& model);

            Real operator()(Time t, Real x) const;

          private:
            const boost::shared_ptr<Matrix> s_, x_;
            typedef std::map<Time, boost::shared_ptr<LagrangeInterpolation> >
                interpl_type;

            interpl_type interpl;
        };

        std::pair<Real, Real> nonCentralChiSquaredParams(const Date& d) const;

        const Real pMax_, pMin_;
        const boost::shared_ptr<GeneralizedBlackScholesProcess> bsProcess_;
        const boost::shared_ptr<SquareRootProcess> sqrtProcess_;
        const std::vector<Date> maturityDates_;
        const Size lagrangeOrder_;
        const boost::shared_ptr<GBSMRNDCalculator> rndCalculator_;

        mutable boost::function<Real(Time, Real)> g_;
    };
}

#endif


/**
 * @file Compute condition numbers of cell mass and stiffness matrices
 * for a bunch of finite element shape function sets.
 *
 * @note This file  is part of https://github.com/guidokanschat/benchmarks.git
 */

#include <iostream>
#include <fstream>

#include <deal.II/base/logstream.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_bernstein.h>
#include <deal.II/lac/lapack_full_matrix.h>

using namespace dealii;

enum class MatrixTypes
{
    mass,
    stiffness
};

template <int dim>
void matrix (const FiniteElement<dim>& element,
	     const MatrixTypes type)
{
  Triangulation<dim> tr;
  GridGenerator::hyper_cube (tr, 0., 1.);
  DoFHandler<dim> dh (tr);
  dh.distribute_dofs (element);

  const unsigned int degree = element.tensor_degree();
  QGauss<dim> quadrature(degree+1);
  LAPACKFullMatrix<double> M(element.dofs_per_cell);

  FEValues<dim> fe(element, quadrature,
		   update_JxW_values | update_values | update_gradients);
  fe.reinit(dh.begin_active());

  for (unsigned int k=0;k<quadrature.size();++k)
    for (unsigned int i=0;i<M.m();++i)
      for (unsigned int j=0;j<M.n();++j)
	for (unsigned int d=0;d<element.n_components();++d)
	  {
	    switch (type)
	      {
	    	case MatrixTypes::mass:
		      M(i,j) += fe.JxW(k)
				* fe.shape_value_component(j,k,d)
				* fe.shape_value_component(i,k,d);
	      	      break;
	      	case MatrixTypes::stiffness:
	      	      M(i,j) += fe.JxW(k)
	      			* (fe.shape_grad_component(j,k,d)
	      			   * fe.shape_grad_component(i,k,d));
	      	      break;
	      }
	  }

  M.compute_eigenvalues();
  double lmin = 1.e30;
  double lmax = -1.;

  for (unsigned int i=0;i<M.m();++i)
    {
      const double lambda = M.eigenvalue(i).real();
if (std::fabs(lambda) >= 1.e-9)
	{
	  if (lambda < lmin)
	    lmin = lambda;
	  if (lambda > lmax)
	    lmax = lambda;
	}
    }
  deallog << "\t[" << lmin << "\t, " << lmax
	  << "]\tcond " << lmax/lmin;
  deallog << std::endl;
}

template<int dim>
void doit ()
{
  typedef std::shared_ptr<const FiniteElement<dim> > FEPtr;
  std::vector<FEPtr> elements;
  elements.push_back(FEPtr(new FE_Q<dim>(1)));
  elements.push_back(FEPtr(new FE_Q<dim>(2)));
  elements.push_back(FEPtr(new FE_Q<dim>(3)));
  elements.push_back(FEPtr(new FE_Q<dim>(4)));
  elements.push_back(FEPtr(new FE_Q<dim>(5)));
  elements.push_back(FEPtr(new FE_Q<dim>(6)));
  elements.push_back(FEPtr(new FE_Q<dim>(7)));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(1))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(2))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(3))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(4))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(5))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(6))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(7))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGauss<1>(8))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(2))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(3))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(4))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(5))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(6))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(7))));
  elements.push_back(FEPtr(new FE_DGQArbitraryNodes<dim>(QGaussLobatto<1>(8))));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(1)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(2)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(3)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(4)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(5)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(6)));
  elements.push_back(FEPtr(new FE_Bernstein<dim>(7)));

for (const FEPtr& fe : elements)
  {
deallog << fe->get_name() << "::mass ";
matrix(*fe, MatrixTypes::mass);
deallog << fe->get_name() << "::stiff";
matrix(*fe, MatrixTypes::stiffness);
}

}

int main ()
{
  doit<2> ();
deallog << std::endl;
  doit<3> ();
}


// Filename: umfpack_solve_example.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main(int, char**)
{
#ifdef MTL_HAS_UMFPACK
    typedef mtl::compressed2D<double> matrix_type;

    matrix_type A(5, 5);
    A= 2.,  3.,  0.,  0.,  0.,
       3.,  0.,  4.,  0.,  6.,
       0., -1., -3.,  2.,  0.,
       0.,  0.,  1.,  0.,  0.,
       0.,  4.,  2.,  0.,  1.;
    crop(A);

    mtl::dense_vector<double>   x(5), b(5);
    b= 8., 45., -3., 3., 19.;
    mtl::dense_vector<double>   b2(2 * b);
    cout << "A = \n" << A << "b = " << b << "\n";

    // Factorize and solve
    umfpack_solve(A, x, b);
    cout << "\nA \\ b using umfpack_solve = " << x << "\n";

    // Define a solver object by internally factorizing A
    mtl::mat::umfpack::solver<matrix_type> solver(A);

    // Solve A * x == b and b2 with the solver object
    solver(x, b);
    solver(x, b2);

    // Change one or more matrix entries while keeping the sparsity pattern
    A.lvalue(1, 2)= 5.0;

    // Compute a new factorization (relying on unchanged sparsity)
    solver.update_numeric();

    // If we change b accordingly we will get the same result
    b[1]= 48;
    solver(x, b);
    cout << "\nA \\ b after numeric update = " << x << "\n";

    // Change matrix's values and sparsity
    {
	mtl::mat::inserter<matrix_type> ins(A);
	ins[3][4] << 2.;
    }
    cout << "\nA is now = \n" << A << "\n";

    // Perform a completely new factorization
    solver.update();

    b[3]= 13.;
    int status= solver(x, b);
    cout << "A \\ b after (complete) update = " << x << ", status is " << status << "\n";

#endif
    return 0;
}


#include "em_gmm.h"

#include <cassert>
#include <iostream>
#include <vector>
#include <random>

//< Uncomment the line below to utilize Math Kernel Library
//#define EIGEN_USE_MKL_ALL
#include <Eigen/Eigen>

namespace {
    //< Constant for numerical stability
    const float eps_covariance = 1e-10;
    const float eps_zero = 1e-10;
    const float eps_log_negative_inf = -1e30;
    const float eps_convergence = 1e-4;
    const float eps_regularize = 1e-30;
}

using namespace std;
using namespace Eigen;

typedef Matrix<float, Dynamic, Dynamic, RowMajor> RowMatrixXf;

inline float log_sum(const float& log_a, const float& log_b) {
    return log_a < log_b ?
         (log_b + std::log (1.0 + std::exp (log_a - log_b)))
         : (log_a + std::log (1.0 + std::exp (log_b - log_a)));
}

void calculate_log_prob_spherical(
        const RowMatrixXf& mat_data,
        const VectorXf& vec_nrm2_pts,
        const RowVectorXf& vec_weights,
        const RowMatrixXf& mat_means,
        const RowMatrixXf& mat_diag_covs,
        RowMatrixXf& mat_log_probs) {

    const long num_modes = mat_means.rows();
    const long num_pts = mat_data.rows();
    const long dim = mat_data.cols();

    assert( vec_nrm2_pts.rows() == num_pts &&
            vec_weights.cols() == num_modes &&
            mat_means.cols() == dim &&
            mat_diag_covs.rows() == num_modes &&
            mat_diag_covs.cols() == dim &&
            mat_log_probs.rows() == num_pts &&
            mat_log_probs.cols() == num_modes );

    RowVectorXf vec_nrm2_centers(num_modes);
    for (long c = 0; c < num_modes; c++) {
        vec_nrm2_centers(c) = mat_means.row(c).squaredNorm();
    }

    mat_log_probs.noalias() = mat_data * mat_means.transpose();
    mat_log_probs *= -2;
    #pragma omp parallel for
    for (long c = 0; c < num_modes; c++) {
        mat_log_probs.col(c) += vec_nrm2_pts;
    }
    #pragma omp parallel for
    for (long n = 0; n < num_pts; n++) {
        mat_log_probs.row(n) += vec_nrm2_centers;
    }

    #pragma omp parallel for
    for (long c = 0; c < num_modes; c++) {
        const float cov = mat_diag_covs(c,0);
        const float c1 = log(vec_weights(c) + eps_regularize)
            - 0.5*dim*log(2*M_PI) - 0.5*dim*log(cov);
        const float c2 = -0.5f/cov;
        mat_log_probs.col(c) *= c2;
        mat_log_probs.col(c) = (mat_log_probs.col(c).array() + c1).matrix();
    }
}

void calculate_log_prob_diagonal(
        const RowMatrixXf& mat_data,
        const RowVectorXf& vec_weights,
        const RowMatrixXf& mat_means,
        const RowMatrixXf& mat_diag_covs,
        RowMatrixXf& mat_log_probs) {

    const long num_modes = mat_means.rows();
    const long num_pts = mat_data.rows();
    const long dim = mat_data.cols();

    assert( vec_weights.cols() == num_modes &&
            mat_means.cols() == dim &&
            mat_diag_covs.rows() == num_modes &&
            mat_diag_covs.cols() == dim &&
            mat_log_probs.rows() == num_pts &&
            mat_log_probs.cols() == num_modes);

    const float c0(-0.5f*dim*log(2*M_PI));
    RowVectorXf vec_c1_cov_prod(num_modes);
    for (long c = 0; c < num_modes; c++) {
        vec_c1_cov_prod(c) = -0.5f*(mat_diag_covs.row(c).array().log().sum());
    }

    #pragma omp parallel for
    for (long n = 0; n < num_pts; n++) {
        RowVectorXf vec_data = mat_data.row(n);
        for (long c = 0; c < num_modes; c++) {
            RowVectorXf delta = (vec_data - mat_means.row(c));
            mat_log_probs(n,c) = -0.5f*(delta.array()*mat_diag_covs.row(c).array().cwiseInverse()).matrix().dot(delta)
                + c0 + vec_c1_cov_prod(c);
        }
    }
}

void em_gmm(
        const float *data,
        const long num_pts,
        const long dim,
        const int num_modes,
        float *means,
        float *diag_covs,
        float *weights,
        bool should_fit_spherical_gaussian) {

    using namespace std;

    assert (num_modes < num_pts && "Not enough data for em");

    RowVectorXf vec_eps_regularize(num_modes);
    vec_eps_regularize.fill(eps_regularize);

    //< K-means to initialize the EM
    std::vector<int> labels(num_pts, -1);

    Map<const RowMatrixXf> mat_data(data, num_pts, dim);
    Map<RowMatrixXf> mat_means(means, num_modes, dim);
    Map<RowMatrixXf> mat_diag_covs(diag_covs, num_modes, dim);
    Map<RowVectorXf> vec_weights(weights, num_modes);

    //< Random init
    random_device rd;
    default_random_engine gen(rd());
    uniform_int_distribution<long> kmeans_seed_dist(0, num_pts-1);
    vector<long> center_indices(num_modes);
    generate(center_indices.begin(), center_indices.end(), [&]{
        return kmeans_seed_dist(gen);
    });
    #pragma omp parallel for
    for (int c = 0; c < num_modes; c++) {
        long n = center_indices[c];
        mat_means.row(c) = mat_data.row(n);
    }

    //< kmeans convergence
    const int max_kmeans_iterations = 20;
    bool is_converged = false;
    float eps(0.0f);

    RowMatrixXf mat_distance(num_pts, num_modes);

    VectorXf vec_nrm2_pts(num_pts);
    #pragma omp parallel for
    for (long r = 0; r < num_pts; r++) {
        vec_nrm2_pts(r) = mat_data.row(r).squaredNorm();
    }

    RowVectorXf vec_nrm2_centers(num_modes);

    //< K-means
    RowMatrixXf mat_saved_means(num_modes, dim);
    RowVectorXf assigned_counts(num_modes);

    int iterations = 0;
    while ((iterations++ < max_kmeans_iterations) && !is_converged) {

        //< save previous centers
        mat_saved_means = mat_means;

        //< calculate point to center L2 distance
        //< (X - C)^2 = X^2 + C^2 - 2*X*C
        #pragma omp parallel for
        for (int c = 0; c < num_modes; c++) {
            vec_nrm2_centers(c) = mat_means.row(c).squaredNorm();
        }
        mat_distance.noalias() = mat_data * mat_means.transpose();
        mat_distance *= -2;
        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            mat_distance.col(c) += vec_nrm2_pts;
        }

        //< nearest centers along one column
        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            mat_distance.row(n) += vec_nrm2_centers;
            mat_distance.row(n).minCoeff(&labels[n]);
        }

        assigned_counts.fill(0.0f);
        for (long n = 0; n < num_pts; n++) {
            long c = labels[n];
            if (assigned_counts(c) < 1e-3) {
                mat_means.row(c) = mat_data.row(n);
            } else {
                mat_means.row(c) += mat_data.row(n);
            }
            assigned_counts(c) += 1.0f;
        }

        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            if (assigned_counts(c) > 1e-3) {
                mat_means.row(c) /= assigned_counts(c);
            }
        }

        //< evaluation
        const float prev_eps = eps;
        eps = (mat_saved_means - mat_means).norm();
        is_converged = (eps < eps_convergence);
        cout << "kmeans " << "[" << iterations << "] " << prev_eps << " " << eps << endl;
    }

    //< covariances and weights
    vec_weights = assigned_counts / (float)num_pts;
    mat_diag_covs.fill(0);
    for (long n = 0; n < num_pts; n++) {
        long c = labels[n];
        mat_diag_covs.row(c) += mat_data.row(n).array().square().matrix();
    }
    for (long c = 0; c < num_modes; c++) {
        if (assigned_counts(c) > 1e-3) {
            mat_diag_covs.row(c) /= assigned_counts(c);
            mat_diag_covs.row(c) -= mat_means.row(c).array().square().matrix();
        }
    }

    if (should_fit_spherical_gaussian) {
        for (int c = 0; c < num_modes; c++) {
            const float spherical_val = std::max(mat_diag_covs.row(c).sum()/dim, eps_covariance);
            mat_diag_covs.row(c).fill(spherical_val);
        }
    }

    //< EM
    RowMatrixXf& mat_log_probs = mat_distance;

    const int max_em_iterations = 20;
    is_converged = false; //< use weights as approximated indicator
    float expectation(std::numeric_limits<float>::lowest());

    RowVectorXf vec_evals(num_pts);
    RowVectorXf vec_log_sum_probs(num_pts);
    RowVectorXf vec_occup_eN(num_modes);
    RowMatrixXf mat_occup_eX(num_modes, dim);
    RowMatrixXf mat_occup_eX2(num_modes, dim);

    iterations = 0;
    while ((iterations++ < max_em_iterations) && !is_converged) {

        //< calculate log probabilities
        if (should_fit_spherical_gaussian) {
            calculate_log_prob_spherical(mat_data, vec_nrm2_pts, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
        } else {
            calculate_log_prob_diagonal(mat_data, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
        }

        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            vec_log_sum_probs(n) = mat_log_probs(n,0);
            for (long c = 1; c < num_modes; c++) {
                if (mat_log_probs(n,c) > eps_log_negative_inf) {
                    vec_log_sum_probs(n) = log_sum(vec_log_sum_probs(n), mat_log_probs(n,c));
                }
            }
        }

        #pragma omp parallel for
        for (long n = 0; n < num_pts; n++) {
            RowVectorXf soft_count = (mat_log_probs.row(n).array() - vec_log_sum_probs(n)).exp().matrix();
            vec_evals(n) = 0;
            for (long c = 0; c < num_modes; c++) {
                if (soft_count(c) > eps_zero) {
                    vec_evals(n) += mat_log_probs(n,c)*soft_count(c);
                }
            }
            mat_log_probs.row(n) = soft_count;
        }

        ////< Occupation counts
        #pragma omp parallel for
        for (long c = 0; c < num_modes; c++) {
            vec_occup_eN(c) = mat_log_probs.col(c).sum();
        }

        mat_occup_eX = mat_log_probs.transpose() * mat_data;
        //< M-Step: update means/diag_covs/weights
        vec_weights = vec_occup_eN / vec_occup_eN.sum();
        vec_occup_eN += vec_eps_regularize;
        #pragma omp parallel for
        for (int c = 0; c < num_modes; c++) {
            if (vec_weights(c) > eps_zero) {
                mat_means.row(c) = mat_occup_eX.row(c)/vec_occup_eN(c);
            }
        }

        VectorXf vec_sum_occup_nrm2_pts = mat_log_probs.transpose() * vec_nrm2_pts;
        if (should_fit_spherical_gaussian) {
            for (int c = 0; c < num_modes; c++) {
                const float spherical_val
                    = (vec_sum_occup_nrm2_pts(c) - 2*mat_means.row(c).dot(mat_occup_eX.row(c)) + vec_nrm2_centers(c)*vec_occup_eN(c))
                    / (dim*vec_occup_eN(c));
                mat_diag_covs.row(c).fill(spherical_val);
            }
        } else {
            mat_occup_eX2 = mat_log_probs.transpose() * mat_data.array().square().matrix();
            for (long c = 0; c < num_modes; c++) {
                mat_diag_covs.row(c) = ((mat_occup_eX2.row(c)/vec_occup_eN(c)).array() - mat_means.row(c).array().square()).max(eps_covariance).matrix();
            }
        }

        const float prev_expectation = expectation;
        expectation = vec_evals.sum();
        const float scale = 1e5;
        const float delta = exp((expectation - prev_expectation)/scale) - 1;
        is_converged = (iterations > 0 && delta < eps_convergence);
        cout << "em " << "[" << iterations << "] " << delta << " " << expectation << endl;
    } //< em LOOP END

} //< function: fit_mixture_model

void likelihood_gmm(
        const float *data,
        const long num_pts,
        const long dim,
        const int num_modes,
        const float *means,
        const float *diag_covs,
        const float *weights,
        float *log_probs, //< num_pts x num_modes
        bool is_spherical_gaussian) {

    Map<const RowMatrixXf> mat_data(data, num_pts, dim);
    Map<const RowMatrixXf> mat_means(means, num_modes, dim);
    Map<const RowMatrixXf> mat_diag_covs(diag_covs, num_modes, dim);
    Map<const RowVectorXf> vec_weights(weights, num_modes);

    VectorXf vec_nrm2_pts(num_pts);
    #pragma omp parallel for
    for (long r = 0; r < num_pts; r++) {
        vec_nrm2_pts(r) = mat_data.row(r).squaredNorm();
    }

    RowVectorXf vec_nrm2_centers(num_modes);

    RowMatrixXf mat_log_probs(num_pts, num_modes);

    //< calculate log probabilities
    if (is_spherical_gaussian) {
        calculate_log_prob_spherical(mat_data, vec_nrm2_pts, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
    } else {
        calculate_log_prob_diagonal(mat_data, vec_weights, mat_means, mat_diag_covs, mat_log_probs);
    }

    std::copy(mat_log_probs.data(), mat_log_probs.data() + num_pts*num_modes, log_probs);
}


/*=============================================================================
    Copyright (c) 2001-2003 Hartmut Kaiser
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  This sample shows, how to use Phoenix for implementing a
//  simple (RPN style) calculator [ demonstrating phoenix ]
//
//  [ HKaiser 2001 ]
//  [ JDG 6/29/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_attribute.hpp>
#include <boost/spirit/include/phoenix1_functions.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace BOOST_SPIRIT_CLASSIC_NS;
using namespace phoenix;

///////////////////////////////////////////////////////////////////////////////
//
//  Our RPN calculator grammar using phoenix to do the semantics
//  The class 'RPNCalculator' implements a polish reverse notation
//  calculator which is equivalent to the following YACC description.
//
//  exp:
//        NUM           { $$ = $1;           }
//      | exp exp '+'   { $$ = $1 + $2;      }
//      | exp exp '-'   { $$ = $1 - $2;      }
//      | exp exp '*'   { $$ = $1 * $2;      }
//      | exp exp '/'   { $$ = $1 / $2;      }
//      | exp exp '^'   { $$ = pow ($1, $2); }  /* Exponentiation */
//      | exp 'n'       { $$ = -$1;          }  /* Unary minus */
//      ;
//
//  The different notation results from the requirement of LL parsers not to
//  allow left recursion in their grammar (would lead to endless recursion).
//  Therefore the left recursion in the YACC script before is transformated
//  into iteration. To some, this is less intuitive, but once you get used
//  to it, it's very easy to follow.
//
//  Note:   The top rule propagates the expression result (value) upwards
//          to the calculator grammar self.val closure member which is
//          then visible outside the grammar (i.e. since self.val is the
//          member1 of the closure, it becomes the attribute passed by
//          the calculator to an attached semantic action. See the
//          driver code that uses the calculator below).
//
///////////////////////////////////////////////////////////////////////////////
struct pow_
{
    template <typename X, typename Y>
    struct result { typedef X type; };

    template <typename X, typename Y>
    X operator()(X x, Y y) const
    {
        using namespace std;
        return pow(x, y);
    }
};

//  Notice how power(x, y) is lazily implemented using Phoenix function.
function<pow_> power;

struct calc_closure : BOOST_SPIRIT_CLASSIC_NS::closure<calc_closure, double, double>
{
    member1 x;
    member2 y;
};

struct calculator : public grammar<calculator, calc_closure::context_t>
{
    template <typename ScannerT>
    struct definition {

        definition(calculator const& self)
        {
            top = expr                      [self.x = arg1];
            expr =
                real_p                      [expr.x = arg1]
                >> *(
                        expr                [expr.y = arg1]
                        >>  (
                                ch_p('+')   [expr.x += expr.y]
                            |   ch_p('-')   [expr.x -= expr.y]
                            |   ch_p('*')   [expr.x *= expr.y]
                            |   ch_p('/')   [expr.x /= expr.y]
                            |   ch_p('^')   [expr.x = power(expr.x, expr.y)]
                            )
                    |   ch_p('n')           [expr.x = -expr.x]
                    )
                ;
        }

        typedef rule<ScannerT, calc_closure::context_t> rule_t;
        rule_t expr;
        rule<ScannerT> top;

        rule<ScannerT> const&
        start() const { return top; }
    };
};

///////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tExpression parser using Phoenix...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type an expression...or [q or Q] to quit\n\n";

    calculator calc;    //  Our parser

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        double n = 0;
        parse_info<> info = parse(str.c_str(), calc[var(n) = arg1], space_p);

        //  calc[var(n) = arg1] invokes the calculator and extracts
        //  the result of the computation. See calculator grammar
        //  note above.

        if (info.full)
        {
            cout << "-------------------------\n";
            cout << "Parsing succeeded\n";
            cout << "result = " << n << endl;
            cout << "-------------------------\n";
        }
        else
        {
            cout << "-------------------------\n";
            cout << "Parsing failed\n";
            cout << "stopped at: \": " << info.stop << "\"\n";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


// Método da bisseção para obter raizes de equações
#include <iostream>
#include <boost/math/tools/roots.hpp>

using namespace std;
using namespace boost::math::tools;

struct fn {
    double operator() (double x) {
        return 2 * x - 1;
    }
};

struct condition {
    double m_limit;
    condition(double limit) : m_limit(limit) { }
    double operator() (double min, double max) {
        return abs(min - max) <= m_limit;
    }
};

int main() {
    std::pair<double, double> result = bisect(fn(), -1.0, 1.0, condition(0.0001));

    double root = (result.first + result.second) / 2;

    cout << "Raiz: " << root << endl;

    return 0;
}


#ifndef HAMILTONIANS_XXX_HPP
#define HAMILTONIANS_XXX_HPP
#include <Eigen/Eigen>

class XXX
{
private:
	int n_;

public:

	XXX(int n)
		: n_(n)
	{
	}

	template<class State>
	typename State::ValueType operator()(const State& smp) const
	{
		typename State::ValueType s = 0.0;
		//Nearest-neighbor
		for(int i = 0; i < n_; i++)
		{
			double yysign = smp.sigmaAt(i)*smp.sigmaAt((i+1)%n_);
			s += yysign; //zz
			s += (1.0-yysign)*smp.ratio(i, (i+1)%n_); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		std::map<uint32_t,double> res;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			res[col ^ x] += 1-sgn;
			res[col] += sgn;
		}
		return res;
	}
};
#endif//HAMILTONIANS_XXX_HPP


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main()
{
  ArrayXXf  m(2,2);

  //assign some values coefficient by coefficient
  m(0,0) = 1.0; m(0,1) = 2.0;
  m(1,0) = 3.0; m(1,1) = 4.0;

  //print values to standard output
  cout << m << endl << endl;

  // using the comma-initializer is also allowed
  m << 1.0,2.0,
       3.0,4.0;

  //print values to standard output
  cout << m << endl;
}