//==================================================================================================
/**
  Copyright 2016 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)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_BITWISE_SELECT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_BITWISE_SELECT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_andnot.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/genmask.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( bitwise_select_
                            , (typename A0, typename A1, typename X, typename Y)
                            , (brigand::bool_<A0::static_size==A1::static_size>)
                            , bd::cpu_
                            , bs::pack_<bd::arithmetic_<A0>, X >
                            , bs::pack_<bd::arithmetic_<A1>, Y >
                            , bs::pack_<bd::arithmetic_<A1>, Y >
                            )
  {
    BOOST_FORCEINLINE A1 operator()(A0 const& a0, A1 const& a1, A1 const& a2) const BOOST_NOEXCEPT
    {
      return bitwise_or(bitwise_and(a1, a0), bitwise_andnot(a2,a0));
    }
  };
} } }

#endif


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2010 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  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              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if defined(USING_NUMARRAY)
#	define PY_ARRAY_UNIQUE_SYMBOL PyArrayHandle
#	define NO_IMPORT_ARRAY
#endif

#include <numeric>
#include <functional>
#include <cmath>
#include <boost/lambda/lambda.hpp>

#include "multivariate_probability_distribution.hpp"
#include "relative_rate_distribution.hpp"
//#include "ncl/nxsexception.h"

namespace phycas
{

/*----------------------------------------------------------------------------------------------------------------------
|	Construct the equivalent of a flat Beta distribution by default.
*/
RelativeRateDistribution::RelativeRateDistribution()
  : DirichletDistribution(), dim(2)
	{
	sum_params = std::accumulate(dirParams.begin(), dirParams.end(), 0.0);
    _subset_proportions = SubsetProportionsShPtr(new SubsetProportions());
    _subset_proportions->setSubsetProportions(std::vector<double>(2, 0.5));
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a RelativeRateDistribution based on the parameter values supplied in the vector `params' and the
|   coefficients supplied in the vector 'coeffs'.
*/
RelativeRateDistribution::RelativeRateDistribution(
  const double_vect_t & params, /**< is the vector of parameters */
  const double_vect_t & coeffs) /**< is the vector of coefficients */
  : DirichletDistribution(params), dim(2)
	{
	PHYCAS_ASSERT(params.size() > 1);
	dim = (unsigned)params.size();
	PHYCAS_ASSERT(coeffs.size() == dim);
	sum_params = std::accumulate(dirParams.begin(), dirParams.end(), 0.0);
    _subset_proportions = SubsetProportionsShPtr(new SubsetProportions());
    _subset_proportions->setSubsetProportions(coeffs);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Initializes `dirParams', `scratchSpace' and `paramDistributions' vectors to the values contained in their
|   counterparts in `other'.
*/
RelativeRateDistribution::RelativeRateDistribution(
  const RelativeRateDistribution & other)	/* the relative rate distribution to clone */
	{
	dirParams.resize(other.dirParams.size());
	std::copy(other.dirParams.begin(), other.dirParams.end(), dirParams.begin());

	scratchSpace.resize(other.scratchSpace.size());
	std::copy(other.scratchSpace.begin(), other.scratchSpace.end(), scratchSpace.begin());

    paramDistributions.clear();
    for (std::vector<GammaDistribution>::const_iterator it = other.paramDistributions.begin(); it != other.paramDistributions.end(); ++it)
	    paramDistributions.push_back(GammaDistribution(*it));

	dim = other.dim;
	sum_params = other.sum_params;
    _subset_proportions = other._subset_proportions;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns the string "RelativeRateDistribution".
*/
std::string RelativeRateDistribution::GetDistributionName() const
	{
	return "RelativeRateDistribution";
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns a string similar to "RelativeRateDistribution(1.0,1.0,1.0,0.33333,0.33333,0.33333)", where the first 3
|   arguments are the underlying parameters while the last 3 are the probabilities associated with each relative rate.
*/
std::string RelativeRateDistribution::GetDistributionDescription() const
	{
	PHYCAS_ASSERT(dim > 1);
	std::string s;
    double_vect_t::const_iterator it;
	s << "RelativeRateDistribution(";
	for (it = dirParams.begin(); it != dirParams.end(); ++it)
		{
		if (it == dirParams.begin())
			s << boost::str(boost::format("%#.5f") % (*it));
		else
			s << boost::str(boost::format(",%#.5f") % (*it));
		}
    std::vector<double> const & p = _subset_proportions->getSubsetProportions();
	for (it = p.begin(); it != p.end(); ++it)
		{
		if (it == p.begin())
			s << boost::str(boost::format("%#.5f") % (*it));
		else
			s << boost::str(boost::format(",%#.5f") % (*it));
		}
	s << ')';
	return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	This function is needed because in Python the parameters of the distribution are supplied to the constructor via
|	a tuple, which requires extra sets of parentheses. For example, in Phycas, one could sample from a 5-parameter
|	relative rate distribution as follows:
|
|	>>> RelativeRateDistribution((0.8,0.9,1.0,1.1,1.2),(0.2,0.2,0.2,0.2,0.2)).sample()
|
|	Note the parentheses around the parameters and coefficients needed in the Python representation.
*/
std::string RelativeRateDistribution::GetDescriptionForPython() const
	{
	PHYCAS_ASSERT(dim > 1);
	std::string s;
    double_vect_t::const_iterator it;
	s << "RelativeRateDistribution((";
	for (it = dirParams.begin(); it != dirParams.end(); ++it)
		{
		if (it == dirParams.begin())
			s << boost::str(boost::format("%#.5f") % (*it));
		else
			s << boost::str(boost::format(",%#.5f") % (*it));
		}
	s << "),(";
    std::vector<double> const & p = _subset_proportions->getSubsetProportions();
	for (it = p.begin(); it != p.end(); ++it)
		{
		if (it == p.begin())
			s << boost::str(boost::format("%#.5f") % (*it));
		else
			s << boost::str(boost::format(",%#.5f") % (*it));
		}
	s << "))";
	return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns a vector of doubles representing the mean of this RelativeRateDistribution. The mean is `dim' times the
|	mean vector for a Dirichlet random variable with the same parameters.
*/
double_vect_t RelativeRateDistribution::GetMean() const
	{
    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t retvect(dim, 0.0);
	for (unsigned i = 0; i < dim; ++i)
		{
		retvect[i] = dirParams[i]/(p[i]*sum_params);
		}
	return retvect;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns a vector of doubles representing the variance of this RelativeRateDistribution. The variance is `dim'
|	squared times the variance vector for a Dirichlet random variable with the same parameters.
*/
double_vect_t RelativeRateDistribution::GetVar() const
	{
    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t retvect(dim, 0.0);
	double denom = sum_params*sum_params*(sum_params + 1.0);
	for (unsigned i = 0; i < dim; ++i)
		{
		retvect[i] = dirParams[i]*(sum_params - dirParams[i])/(denom*p[i]*p[i]);
		}
	return retvect;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns a vector of doubles representing the square root of this RelativeRateDistribution. The square root is `dim'
|	times the square root vector for a Dirichlet random variable with the same parameters.
*/
double_vect_t RelativeRateDistribution::GetStdDev() const
	{
    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t retvect(dim, 0.0);
	double denom = sum_params*sqrt(sum_params + 1.0);
	for (unsigned i = 0; i < dim; ++i)
		{
		double numer = dirParams[i]*(sum_params - dirParams[i]);
		retvect[i] = sqrt(numer)/(denom*p[i]);
		}
	return retvect;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Approximates the cumulative distribution function evaluated at the supplied point `x'. The precision of the
|	approximation is controlled by `nsamples'. The approximation is done using a brute force approach: `nsamples'
|	samples are drawn from this RelativeRateDistribution, and the proportion of those samples that are inside the region
|	defined by `x' is returned as the approximated CDF. The supplied point should be a vector of length k, where k is
|	one fewer than the number of parameters of the RelativeRateDistribution. If `x' has length greater than k, all
|	elements of `x' after the (k-1)th. will be ignored.
*/
double RelativeRateDistribution::ApproxCDF(
  const double_vect_t & x,		/**< is the point to be evaluated */
  unsigned nsamples)			/**< is the number of samples to use in approximating the CDF */
  const
	{
	PHYCAS_ASSERT(nsamples > 0);
	PHYCAS_ASSERT(x.size() >= dim - 1);
    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t tmp(x.size(), 0.0);
	for (unsigned i = 0; i < dim; ++i)
		{
		tmp[i] = p[i]*x[i];
		}
	return DirichletDistribution::ApproxCDF(tmp, nsamples);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Samples and returns a `dim'-dimensional point from this distribution.
*/
double_vect_t RelativeRateDistribution::Sample() const
	{
    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t x(dim, 0.0);

	double sum = 0.0;
	for (unsigned i = 0; i < dim; ++i)
		{
		const double y = paramDistributions[i].Sample();
		scratchSpace[i] = y;
		sum += y;
		}

	for (unsigned i = 0; i < dim; ++i)
		{
		x[i] = scratchSpace[i]/(p[i]*sum);
		}
	return x;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the natural logarithm of the probability density function evaluated at the supplied point `x'. To derive
|	the relative rate distribution density, imagine a random variable Y ~ Beta(a,b) and assume X=Y/p. X has a relative
|	rate distribution with density
|>
|	f_X(x) = f_Y(pX) dy/dx
|	dy/dx = p
|>
|	Thus, the density of X will be p times the Beta density evaluated at pX. This generalizes to the Dirichlet: the
|	density of a value x of a random variable X having an n-parameter relative rate distribution will be p_1 p_2 ... p_n
|	times the corresponding Dirichlet pdf evaluated at the point (p_1 x_1, p_2 x_2, ..., p_n x_n), where p_i is the
|	weight associated with subset i and the sum of the p_i values equals 1.
*/
double RelativeRateDistribution::GetLnPDF(
  const double_vect_t & x) const 	/**< is the point at which to evaluate the density */
	{
	unsigned sz = (unsigned)x.size();

	if (sz == dim || sz == dim - 1)
		{
		double_vect_t tmp(dim, 0.0);

        std::vector<double> const & p = _subset_proportions->getSubsetProportions();

		std::transform(x.begin(), x.end(), p.begin(), tmp.begin(), boost::lambda::_1*boost::lambda::_2);

#if 0
        //POL CHECK SUBSET RELRATES
        std::cerr << "\nIn RelativeRateDistribution::GetLnPDF:" << std::endl;
        std::cerr << "i" << "\t" << "p[i]" << "\t" << "x[i]" << "\t" << "p[i]*x[i]" << "\t" << "tmp[i]" << std::endl;
        double doof_sum = 0.0;
        for (unsigned i = 0; i < sz; ++i)
            {
            std::cerr << i << "\t" << p[i] << "\t" << x[i] << "\t" << (p[i]*x[i]) << "\t" << tmp[i] << std::endl;
            doof_sum += p[i]*x[i];
            }
        std::cerr << "sum p[i]*x[i] = " << doof_sum;
        std::cerr << std::endl;
#endif

		double sum_tmp = std::accumulate(tmp.begin(), tmp.end(), 0.0);
        double log_prod_p = _subset_proportions->getLogProdProportions();
		if (sz == dim - 1)
			{
			// last element of tmp needs to be calculated because supplied vector x was one element short
			tmp[sz] = 1.0 - sum_tmp;
			sum_tmp = 1.0;
			double ln_pdf = log_prod_p + DirichletDistribution::GetLnPDF(tmp);
			return ln_pdf;
			}
		if (fabs(sum_tmp - 1.0) > 0.001)
			throw XProbDist(boost::str(boost::format("Vector supplied to RelativeRateDistribution::GetLnPDF is not valid: sum of transformed elements was %g but was expected to be 1.0 (tolerance +-0.001)") % sum_tmp));
		double ln_pdf = log_prod_p + DirichletDistribution::GetLnPDF(tmp);
		return ln_pdf;
		}
	else
		throw XProbDist(boost::str(boost::format("Vector supplied to RelativeRateDistribution::GetLnPDF has dimension %d but dimension should be either %d or %d") % sz % dim % (dim-1)));
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the natural logarithm of the probability density function evaluated at the supplied point `x'.
*/
double RelativeRateDistribution::GetRelativeLnPDF(
  const double_vect_t & x) const
	{
	return GetLnPDF(x);
	}

#if defined(PYTHON_ONLY)
/*----------------------------------------------------------------------------------------------------------------------
|	Returns the variance-covariance matrix in the form of a 1-dimensional vector (first row, then second row, etc.)
*/
double_vect_t RelativeRateDistribution::GetVarCovarMatrix()
	{
	unsigned i, j;

	double c = sum_params;
	double denom = c*c*(c + 1.0);

    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t V;
	for (i = 0; i < dim; ++i)
		{
		for (j = 0; j < dim; ++j)
			{
			if (i == j)
				{
				double var_i = dirParams[i]*(c - dirParams[i])/(p[i]*p[i]*denom);
				V.push_back(var_i);
				}
			else
				{
				double cov_ij = -dirParams[i]*dirParams[j]/(p[i]*p[j]*denom);
				V.push_back(cov_ij);
				}
			}
		}

	return V;
	}
#endif

/*----------------------------------------------------------------------------------------------------------------------
|	Sets parameters of the distribution from the vector of means and vector of variances (note: v is not the variance
|	covariance matrix, but just a single-dimensional array of variances - the covariances are not needed). Specifically,
|	if X is a relative rate vector, and Yi = Xi*pi, then E[Yi] = pi*E[Xi] and Var(Yi) = pi^2*Var(Xi). Thus, simple
|	transformations of the means (E[Xi]) and variances (Var(Xi)) of the elements of X (supplied in the form of `m' and
|	`v') can be turned over to the DirichletDistribution::SetMeanAndVariance function to parameterize the underlying
|	Dirichlet base class data members.
*/
void RelativeRateDistribution::SetMeanAndVariance(
  const double_vect_t & m, 	/**< is the vector of means */
  const double_vect_t & v) 	/**< is the vector of variances */
	{
	if (dim != (unsigned)m.size())
		throw XProbDist(boost::str(boost::format("Mean vector supplied to SetMeanAndVariance has dimension %d but RelativeRateDistribution has dimension %d") % (unsigned)m.size() % dim));
	if (dim != (unsigned)v.size())
		throw XProbDist(boost::str(boost::format("Variance vector supplied to SetMeanAndVariance has dimension %d but RelativeRateDistribution has dimension %d") % (unsigned)v.size() % dim));

    double_vect_t const & p = _subset_proportions->getSubsetProportions();
	double_vect_t transformed_means(dim);
	std::transform(m.begin(), m.end(), p.begin(), transformed_means.begin(), boost::lambda::_1*boost::lambda::_2);

	double_vect_t transformed_variances(dim);
	std::transform(v.begin(), v.end(), p.begin(), transformed_variances.begin(), boost::lambda::_1*boost::lambda::_2*boost::lambda::_2);

	DirichletDistribution::SetMeanAndVariance(transformed_means, transformed_variances);
	sum_params = std::accumulate(dirParams.begin(), dirParams.end(), 0.0);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns number of parameters.
*/
unsigned RelativeRateDistribution::GetNParams() const
	{
	return dim;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a new object that is a clone of this object, calls the new object's SetLot member function (passing the
|	supplied Lot object `other'), and returns a pointer to it. The caller is expected to manage the new object.
*/
RelativeRateDistribution * RelativeRateDistribution::cloneAndSetLot(Lot * other) const
	{
    RelativeRateDistribution * clone = new RelativeRateDistribution(*this);
	clone->SetLot(other);
	return clone;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Creates a new object that is a clone of this object and returns a pointer to it. Caller is expected to manage the
|   new object.
*/
RelativeRateDistribution * RelativeRateDistribution::Clone() const
	{
    return new RelativeRateDistribution(*this);
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Sets the data member `_subset_proportions' to the supplied `subset_proportions'.
*/
void RelativeRateDistribution::setSubsetProportions(SubsetProportionsShPtr subset_proportions)
	{
    _subset_proportions = subset_proportions;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Copies the supplied coefficiences in `coeff' to the data member `p'.
*/
//void RelativeRateDistribution::SetCoefficients(
//  const double_vect_t & coeff)	/**< is the new vector of coefficients */
//	{
//	// Throw exception if coeff has an incorrect number of elements
//	if (dim != (unsigned)coeff.size())
//		throw XProbDist(boost::str(boost::format("Coefficient vector supplied to RelativeRateDistribution::SetCoefficients has dimension %d but RelativeRateDistribution has dimension %d") % (unsigned)coeff.size() % dim));
//
//	// Throw exception if any elements of coeff are negative or zero
//	for (double_vect_t::const_iterator it = coeff.begin(); it != coeff.end(); ++it)
//		{
//		double x = *it;
//		if (x <= 0.0)
//			throw XProbDist(boost::str(boost::format("Coefficient vector supplied to RelativeRateDistribution::SetCoefficients has at least one element (%g) with value less than or equal to zero") % x));
//		}
//
//	// Don't assume that the elements of coeff add up to 1.0
//	double sum_coeff = std::accumulate(coeff.begin(), coeff.end(), 0.0);
//	std::transform(coeff.begin(), coeff.end(), p.begin(), boost::lambda::_1/sum_coeff);
//
//	// Recalculate log_prod_p
//	log_prod_p = 0.0;
//	for (unsigned i = 0; i < dim - 1; ++i)
//		{
//		log_prod_p += log(p[i]);
//		}
//    }

} // namespace phycas


#include <hpx/config.hpp>

#include <qubus/performance_models/simple_statistical_performance_model.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/error_of.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <hpx/include/local_lcos.hpp>

#include <qubus/util/assert.hpp>
#include <qubus/util/unused.hpp>

#include <map>
#include <mutex>
#include <utility>
#include <vector>

namespace qubus
{

namespace
{
std::vector<hpx::naming::gid_type> generate_key_from_ctx(const execution_context& ctx)
{
    std::vector<hpx::naming::gid_type> key;
    key.reserve(ctx.args().size());

    for (const auto& obj : ctx.args())
    {
        key.push_back(obj.get_raw_gid());
    }

    return key;
}

class accumulator
{
public:
    using value_type = std::chrono::microseconds::rep;

    void sample(std::chrono::microseconds execution_time)
    {
        acc_(execution_time.count());
    }

    std::chrono::microseconds mean() const
    {
        value_type value = boost::accumulators::mean(acc_);

        return std::chrono::microseconds(std::move(value));
    }

    std::chrono::microseconds variance() const
    {
        value_type value = boost::accumulators::variance(acc_);

        return std::chrono::microseconds(std::move(value));
    }

    std::chrono::microseconds error_of_mean() const
    {
        value_type value = boost::accumulators::error_of<boost::accumulators::tag::mean>(acc_);

        return std::chrono::microseconds(std::move(value));
    }

private:
    boost::accumulators::accumulator_set<
        value_type, boost::accumulators::stats<
                        boost::accumulators::tag::count, boost::accumulators::tag::mean,
                        boost::accumulators::tag::variance,
                        boost::accumulators::tag::error_of<boost::accumulators::tag::mean>>>
        acc_;
};
}

class simple_statistical_performance_model_impl
{
public:
    void sample_execution_time(const symbol_id& QUBUS_UNUSED(func), const execution_context& ctx,
                               std::chrono::microseconds execution_time)
    {
        std::lock_guard<hpx::lcos::local::mutex> guard(accumulators_mutex_);

        auto key = generate_key_from_ctx(ctx);

        auto search_result = accumulators_.find(key);

        if (search_result == accumulators_.end())
        {
            search_result = accumulators_.emplace(key, accumulator()).first;
        }

        search_result->second.sample(std::move(execution_time));
    }

    boost::optional<performance_estimate>
    try_estimate_execution_time(const symbol_id& QUBUS_UNUSED(func), const execution_context& ctx) const
    {
        std::lock_guard<hpx::lcos::local::mutex> guard(accumulators_mutex_);

        auto key = generate_key_from_ctx(ctx);

        auto search_result = accumulators_.find(key);

        if (search_result != accumulators_.end())
        {
            return performance_estimate{search_result->second.mean(),
                                        search_result->second.error_of_mean()};
        }
        else
        {
            return boost::none;
        }
    }

private:
    mutable hpx::lcos::local::mutex accumulators_mutex_;
    std::map<std::vector<hpx::naming::gid_type>, accumulator> accumulators_;
};

simple_statistical_performance_model::simple_statistical_performance_model()
: impl_(std::make_unique<simple_statistical_performance_model_impl>())
{
}

simple_statistical_performance_model::~simple_statistical_performance_model() = default;

void simple_statistical_performance_model::sample_execution_time(
    const symbol_id& func, const execution_context& ctx, std::chrono::microseconds execution_time)
{
    QUBUS_ASSERT(impl_, "Uninitialized object.");

    impl_->sample_execution_time(func, ctx, std::move(execution_time));
}

boost::optional<performance_estimate>
simple_statistical_performance_model::try_estimate_execution_time(
    const symbol_id& func, const execution_context& ctx) const
{
    QUBUS_ASSERT(impl_, "Uninitialized object.");

    return impl_->try_estimate_execution_time(func, ctx);
}
}


//==================================================================================================
/*!
  @file
  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SIGNGAM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SIGNGAM_HPP_INCLUDED

#include <boost/simd/function/scalar/is_nan.hpp>
#include <boost/simd/function/scalar/is_lez.hpp>
#include <boost/simd/function/scalar/is_flint.hpp>
#include <boost/simd/function/scalar/is_odd.hpp>
#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/scalar/is_inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/nan.hpp>

#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( signgam_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
#ifdef BOOST_SIMD_NO_INVALIDS
      if (is_lez(a0))
      {
        if (is_flint(a0))
          return Nan<A0>();
        else
          return One<A0>()-bool(is_odd(floor(a0)))*Two<A0>();
      }
#else
      if (is_lez(a0))
      {
        if (is_flint(a0)||is_inf(a0))
          return Nan<A0>();
        else
          return One<A0>()-bool(is_odd(floor(a0)))*Two<A0>();
      }
      else if (is_nan(a0)) return a0;
#endif
      return One<A0>();
    }
  };
} } }

#endif


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(length(list()) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(length(list(1, '2', 3.0)) == int_<3>);

    BOOST_HANA_CONSTANT_ASSERT(length(nothing) == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(length(just('x')) == int_<1>);
    //! [main]
}


#include <cstdio>
#include <iostream>

#include "base/eigen2hdf.hpp"
#include "base/init.hpp"
#include "base/timer.hpp"
#include "fft/fft2_r2c.hpp"
#include "fft/planner.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

#include <boost/program_options.hpp>
#include <iomanip>

using namespace std;

const char* fname = "test_fft2.h5";

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

  namespace po = boost::program_options;

  unsigned int Jx, Jy, rho_x, rho_y;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(3), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(3), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(10), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(10), "rho_x");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }
  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n";

  FFTr2c<PlannerR2C> fft;

  RDTSCTimer timer;
  timer.start();
  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  auto tlap = timer.stop();
  cout << "RidgeletFrame::RidgeletFrame(): " << tlap / 1e9 << " [Gcycles]"
       << "\n";

  timer.start();
  init_fftw(fft, FFTW_MEASURE, frame);
  auto tlap_fftw_plans = timer.stop();
  timer.print(cout, tlap_fftw_plans, "fftw plans");

  unsigned int Nx = frame.Nx();
  unsigned int Ny = frame.Ny();

  cout << "Nx: " << Nx
       << "\n"
       << "Ny: " << Ny
       << "\n";

  return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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/config.hpp>
#include <iostream>
#include <functional>
#include <string>

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

/*
  Sample output:

  0  --chandler--> 1   --joe--> 1
  1  --chandler--> 0   --joe--> 0   --curly--> 2   --dick--> 3   --dick--> 3
  2  --curly--> 1   --tom--> 4
  3  --dick--> 1   --dick--> 1   --harry--> 4
  4  --tom--> 2   --harry--> 3

  name(0,1) = chandler

  name(0,1) = chandler
  name(0,1) = joe

 */

template <class StoredEdge>
struct order_by_name
  : public std::binary_function<StoredEdge,StoredEdge,bool>
{
  bool operator()(const StoredEdge& e1, const StoredEdge& e2) const {
    // Order by target vertex, then by name.
    // std::pair's operator< does a nice job of implementing
    // lexicographical compare on tuples.
    return std::make_pair(e1.get_target(), boost::get(boost::edge_name, e1))
      < std::make_pair(e2.get_target(), boost::get(boost::edge_name, e2));
  }
};

#if !defined BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
struct ordered_set_by_nameS { };
namespace boost {
  template <class ValueType>
  struct container_gen<ordered_set_by_nameS, ValueType> {
    typedef std::multiset<ValueType, order_by_name<ValueType> > type;
  };
}
#else
struct ordered_set_by_nameS {
  template <class T>
  struct bind_ { typedef std::multiset<T, order_by_name<T> > type; };
};
namespace boost {
  template <> struct container_selector<ordered_set_by_nameS>  {
    typedef ordered_set_by_nameS type;
  };
}
#endif

namespace boost {
  template <>
  struct parallel_edge_traits<ordered_set_by_nameS> {
    typedef allow_parallel_edge_tag type;
  };
}

int
main()
{
#ifdef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION
  std::cout << "This program requires partial specialization" << std::endl;
#else
  using namespace boost;
  typedef property<edge_name_t, std::string> EdgeProperty;
  typedef adjacency_list<ordered_set_by_nameS, vecS, undirectedS,
    no_property, EdgeProperty> graph_type;
  graph_type g;

  add_edge(0, 1, EdgeProperty("joe"), g);
  add_edge(1, 2, EdgeProperty("curly"), g);
  add_edge(1, 3, EdgeProperty("dick"), g);
  add_edge(1, 3, EdgeProperty("dick"), g);
  add_edge(2, 4, EdgeProperty("tom"), g);
  add_edge(3, 4, EdgeProperty("harry"), g);
  add_edge(0, 1, EdgeProperty("chandler"), g);

  property_map<graph_type, vertex_index_t>::type id = get(vertex_index, g);
  property_map<graph_type, edge_name_t>::type name = get(edge_name, g);

  graph_traits<graph_type>::vertex_iterator i, end;
  graph_traits<graph_type>::out_edge_iterator ei, edge_end;
  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    std::cout << id[*i] << " ";
    for (boost::tie(ei, edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      std::cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    std::cout << std::endl;
  }
  std::cout << std::endl;

  bool found;
  typedef graph_traits<graph_type> Traits;
  Traits::edge_descriptor e;
  Traits::out_edge_iterator e_first, e_last;

  boost::tie(e, found) = edge(0, 1, g);
  if (found)
    std::cout << "name(0,1) = " << name[e] << std::endl;
  else
    std::cout << "not found" << std::endl;
  std::cout << std::endl;

  boost::tie(e_first, e_last) = edge_range(0, 1, g);
  while (e_first != e_last)
    std::cout << "name(0,1) = " << name[*e_first++] << std::endl;
#endif
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <nt2/include/functions/conv.hpp>
#include <nt2/include/functions/cons.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/complexify.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <complex>
#include <nt2/sdk/meta/as.hpp>

NT2_TEST_CASE_TPL ( conv,  NT2_REAL_TYPES)
{
  typedef std::complex<T> cT;
  using nt2::conv;
  using nt2::tag::conv_;
  nt2::container::table<cT> a =  nt2::complexify(nt2::_(T(1), T(4)));
  nt2::container::table<cT> b =  nt2::complexify(nt2::_(T(1), T(3)));
  nt2::container::table<cT> c(nt2::of_size(1, 0));
  nt2::container::table<cT> ab = nt2::cons<cT>(nt2::of_size(1, 6), cT(1), cT(4), cT(10), cT(16), cT(17), cT(12));
  NT2_TEST_EQUAL(ab,conv(a, b));
  NT2_TEST_EQUAL(conv(a, c), nt2::zeros(1, 3, nt2::meta::as_<cT>()));
  NT2_TEST_EQUAL(conv(c, b), nt2::zeros(1, 2, nt2::meta::as_<cT>()));
  NT2_TEST_EQUAL(conv(b, b), nt2::cons<cT>(nt2::of_size(1, 5), cT(1), cT(4), cT(10), cT(12), cT(9)));
}

NT2_TEST_CASE_TPL ( conv_2,  NT2_REAL_TYPES)
{
  typedef std::complex<T> cT;
  using nt2::conv;
  using nt2::tag::conv_;
  cT a = T(1.0);
  cT b = T(0.0);
  cT c = T(2.0);
  nt2::container::table<cT> aa =  nt2::complexify(nt2::_(T(1), T(1)));
  nt2::container::table<cT> bb =  nt2::complexify(nt2::_(T(0), T(0)));
  nt2::container::table<cT> ab = cT(0);
  NT2_TEST_EQUAL(ab,conv(aa, bb));
  NT2_TEST_EQUAL(ab,conv(a, b));
  NT2_TEST_EQUAL(c,conv(a, c));
  NT2_TEST_EQUAL(ab,conv(aa, b));
  NT2_TEST_EQUAL(ab,conv(a, bb));
}


// Copyright (C) 2019  Rhys Mainwaring
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

#include "asv_wave_sim_gazebo_plugins/CGALTypes.hh"
#include "asv_wave_sim_gazebo_plugins/Wavefield.hh"

#include <Eigen/Dense>

#include <gtest/gtest.h>

#include <iostream>
#include <memory>
#include <string>

using namespace asv;

///////////////////////////////////////////////////////////////////////////////
// Utilities

std::ostream& operator<<(std::ostream& os, const std::vector<double>& _vec)
{
  for (auto&& v : _vec )
    os << v << ", ";
  return os;
}

std::ostream& operator<<(std::ostream& os, const std::vector<Eigen::Vector2d>& _vec)
{
  for (auto&& v : _vec )
    os << v.transpose() << ", ";
  return os;
}

///////////////////////////////////////////////////////////////////////////////
// Define tests

// One dimension, one wave component
TEST(Wavefield, WaveSolver1D)
{
  struct WaveParams
  {
    double a;
    double k;
    double omega;
    double phi;
  };

  auto dispersion = [=](auto omega)
  {
    return omega * omega / 9.8;
  };

  auto wave = [=](auto x, auto t, auto& wp)
  {
    const double theta = wp.k * x - wp.omega * t;
    const double s = std::sin(theta);
    const double c = std::cos(theta);
    const double px = x - wp.a * s;
    const double pz = wp.a * c;

    return px;
  };

  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
  {
    const double theta = wp.k * x - wp.omega * t;
    const double s = std::sin(theta);
    const double c = std::cos(theta);
    const double f = p - x + wp.a * s;
    return f;
  };

  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
  {
    const double theta = wp.k * x - wp.omega * t;
    const double s = std::sin(theta);
    const double c = std::cos(theta);
    const double df = wp.a * wp.k * c - 1;
    return df;
  };

  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p, auto t, auto& wp, auto tol, auto nmax)
  {
    int n = 0;
    double err = 1;
    double xn = x0;
    while (std::abs(err) > tol && n < nmax)
    {
      const double f = func(x0, p, t, wp);
      const double df = dfunc(x0, p, t, wp);
      const double d = -f/df;
      xn = x0 + d;
      x0 = xn;
      err = f;
      n++;
    }
    return xn;
  };

  WaveParams wp;
  wp.a = 3.0;
  wp.omega = 2*M_PI/4.0;
  wp.k = dispersion(wp.omega);
  wp.phi = 0.0;

  double t = 0;
  double x0 = 2.0;

  double p = wave(x0, t, wp);
  double f  = wave_f(x0, p, t, wp);
  double df = wave_df(x0, p, t, wp);

  const double tol = 1E-8;
  const double nmax = 20;
  double x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);

  const double eps = 1E-8;
  EXPECT_NEAR(wp.a, 3.0, eps);
  EXPECT_NEAR(wp.k, 0.251775622, eps);
  EXPECT_NEAR(wp.omega, 1.570796327, eps);
  EXPECT_NEAR(x0, 2.0, eps);
  EXPECT_NEAR(p, 0.552382941, eps);
  EXPECT_LE(f, eps);
  EXPECT_NEAR(df, -0.338428477, eps);
  EXPECT_NEAR(x, 2.0, eps);

  // std::cout << "a:      " << wp.a << std::endl;
  // std::cout << "k:      " << wp.k << std::endl;
  // std::cout << "omega:  " << wp.omega << std::endl;
  // std::cout << "x0:     " << x0 << std::endl;
  // std::cout << "p:      " << p << std::endl;
  // std::cout << "f:      " << f << std::endl;
  // std::cout << "df:     " << df << std::endl;
  // std::cout << "x:      " << x << std::endl;
}

// Two dimensions, one wave component
TEST(Wavefield, WaveSolver2D)
{
  struct WaveParams
  {
    double a;
    double k;
    double omega;
    double phi;
    Eigen::Vector2d dir;
  };

  auto dispersion = [=](auto omega)
  {
    return omega * omega / 9.8;
  };

  auto wave = [=](auto x, auto t, auto& wp)
  {
    const double theta = wp.k * x.dot(wp.dir)  - wp.omega * t;
    const double s = std::sin(theta);
    const double c = std::cos(theta);
    const double dx = wp.dir.x();
    const double dy = wp.dir.y();
    const Eigen::Vector3d p(
      x.x() - wp.a * dx * s,
      x.y() - wp.a * dy * s,
      wp.a * c
    );
    return p;
  };

  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
  {
    const double theta = wp.k * x.dot(wp.dir)  - wp.omega * t;
    const double s = std::sin(theta);
    const double dx = wp.dir.x();
    const double dy = wp.dir.y();
    const Eigen::Vector2d f(
      p.x() - x.x() + wp.a * dx * s,
      p.y() - x.y() + wp.a * dy * s
    );
    return f;
  };

  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
  {
    const double theta = wp.k * x.dot(wp.dir)  - wp.omega * t;
    const double c = std::cos(theta);
    const double dx = wp.dir.x();
    const double dy = wp.dir.y();
    const double akc = wp.a * wp.k * c;
    const double df1x = akc * dx * dx - 1.0;
    const double df1y = akc * dx * dy;
    const double df2x = df1y;
    const double df2y = akc * dy * dy - 1.0;
    Eigen::Matrix2d J;
    J(0, 0) = df1x;
    J(0, 1) = df1y;
    J(1, 0) = df2x;
    J(1, 1) = df2y;
    return J;
  };

  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p, auto t, auto& wp, auto tol, auto nmax)
  {
    int n = 0;
    double err = 1;
    auto xn = x0;
    while (std::abs(err) > tol && n < nmax)
    {
      const auto F = func(x0, p, t, wp);
      const auto J = dfunc(x0, p, t, wp);
      xn = x0 - J.inverse() * F;
      x0 = xn;
      err = F.norm();
      n++;
    }
    return xn;
  };

  WaveParams wp;
  wp.a = 3.0;
  wp.omega = 2*M_PI/4.0;
  wp.k = dispersion(wp.omega);
  wp.phi = 0.0;
  wp.dir = Eigen::Vector2d(1, 0);

  double t = 0;
  Eigen::Vector2d x0(2.0, 3.0);

  auto p3 = wave(x0, t, wp);
  Eigen::Vector2d p(p3(0), p3(1));
  auto F  = wave_f(x0, p, t, wp);
  auto J = wave_df(x0, p, t, wp);

  const double tol = 1E-8;
  const double nmax = 20;
  auto x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);

  const double eps = 1E-8;
  EXPECT_NEAR(wp.a, 3.0, eps);
  EXPECT_NEAR(wp.k, 0.251775622, eps);
  EXPECT_NEAR(wp.omega, 1.570796327, eps);
  EXPECT_DOUBLE_EQ(wp.dir.x(), 1.0);
  EXPECT_DOUBLE_EQ(wp.dir.y(), 0.0);
  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
  EXPECT_DOUBLE_EQ(x.x(), 2.0);
  EXPECT_DOUBLE_EQ(x.y(), 3.0);
  EXPECT_LE(F.norm(), eps);

  // std::cout << "a:      " << wp.a << std::endl;
  // std::cout << "k:      " << wp.k << std::endl;
  // std::cout << "omega:  " << wp.omega << std::endl;
  // std::cout << "x0:     " << x0.transpose() << std::endl;
  // std::cout << "p3:     " << p3.transpose() << std::endl;
  // std::cout << "p:      " << p.transpose() << std::endl;
  // std::cout << "F:      " << F.transpose() << std::endl;
  // std::cout << "J:      " << J << std::endl;
  // std::cout << "x:      " << x.transpose() << std::endl;
}

// Two dimensions, many wave components
TEST(Wavefield, NWaveSolver2D)
{
  struct WaveParams
  {
    std::vector<double> a;
    std::vector<double> k;
    std::vector<double> omega;
    std::vector<double> phi;
    std::vector<Eigen::Vector2d> dir;
  };

  auto dispersion = [=](auto omega)
  {
    return omega * omega / 9.8;
  };

  auto wave = [=](auto x, auto t, auto& wp)
  {
    Eigen::Vector3d p(x.x(), x.y(), 0.0);
    const size_t n = wp.a.size();
    for (auto&& i=0; i<n; ++i)
    {
      const double theta = wp.k[i] * x.dot(wp.dir[i])  - wp.omega[i] * t;
      const double s = std::sin(theta);
      const double c = std::cos(theta);
      const double dx = wp.dir[i].x();
      const double dy = wp.dir[i].y();
      const double a = wp.a[i];
      p += Eigen::Vector3d(
        - a * dx * s,
        - a * dy * s,
        a * c
      );
    }
    return p;
  };

  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
  {
    Eigen::Vector2d f(p.x() - x.x(), p.y() - x.y());
    const size_t n = wp.a.size();
    for (auto&& i=0; i<n; ++i)
    {
      const double theta = wp.k[i] * x.dot(wp.dir[i])  - wp.omega[i] * t;
      const double s = std::sin(theta);
      const double dx = wp.dir[i].x();
      const double dy = wp.dir[i].y();
      const double a = wp.a[i];
      f += Eigen::Vector2d(
        a * dx * s,
        a * dy * s
      );
    }
    return f;
  };

  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
  {
    Eigen::Matrix2d J;
    J(0, 0) = -1;
    J(0, 1) =  0;
    J(1, 0) =  0;
    J(1, 1) = -1;
    const size_t n = wp.a.size();
    for (auto&& i=0; i<n; ++i)
    {
      const double theta = wp.k[i] * x.dot(wp.dir[i])  - wp.omega[i] * t;
      const double c = std::cos(theta);
      const double dx = wp.dir[i].x();
      const double dy = wp.dir[i].y();
      const double akc = wp.a[i] * wp.k[i] * c;
      const double df1x = akc * dx * dx;
      const double df1y = akc * dx * dy;
      const double df2x = df1y;
      const double df2y = akc * dy * dy;
      J(0, 0) += df1x;
      J(0, 1) += df1y;
      J(1, 0) += df2x;
      J(1, 1) += df2y;
    }
    return J;
  };

  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p, auto t, auto& wp, auto tol, auto nmax)
  {
    int n = 0;
    double err = 1;
    auto xn = x0;
    while (std::abs(err) > tol && n < nmax)
    {
      const auto F = func(x0, p, t, wp);
      const auto J = dfunc(x0, p, t, wp);
      xn = x0 - J.inverse() * F;
      x0 = xn;
      err = F.norm();
      n++;
    }
    return xn;
  };

  WaveParams wp;
  // wp.a = { 3.0, 0.0, 0.0 };
  // wp.omega = { 2*M_PI/4.0, 2*M_PI, 2*M_PI };
  wp.a = { 1.0, 2.0, 3.0 };
  wp.omega = { 2*M_PI/50.0, 2*M_PI/10.0, 2*M_PI/20.0 };
  wp.k = { dispersion(wp.omega[0]), dispersion(wp.omega[1]), dispersion(wp.omega[2]) };
  wp.phi = { 0.0, 0.0, 0.0 };
  wp.dir = { Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0) };

  double t = 0;
  Eigen::Vector2d x0(2.0, 3.0);

  auto p3 = wave(x0, t, wp);
  Eigen::Vector2d p(p3(0), p3(1));
  auto F  = wave_f(x0, p, t, wp);
  auto J = wave_df(x0, p, t, wp);

  const double tol = 1E-8;
  const double nmax = 20;
  auto x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);

  const double eps = 1E-8;
  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
  EXPECT_DOUBLE_EQ(x.x(), 2.0);
  EXPECT_DOUBLE_EQ(x.y(), 3.0);
  EXPECT_LE(F.norm(), eps);

  // std::cout << "a:      " << wp.a << std::endl;
  // std::cout << "k:      " << wp.k << std::endl;
  // std::cout << "omega:  " << wp.omega << std::endl;
  // std::cout << "dir:    " << wp.dir << std::endl;
  // std::cout << "x0:     " << x0.transpose() << std::endl;
  // std::cout << "p3:     " << p3.transpose() << std::endl;
  // std::cout << "p:      " << p.transpose() << std::endl;
  // std::cout << "F:      " << F.transpose() << std::endl;
  // std::cout << "J:      " << J << std::endl;
  // std::cout << "x:      " << x.transpose() << std::endl;
}

// Two dimensions, many wave components, combined function and jacobian calculation
TEST(Wavefield, NWaveFdFSolver2D)
{
  struct WaveParams
  {
    std::vector<double> a;
    std::vector<double> k;
    std::vector<double> omega;
    std::vector<double> phi;
    std::vector<Eigen::Vector2d> dir;
  };

  auto dispersion = [=](auto omega)
  {
    return omega * omega / 9.8;
  };

  auto wave = [=](auto x, auto t, auto& wp)
  {
    Eigen::Vector3d p(x.x(), x.y(), 0.0);
    const size_t n = wp.a.size();
    for (auto&& i=0; i<n; ++i)
    {
      const double theta = wp.k[i] * x.dot(wp.dir[i])  - wp.omega[i] * t;
      const double s = std::sin(theta);
      const double c = std::cos(theta);
      const double dx = wp.dir[i].x();
      const double dy = wp.dir[i].y();
      const double a = wp.a[i];
      p += Eigen::Vector3d(
        - a * dx * s,
        - a * dy * s,
        a * c
      );
    }
    return p;
  };

  auto wave_fdf = [=](auto x, auto p, auto t, auto& wp, auto& F, auto& J)
  {
    F(0) = p.x() - x.x();
    F(1) = p.y() - x.y();
    J(0, 0) = -1;
    J(0, 1) =  0;
    J(1, 0) =  0;
    J(1, 1) = -1;
    const size_t n = wp.a.size();
    for (auto&& i=0; i<n; ++i)
    {
      const double theta = wp.k[i] * x.dot(wp.dir[i])  - wp.omega[i] * t;
      const double s = std::sin(theta);
      const double c = std::cos(theta);
      const double dx = wp.dir[i].x();
      const double dy = wp.dir[i].y();
      const double a  = wp.a[i];
      const double akc = a * wp.k[i] * c;
      const double df1x = akc * dx * dx;
      const double df1y = akc * dx * dy;
      const double df2x = df1y;
      const double df2y = akc * dy * dy;
      F(0) += a * dx * s;
      F(1) += a * dy * s;
      J(0, 0) += df1x;
      J(0, 1) += df1y;
      J(1, 0) += df2x;
      J(1, 1) += df2y;
    }
  };

  auto solver = [=](auto& fdfunc, auto x0, auto p, auto t, auto& wp, auto tol, auto nmax)
  {
    int n = 0;
    double err = 1;
    auto xn = x0;
    Eigen::Vector2d F;
    Eigen::Matrix2d J;
    while (std::abs(err) > tol && n < nmax)
    {
      fdfunc(x0, p, t, wp, F, J);
      xn = x0 - J.inverse() * F;
      x0 = xn;
      err = F.norm();
      n++;
    }
    return xn;
  };

  WaveParams wp;
  wp.a = { 1.0, 2.0, 3.0 };
  wp.omega = { 2*M_PI/50.0, 2*M_PI/10.0, 2*M_PI/20.0 };
  wp.k = { dispersion(wp.omega[0]), dispersion(wp.omega[1]), dispersion(wp.omega[2]) };
  wp.phi = { 0.0, 0.0, 0.0 };
  wp.dir = { Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0) };

  double t = 0;
  Eigen::Vector2d x0(2.0, 3.0);

  auto p3 = wave(x0, t, wp);
  Eigen::Vector2d p(p3(0), p3(1));
  Eigen::Vector2d F;
  Eigen::Matrix2d J;
  wave_fdf(x0, p, t, wp, F, J);

  const double tol = 1E-8;
  const double nmax = 20;
  auto x = solver(wave_fdf, p, p, t, wp, tol, nmax);

  const double eps = 1E-8;
  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
  EXPECT_DOUBLE_EQ(x.x(), 2.0);
  EXPECT_DOUBLE_EQ(x.y(), 3.0);
  EXPECT_LE(F.norm(), eps);
}

///////////////////////////////////////////////////////////////////////////////
// Run tests

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include <bio/markov_model.h>
#include <bio/sequence.h>
USING_BIO_NS

#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost;

#include <iostream>
using namespace std;

//#define VERBOSE_CHECKING

const SeqList symbol_seqs = boost::assign::list_of
	("")
	("N")
	("A")
	("AN")
	("NA")
	("NAC")
	("AC")
	("ACG")
	("ACGT")
	("NACGT")
	("ANCGT")
	("ACNGT")
	("ACGNT")
	("ACGTN")
	("ACGTNACGT")
	;

template <typename MM>
struct MMCounter
{
	MM & mm;

	MMCounter(MM & mm)
		: mm(mm)
	{
	}

	void operator()(const seq_t & sequence)
	{
		mm.add_to_counts(sequence.begin(), sequence.end(), DnaSymbolAlphabet());
	}
};

BOOST_TEST_DONT_PRINT_LOG_VALUE( seq_t::const_iterator );

template <unsigned order>
void
check_symbol_iterator()
{
	cout << "******* check_symbol_iterator() : order = " << order << endl;

	const SeqList symbol_seqs = boost::assign::list_of
		("")
		("N")
		("A")
		("AN")
		("NA")
		("NAC")
		("AC")
		("ACG")
		("ACGT")
		("NACGT")
		("ANCGT")
		("ACNGT")
		("ACGNT")
		("ACGTN")
		("ACGTNACGT")
		;

	for (SeqList::const_iterator s = symbol_seqs.begin();
		symbol_seqs.end() != s;
		++s)
	{
		typedef
			typename MarkovModel<order>::template SymbolIterator<
				typename seq_t::const_iterator,
				DnaSymbolAlphabet>
			SymbolIt;

#ifdef VERBOSE_CHECKING
		cout << "Sequence: \"" << s->c_str() << "\"\n";
#endif

		for (SymbolIt it(s->begin(), s->end(), DnaSymbolAlphabet());
			s->end() != it.begin;
			it.increment())
		{

#ifdef VERBOSE_CHECKING
			copy( it.begin, it.end, ostream_iterator< char >( cout ));
			cout << "\n";
#endif

			BOOST_CHECK_EQUAL(it.end, std::find(it.begin, it.end, 'N'));
		}

#ifdef VERBOSE_CHECKING
		cout << "\n";
#endif

	}
}

template <unsigned order>
void
check_markov_model()
{
	cout << "******* check_markov_model() : order = " << order << endl;

	typedef MarkovModel<order> mm_t;
	mm_t mm(4);
	std::for_each(symbol_seqs.begin(), symbol_seqs.end(), MMCounter<mm_t>(mm));

#ifdef VERBOSE_CHECKING
	typedef boost::array< unsigned, order + 1> indices_t;
	indices_t indices;
	std::fill( indices.begin(), indices.end(), 0 );
	DnaSymbolAlphabet alphabet;

	cout << "Total : " << mm.total_count << "\n";
	while (true)
	{
		//print the symbol
		for (indices_t::const_iterator i = indices.begin();
			indices.end() != i;
			++i)
		{
			cout << alphabet.get_char(*i);
		}
		cout << " : " << mm.counts(indices) << "\n";

		//increment the indices
		for (unsigned i = order + 1;
			0 != i;
			--i)
		{
			++indices[i - 1];
			if (DnaSymbolAlphabet::get_alphabet_size() == indices[i - 1])
			{
				indices[i - 1] = 0;

				if (1 == i)
				{
					return;
				}
			}
			else
			{
				break;
			}
		}
	}
#endif
}

void
register_markov_model_tests(boost::unit_test::test_suite * test)
{
	test->add( BOOST_TEST_CASE( &check_symbol_iterator<0> ), 0);
	test->add( BOOST_TEST_CASE( &check_symbol_iterator<1> ), 0);
	test->add( BOOST_TEST_CASE( &check_symbol_iterator<2> ), 0);
	test->add( BOOST_TEST_CASE( &check_markov_model<0> ), 0);
	test->add( BOOST_TEST_CASE( &check_markov_model<1> ), 0);
	test->add( BOOST_TEST_CASE( &check_markov_model<2> ), 0);
}


/**
 * @file LinearConstraint.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <optional>
#include <string>

#include <Eigen/Dense>

#include <ScsEigen/LinearConstraint.h>
#include <ScsEigen/Logger.h>

using namespace ScsEigen;

LinearConstraint::LinearConstraint(const Eigen::Ref<const Eigen::MatrixXd>& A,
                                   const Eigen::Ref<const Eigen::VectorXd>& lowerBound,
                                   const Eigen::Ref<const Eigen::VectorXd>& upperBound,
                                   std::optional<Eigen::Ref<const SparsityPattern>> sparsityPattern)
    : Constraint(A.cols(), "Linear constraint")
{
    constexpr double tolerance = 1e-10;

    if (!this->setBounds(lowerBound, upperBound))
    {
        log()->error("[LinearConstraint::LinearConstraint] Unable to set the bounds.");
        assert(false);
    }

    if (A.rows() != this->getNumberOfConstraints())
    {
        log()->error("[LinearConstraint::LinearConstraint] Size mismatch for the constraint "
                     "matrix.");
        assert(false);
    } else
    {

        m_A = A;
    }

    if (!sparsityPattern.has_value())
        m_ASparsityPattern = (m_A.array().abs() > tolerance);
    else if (sparsityPattern->size() != m_A.size())
    {
        log()->error("[LinearConstraint::LinearConstraint] Size mismatch for the sparsity "
                     "pattern.");
        assert(false);
    } else
    {
        m_ASparsityPattern = sparsityPattern.value();
    }
}

bool LinearConstraint::setA(const Eigen::Ref<const Eigen::MatrixXd>& A,
                            std::optional<Eigen::Ref<const SparsityPattern>> sparsityPattern)
{
    if (m_A.size() != 0)
    {
        if (A.size() != m_A.size())
        {
            log()->error("[LinearConstraint::setA] The size of the matrix 'A' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(m_A.cols()))
    {
        log()->error("[LinearConstraint::setA] Unable to set the number of variables.");
        return false;
    } else if (!this->setNumberOfConstraints(m_A.rows()))
    {
        log()->error("[LinearConstraint::setA] Unable to set the number of constraints.");
        return false;
    }

    m_A = A;

    // check if sparsity pattern has been already set
    //
    if (m_ASparsityPattern.size() == 0)
    {
        if (!sparsityPattern.has_value())
        {
            constexpr double tolerance = 1e-10;
            m_ASparsityPattern = (m_A.array().abs() > tolerance);
        } else if (sparsityPattern->size() != m_A.size())
        {
            log()->error("[LinearConstraint::setA] Size mismatch for the sparsity pattern.");
            assert(false);
            return false;
        } else
        {
            m_ASparsityPattern = sparsityPattern.value();
        }
    } else
    {
        log()->debug("[LinearConstraint::setA] The sparsity pattern is set only once.");
    }
    return true;
}

Eigen::Ref<const Eigen::MatrixXd> LinearConstraint::getA() const
{
    return m_A;
}

Eigen::Ref<const LinearConstraint::SparsityPattern> LinearConstraint::getASparsityPattern() const
{
    return m_ASparsityPattern;
}


//=======================================================================
// Copyright (C) 2012 Flavio De Lorenzi (fdlorenzi@gmail.com)
//
// 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/graph/adjacency_list.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
using namespace boost;

int main() {

  typedef adjacency_list<setS, vecS, bidirectionalS> graph_type;

  // Build graph1
  int num_vertices1 = 8; graph_type graph1(num_vertices1);
  add_edge(0, 6, graph1); add_edge(0, 7, graph1);
  add_edge(1, 5, graph1); add_edge(1, 7, graph1);
  add_edge(2, 4, graph1); add_edge(2, 5, graph1); add_edge(2, 6, graph1);
  add_edge(3, 4, graph1);

  // Build graph2
  int num_vertices2 = 9; graph_type graph2(num_vertices2);
  add_edge(0, 6, graph2); add_edge(0, 8, graph2);
  add_edge(1, 5, graph2); add_edge(1, 7, graph2);
  add_edge(2, 4, graph2); add_edge(2, 7, graph2); add_edge(2, 8, graph2);
  add_edge(3, 4, graph2); add_edge(3, 5, graph2); add_edge(3, 6, graph2);

  // Create callback to print mappings
  vf2_print_callback<graph_type, graph_type> callback(graph1, graph2);

  // Print out all subgraph isomorphism mappings between graph1 and graph2.
  // Vertices and edges are assumed to be always equivalent.
  vf2_subgraph_iso(graph1, graph2, callback);

  return 0;
}


//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@even-ag.ch)
// ETH Zurich, Center of Structure Technologies (www.imes.ethz.ch/st)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

#ifndef BOOST_GRAPH_WAVEFRONT_HPP
#define BOOST_GRAPH_WAVEFRONT_HPP

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/detail/numeric_traits.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/config/no_tr1/cmath.hpp>
#include <vector>
#include <algorithm> // for std::min and std::max

namespace boost {

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  ith_wavefront(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g,
                VertexIndexMap index)
  {
    typename graph_traits<Graph>::vertex_descriptor v, w;
    typename graph_traits<Graph>::vertices_size_type b = 1;
    typename graph_traits<Graph>::out_edge_iterator edge_it2, edge_it2_end;
    typename graph_traits<Graph>::vertices_size_type index_i = index[i];
    std::vector<bool> rows_active(num_vertices(g), false);

    rows_active[index_i] = true;

      typename graph_traits<Graph>::vertex_iterator ui, ui_end;
      for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
      {
        v = *ui;
          if(index[v] <= index_i)
            {
              for (boost::tie(edge_it2, edge_it2_end) = out_edges(v, g); edge_it2 != edge_it2_end; ++edge_it2)
              {
                w = target(*edge_it2, g);
                if( (index[w] >= index_i) && (!rows_active[index[w]]) )
                  {
                    b++;
                    rows_active[index[w]] = true;
                  }
              }
            }
      }

    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  ith_wavefront(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g)
  {
    return ith_wavefront(i, g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  max_wavefront(const Graph& g, VertexIndexMap index)
  {
    BOOST_USING_STD_MAX();
    typename graph_traits<Graph>::vertices_size_type b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
      b = max BOOST_PREVENT_MACRO_SUBSTITUTION(b, ith_wavefront(*i, g, index));
    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  max_wavefront(const Graph& g)
  {
    return max_wavefront(g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  double
  aver_wavefront(const Graph& g, VertexIndexMap index)
  {
    double b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
      b += ith_wavefront(*i, g, index);

    b /= num_vertices(g);
    return b;
  }

  template <typename Graph>
  double
  aver_wavefront(const Graph& g)
  {
    return aver_wavefront(g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  double
  rms_wavefront(const Graph& g, VertexIndexMap index)
  {
    double b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (boost::tie(i, end) = vertices(g); i != end; ++i)
      b += std::pow(double ( ith_wavefront(*i, g, index) ), 2.0);

    b /= num_vertices(g);

    return std::sqrt(b);
  }

  template <typename Graph>
  double
  rms_wavefront(const Graph& g)
  {
    return rms_wavefront(g, get(vertex_index, g));
  }

} // namespace boost

#endif // BOOST_GRAPH_WAVEFRONT_HPP


#ifndef _VECTOR_HPP_
#define _VECTOR_HPP_

#include <boost/array.hpp>

template<size_t n, typename T = double>
class Vector  {
    boost::array<T,n> elems __attribute__ ((aligned(16)));
public:
    Vector() {}

    Vector(const T *v) {
	for (uint i = 0; i < n; ++i) {
	    this->elems[i] = v[i];
	}
    }

    template<class C>
    Vector(const C &v) {
	for (uint i = 0; i < n; ++i) {
	    this->elems[i] = v[i];
	}
    }

    typedef T* pointer;
    typedef const T* const_pointer;
    operator pointer() { return elems.c_array(); }
    operator const_pointer() const { return elems.c_array(); }

    T* data() { return elems.data(); }
    const T* data() const { return elems.data(); }

    T& operator[](uint i)  { return elems[i]; }
    const T& operator[](uint i) const { return elems[i]; }

    T inner() const {
	T q = 0.0;
	for (uint i = 0; i < n; ++i) {
	    q += (*this)[i]*(*this)[i];
	}
	return q;
    }

    Vector operator-(const Vector &rhs) const {
	Vector v;
	for (uint i = 0; i < n; ++i) {
	    v[i] = (*this)[i] - rhs[i];
	}
	return v;
    }

    Vector& operator/=(T a) { divide(*this, a); return *this; }

    static Vector center(T w1, const Vector &v1,
			 T w2, const Vector &v2) {
	Vector v;
	T w = 1.0/(w1 + w2);
	for (uint i = 0; i < n; ++i) {
	    v[i] = (w1*v1[i] + w2*v2[i])*w;
	}
	return v;
    }

};

namespace {

    template<size_t N, typename T>
    void divide(Vector<N,T> &v, T a) {
	for (uint i = 0; i < N; ++i) v[i] /= a;
    }

    template<typename T>
    void divide(Vector<0,T> &v, T a) { }
}

#endif /* _VECTOR_HPP_ */


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#include <gtest/gtest.h>
#include <gSparse/UndirectedGraph.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
#include <string>
#include <fstream>

/*******************************************************
 * Set up and utility functions
 * ******************************************************/

class DummyReader : public gSparse::IGraphReader
{
public:
	DummyReader() {}
	void Read(gSparse::EdgeMatrix & Edges,
		gSparse::PrecisionRowMatrix & Weights)
	{

		Edges = gSparse::EdgeMatrix(3, 2);
		Weights = gSparse::PrecisionMatrix(3, 1);

		Edges(0, 0) = 0; Edges(0, 1) = 1;
		Edges(1, 0) = 1; Edges(1, 1) = 2;
		Edges(2, 0) = 2; Edges(2, 1) = 3;

		Weights << 1, 2, 3;
	}
};

/*******************************************************
 * Test Suite
 * ******************************************************/

// Disable this test case, as support for copy by value is disabled.
/*TEST(UndirectedGraph, AssignmentOperator)
{
    // Generate data
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
	Weights << 1, 2, 3;

    gSparse::EdgeMatrix Edges_2(3, 2);
	gSparse::PrecisionMatrix Weights_2(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 2; Edges(1, 1) = 3;
	Edges(2, 0) = 4; Edges(2, 1) = 5;
	Weights_2 << 2, 2, 2;

    // Generate test objects
    gSparse::UndirectedGraph test_1(Edges, Weights);
    gSparse::UndirectedGraph test_2(Edges_2, Weights_2);

    // Test Assignment operator operator
    test_2 = test_1;
    EXPECT_EQ(test_2.GetWeightList(),test_1.GetWeightList());
    EXPECT_EQ(test_2.GetEdgeList(),test_1.GetEdgeList());
}
*/

TEST(UndirectedGraph, Constructor)
{
    // Call constructors
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
	Weights << 1, 2, 3;

    // Call constructors
    gSparse::UndirectedGraph test_1(Edges, Weights);
    gSparse::Graph test_2(new gSparse::UndirectedGraph(Edges, Weights));

    // Test constructors
    EXPECT_EQ(test_1.GetEdgeList(),test_2->GetEdgeList());
}

TEST(UndirectedGraph, BuildEdgeOnly)
{
    // When weight is not specified, it should defaults to one.

    // Build validation data
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 1, 1;

    // Construct the graph
    gSparse::UndirectedGraph test(Edges);
    gSparse::UndirectedGraph validation(Edges,Weights);

    // Validates
    EXPECT_EQ(test.GetEdgeList(),validation.GetEdgeList());
    EXPECT_EQ(test.GetWeightList(),validation.GetWeightList());
}

// Test GraphReader object
TEST(UndirectedGraph, GraphReader)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
	Weights << 1, 2, 3;
	gSparse::GraphReader dummyReader(new DummyReader);
	gSparse::UndirectedGraph test_1(dummyReader);
	gSparse::UndirectedGraph test_2(Edges, Weights);
    EXPECT_EQ(test_1.GetEdgeList(),test_2.GetEdgeList());
    EXPECT_EQ(test_1.GetWeightList(),test_2.GetWeightList());
}

TEST(UndirectedGraph, UnitGraph)
{
	gSparse::EdgeMatrix Edges(3, 2);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
	gSparse::UndirectedGraph test(Edges);
	EXPECT_EQ(test.GetWeightList(), gSparse::PrecisionMatrix::Ones(3,1));
}

// Test if the edge list is initialized properly
TEST(UndirectedGraph, Edges)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    gSparse::UndirectedGraph test(Edges);

    EXPECT_EQ(Edges, test.GetEdgeList());
}

// Test if the weight list is initialized properly
TEST(UndirectedGraph, Weights)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;
    gSparse::UndirectedGraph test(Edges,Weights);

    EXPECT_EQ(Edges, test.GetEdgeList());
    EXPECT_EQ(Weights, test.GetWeightList());
}

// Check if the count of edges and vertices are good
TEST(UndirectedGraph, Counts)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    gSparse::UndirectedGraph test(Edges,Weights);
    EXPECT_EQ(3, test.GetEdgeCount());
    EXPECT_EQ(4, test.GetNodeCount());
}

TEST(UndirectedGraph, AdjacencyMatrix)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    Eigen::MatrixXd ValidateAdjacencyMatrix(4, 4);
	ValidateAdjacencyMatrix <<
        0, 1, 0, 0,
	    1, 0, 2, 0,
		0, 2, 0, 3,
		0, 0, 3, 0;
    gSparse::UndirectedGraph test(Edges,Weights);
    EXPECT_EQ(ValidateAdjacencyMatrix, test.GetAdjacentMatrix().toDense());
}
TEST(UndirectedGraph, WeightMatrix)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    gSparse::PrecisionMatrix ValidateWeightMatrix(3, 3);
	ValidateWeightMatrix <<
		1, 0, 0,
		0, 2, 0,
		0, 0, 3;

    gSparse::UndirectedGraph test(Edges, Weights);
    EXPECT_EQ(test.GetWeightMatrix().toDense(), ValidateWeightMatrix);
}
TEST(UndirectedGraph, DegreeMatrix)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    gSparse::PrecisionMatrix ValidateDegreeMatrix(4, 4);
	ValidateDegreeMatrix <<
        1, 0, 0, 0,
	    0, 3, 0, 0,
		0, 0, 5, 0,
		0, 0, 0, 3;
    gSparse::UndirectedGraph test(Edges, Weights);
    EXPECT_EQ(test.GetDegreeMatrix().toDense(), ValidateDegreeMatrix);
}
TEST(UndirectedGraph, LaplacianMatrix)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    gSparse::PrecisionMatrix ValidateWeightedLaplacian(4, 4);
	ValidateWeightedLaplacian <<
         1, -1,  0,  0,
	    -1,  3, -2,  0,
		 0, -2,  5, -3,
		 0,  0, -3,  3;
    gSparse::UndirectedGraph test(Edges, Weights);
    EXPECT_EQ(test.GetLaplacianMatrix().toDense(), ValidateWeightedLaplacian);
}
TEST(UndirectedGraph, IncidenceMatrix)
{
    gSparse::EdgeMatrix Edges(3, 2);
	gSparse::PrecisionMatrix Weights(3, 1);
	Edges(0, 0) = 0; Edges(0, 1) = 1;
	Edges(1, 0) = 1; Edges(1, 1) = 2;
	Edges(2, 0) = 2; Edges(2, 1) = 3;
    Weights << 1, 2, 3;

    gSparse::PrecisionMatrix ValidateOrientedIncidentMatrix(3, 4);
	ValidateOrientedIncidentMatrix <<
		1, -1, 0, 0,
		0, 1, -1, 0,
		0, 0, 1, -1;
    gSparse::UndirectedGraph test(Edges, Weights);
    EXPECT_EQ(test.GetIncidentMatrix().toDense(), ValidateOrientedIncidentMatrix);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*
 * Copyright 2020 Adobe. All rights reserved.
 * This file is licensed to you 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 REPRESENTATIONS
 * OF ANY KIND, either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
 */
#include <Eigen/Core>
#include <lagrange/testing/common.h>
#include <iostream>

#include <lagrange/Connectivity.h>
#include <lagrange/Mesh.h>
#include <lagrange/common.h>
#include <lagrange/create_mesh.h>

TEST_CASE("ConnectivitySimpleTriangleMesh", "[connectivity][triangle_mesh][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(3, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0;
    Triangles facets(1, 3);
    facets << 0, 1, 2;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_connectivity();

    SECTION("Vertex-vertex adjacency")
    {
        for (int i = 0; i < 3; i++) {
            const auto& adj_v = mesh->get_vertices_adjacent_to_vertex(i);
            REQUIRE(adj_v.size() == 2);
            REQUIRE(adj_v[0] != adj_v[1]);
            REQUIRE(adj_v[0] != i);
            REQUIRE(adj_v[1] != i);
        }
    }

    SECTION("Vertex-facet adjacency")
    {
        for (int i = 0; i < 3; i++) {
            const auto& adj_f = mesh->get_facets_adjacent_to_vertex(i);
            REQUIRE(adj_f.size() == 1);
            REQUIRE(adj_f[0] == 0);
        }
    }

    SECTION("Facet-facet adjacency")
    {
        const auto& adj_f = mesh->get_facets_adjacent_to_facet(0);
        REQUIRE(adj_f.empty());
    }
}

TEST_CASE("ConnectivityTriangleMesh", "[connectivity][triangle_mesh][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(4, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
    Triangles facets(4, 3);
    facets << 0, 2, 1, 0, 1, 3, 1, 2, 3, 2, 0, 3;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_connectivity();

    using Index = decltype(mesh)::element_type::Index;

    SECTION("Vertex-vertex adjacency")
    {
        for (Index i = 0; i < 4; i++) {
            const auto& adj_v = mesh->get_vertices_adjacent_to_vertex(i);
            REQUIRE(adj_v.size() == 3);
        }
        for (Index i = 0; i < 4; i++) {
            for (Index j = 0; j < 3; j++) {
                const Index curr_v = facets(i, j);
                const Index next_v = facets(i, (j + 1) % 3);
                const auto& adj_curr_v = mesh->get_vertices_adjacent_to_vertex(curr_v);
                const auto next_itr = std::find(adj_curr_v.begin(), adj_curr_v.end(), next_v);
                REQUIRE(next_itr != adj_curr_v.end());

                const auto& adj_next_v = mesh->get_vertices_adjacent_to_vertex(next_v);
                const auto curr_itr = std::find(adj_next_v.begin(), adj_next_v.end(), curr_v);
                REQUIRE(curr_itr != adj_next_v.end());
            }
        }
    }

    SECTION("Vertex-facet adjacency")
    {
        for (Index i = 0; i < 4; i++) {
            const auto& adj_f = mesh->get_facets_adjacent_to_vertex(i);
            REQUIRE(adj_f.size() == 3);
        }

        for (Index i = 0; i < 4; i++) {
            for (Index j = 0; j < 3; j++) {
                const Index v = facets(i, j);
                const auto& adj_f = mesh->get_facets_adjacent_to_vertex(v);
                const auto f_itr = std::find(adj_f.begin(), adj_f.end(), i);
                REQUIRE(f_itr != adj_f.end());
            }
        }
    }

    SECTION("Facet-facet adjacency")
    {
        for (Index i = 0; i < 4; i++) {
            const auto& adj_f = mesh->get_facets_adjacent_to_facet(i);
            REQUIRE(adj_f.size() == 3);
            const auto& self_itr = std::find(adj_f.begin(), adj_f.end(), i);
            REQUIRE(self_itr == adj_f.end());
        }
    }
}

TEST_CASE("ConnectivityQuadMesh", "[connectivity][quad_mesh][simple]")
{
    using namespace lagrange;

    Vertices3D vertices(8, 3);
    vertices << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0,
        1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0;
    Quads facets(6, 4);
    facets << 0, 2, 3, 1, 4, 5, 6, 7, 4, 0, 1, 5, 2, 7, 6, 3, 6, 5, 1, 3, 4, 7, 2, 0;

    auto mesh = create_mesh(vertices, facets);
    mesh->initialize_connectivity();

    using Index = decltype(mesh)::element_type::Index;

    SECTION("Vertex-vertex adjacency")
    {
        for (Index i = 0; i < 8; i++) {
            const auto& v_adj = mesh->get_vertices_adjacent_to_vertex(i);
            REQUIRE(v_adj.size() == 3);
        }
        for (Index i = 0; i < 6; i++) {
            for (Index j = 0; j < 4; j++) {
                const Index curr_v = facets(i, j);
                const Index next_v = facets(i, (j + 1) % 4);
                const auto& adj_curr_v = mesh->get_vertices_adjacent_to_vertex(curr_v);
                const auto next_itr = std::find(adj_curr_v.begin(), adj_curr_v.end(), next_v);
                REQUIRE(next_itr != adj_curr_v.end());

                const auto& adj_next_v = mesh->get_vertices_adjacent_to_vertex(next_v);
                const auto curr_itr = std::find(adj_next_v.begin(), adj_next_v.end(), curr_v);
                REQUIRE(curr_itr != adj_next_v.end());
            }
        }
    }

    SECTION("Vertex-facet adjacency")
    {
        for (Index i = 0; i < 8; i++) {
            const auto& adj_f = mesh->get_facets_adjacent_to_vertex(i);
            REQUIRE(adj_f.size() == 3);
        }

        for (Index i = 0; i < 6; i++) {
            for (Index j = 0; j < 4; j++) {
                const Index v = facets(i, j);
                const auto& adj_f = mesh->get_facets_adjacent_to_vertex(v);
                const auto f_itr = std::find(adj_f.begin(), adj_f.end(), i);
                REQUIRE(f_itr != adj_f.end());
            }
        }
    }

    SECTION("Facet-facet adjacency")
    {
        for (Index i = 0; i < 6; i++) {
            const auto& adj_f = mesh->get_facets_adjacent_to_facet(i);
            REQUIRE(adj_f.size() == 4);
            const auto& self_itr = std::find(adj_f.begin(), adj_f.end(), i);
            REQUIRE(self_itr == adj_f.end());
        }
    }
}


/* boost random/poisson_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: poisson_distribution.hpp,v 1.9 2002/12/22 22:03:10 jmaurer Exp $
 *
 */

#ifndef BOOST_RANDOM_POISSON_DISTRIBUTION_HPP
#define BOOST_RANDOM_POISSON_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// Knuth
template<class UniformRandomNumberGenerator, class IntType = int,
         class RealType = double,
         class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class poisson_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  explicit poisson_distribution(base_type & rng,
                                const RealType& mean = RealType(1))
    : _rng(rng), _mean(mean)
  {
    assert(mean > RealType(0));
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  RealType mean() const { return _mean; }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
    // TODO: This is O(_mean), but it should be O(log(_mean)) for large _mean
    RealType product = RealType(1);
    for(int m = 0; ; ++m) {
      product *= _rng();
      if(product <= _exp_mean)
        return m;
    }
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const poisson_distribution& x,
                         const poisson_distribution& y)
  {
    return x._mean == y._mean && x._rng == y._rng;
  }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const poisson_distribution& pd)
  {
    os << pd._mean;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, poisson_distribution& pd)
  {
    is >> std::ws >> pd._mean;
    pd.init();
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const poisson_distribution& rhs) const
  {
    return _mean == rhs._mean && _rng == rhs._rng;
  }
#endif

private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp;
#endif
    _exp_mean = exp(-_mean);
  }

  adaptor_type _rng;
  RealType _mean;
  // some precomputed data from the parameters
  RealType _exp_mean;
};

} // namespace boost

#endif // BOOST_RANDOM_POISSON_DISTRIBUTION_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

#ifndef __PGI
#include <boost/lambda/lambda.hpp>
#endif

int main(int , char**)
{
#ifndef __PGI
    using namespace std;
    using boost::lambda::_1;

    mtl::compressed2D<double> A;
    laplacian_setup(A, 3, 4);

    assign_each_nonzero(A, constant_ref(_1) * constant_ref(_1));
    mtl::io::tout << "A is\n" << A << std::endl;

    MTL_THROW_IF(abs(A[0][0] - 16.0) > 0.001, mtl::runtime_error("Wrong value in diagonal"));
    MTL_THROW_IF(abs(A[0][1] - 1.0) > 0.001, mtl::runtime_error("Wrong value in off-diagonal"));
    MTL_THROW_IF(abs(A[0][2]) > 0.001, mtl::runtime_error("Entry should be empty"));
#endif

    return 0;
}


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

#ifndef BOOST_MPL11_LOGICAL_HPP
#define BOOST_MPL11_LOGICAL_HPP

#include <boost/mpl11/fwd/logical.hpp>

#include <boost/mpl11/bool.hpp>
#include <boost/mpl11/detail/right_folds/variadic.hpp>

namespace boost { namespace mpl11 {
    namespace logical_detail {
        template <bool cond>
        struct if_impl;

        template <>
        struct if_impl<true> {
            template <typename Then, typename Else>
            using result = Then;
        };

        template <>
        struct if_impl<false> {
            template <typename Then, typename Else>
            using result = Else;
        };

        template <typename x, typename y>
        struct and2
            : bool_<
                (bool)if_c<(bool)x::type::value, y, x>::type::value
            >
        { };

        template <typename x, typename y>
        struct or2
            : bool_<
                (bool)if_c<(bool)x::type::value, x, y>::type::value
            >
        { };
    } // end namespace logical_detail

    template <typename ...xs>
    struct and_
        : detail::right_folds::variadic<logical_detail::and2, true_, xs...>
    { };

    template <typename ...xs>
    struct or_
        : detail::right_folds::variadic<logical_detail::or2, false_, xs...>
    { };

    template <typename x>
    struct not_
        : bool_<!x::type::value>
    { };

    template <typename Condition, typename Then, typename Else>
    struct if_ :
        logical_detail::if_impl<(bool)Condition::type::value>::
        template result<Then, Else>
    { };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_LOGICAL_HPP


#pragma once
#include <stdexcept>
#include <vector>
#include <memory>
#include <math.h>
#include <limits>
#include <Eigen/Dense>
#include "SdfObject.hpp"
#include "SdfCombinedObject.hpp"
#include "../accelerate/Bound3.hpp"

class SdfCombinedObject : public SdfObject
{
public:
    float sdf(const Eigen::Vector3f &position) const
    {
        auto min = std::numeric_limits<float>::max();
        for (auto &object : objects)
        {
            // TODO: support mix function
            if (auto sdf = object->sdf(position); sdf < min)
                min = sdf;
        }

        return min;
    };

    void add(std::unique_ptr<SdfObject> object) { objects.emplace_back(std::move(object)); };

    std::unique_ptr<Bound3> build_bound3() const override
    {
        const auto size = objects.size();
        if (size == 0)
            throw std::runtime_error("Build bound3 before add any object");

        if (size == 1)
            return objects[0]->build_bound3();

        auto bound3 = objects[0]->build_bound3();
        for (int i = 1; i < size; i++)
            bound3 = Bound3::union_bound3(bound3, objects[i]->build_bound3());

        return bound3;
    };

protected:
    std::vector<std::unique_ptr<SdfObject>> objects;
};


// Because of Mosek complications, we don't use static library if Mosek is used.
#ifdef LIBIGL_WITH_MOSEK
#ifdef IGL_STATIC_LIBRARY
#undef IGL_STATIC_LIBRARY
#endif
#endif

#include <igl/boundary_conditions.h>
#include <igl/colon.h>
#include <igl/column_to_quats.h>
#include <igl/directed_edge_parents.h>
#include <igl/forward_kinematics.h>
#include <igl/jet.h>
#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>
#include <igl/normalize_row_sums.h>
#include <igl/readDMAT.h>
#include <igl/readMESH.h>
#include <igl/readTGF.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/bbw.h>
//#include <igl/embree/bone_heat.h>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <iostream>

#include "tutorial_shared_path.h"

typedef
  std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond> >
  RotationList;

const Eigen::RowVector3d sea_green(70./255.,252./255.,167./255.);
int selected = 0;
Eigen::MatrixXd V,W,U,C,M;
Eigen::MatrixXi T,F,BE;
Eigen::VectorXi P;
RotationList pose;
double anim_t = 1.0;
double anim_t_dir = -0.03;

bool pre_draw(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(viewer.core.is_animating)
  {
    // Interpolate pose and identity
    RotationList anim_pose(pose.size());
    for(int e = 0;e<pose.size();e++)
    {
      anim_pose[e] = pose[e].slerp(anim_t,Quaterniond::Identity());
    }
    // Propagate relative rotations via FK to retrieve absolute transformations
    RotationList vQ;
    vector<Vector3d> vT;
    igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);
    const int dim = C.cols();
    MatrixXd T(BE.rows()*(dim+1),dim);
    for(int e = 0;e<BE.rows();e++)
    {
      Affine3d a = Affine3d::Identity();
      a.translate(vT[e]);
      a.rotate(vQ[e]);
      T.block(e*(dim+1),0,dim+1,dim) =
        a.matrix().transpose().block(0,0,dim+1,dim);
    }
    // Compute deformation via LBS as matrix multiplication
    U = M*T;

    // Also deform skeleton edges
    MatrixXd CT;
    MatrixXi BET;
    igl::deform_skeleton(C,BE,T,CT,BET);

    viewer.data().set_vertices(U);
    viewer.data().set_edges(CT,BET,sea_green);
    viewer.data().compute_normals();
    anim_t += anim_t_dir;
    anim_t_dir *= (anim_t>=1.0 || anim_t<=0.0?-1.0:1.0);
  }
  return false;
}

void set_color(igl::opengl::glfw::Viewer &viewer)
{
  Eigen::MatrixXd C;
  igl::jet(W.col(selected).eval(),true,C);
  viewer.data().set_colors(C);
}

bool key_down(igl::opengl::glfw::Viewer &viewer, unsigned char key, int mods)
{
  switch(key)
  {
    case ' ':
      viewer.core.is_animating = !viewer.core.is_animating;
      break;
    case '.':
      selected++;
      selected = std::min(std::max(selected,0),(int)W.cols()-1);
      set_color(viewer);
      break;
    case ',':
      selected--;
      selected = std::min(std::max(selected,0),(int)W.cols()-1);
      set_color(viewer);
      break;
  }
  return true;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readMESH(TUTORIAL_SHARED_PATH "/hand.mesh",V,T,F);
  U=V;
  igl::readTGF(TUTORIAL_SHARED_PATH "/hand.tgf",C,BE);
  // retrieve parents for forward kinematics
  igl::directed_edge_parents(BE,P);

  // Read pose as matrix of quaternions per row
  MatrixXd Q;
  igl::readDMAT(TUTORIAL_SHARED_PATH "/hand-pose.dmat",Q);
  igl::column_to_quats(Q,pose);
  assert(pose.size() == BE.rows());

  // List of boundary indices (aka fixed value indices into VV)
  VectorXi b;
  // List of boundary conditions of each weight function
  MatrixXd bc;
  igl::boundary_conditions(V,T,C,VectorXi(),BE,MatrixXi(),b,bc);

  // compute BBW weights matrix
  igl::BBWData bbw_data;
  // only a few iterations for sake of demo
  bbw_data.active_set_params.max_iter = 8;
  bbw_data.verbosity = 2;
  if(!igl::bbw(V,T,b,bc,bbw_data,W))
  {
    return EXIT_FAILURE;
  }

  //MatrixXd Vsurf = V.topLeftCorner(F.maxCoeff()+1,V.cols());
  //MatrixXd Wsurf;
  //if(!igl::bone_heat(Vsurf,F,C,VectorXi(),BE,MatrixXi(),Wsurf))
  //{
  //  return false;
  //}
  //W.setConstant(V.rows(),Wsurf.cols(),1);
  //W.topLeftCorner(Wsurf.rows(),Wsurf.cols()) = Wsurf = Wsurf = Wsurf = Wsurf;

  // Normalize weights to sum to one
  igl::normalize_row_sums(W,W);
  // precompute linear blend skinning matrix
  igl::lbs_matrix(V,W,M);

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(U, F);
  set_color(viewer);
  viewer.data().set_edges(C,BE,sea_green);
  viewer.data().show_lines = false;
  viewer.data().show_overlay_depth = false;
  viewer.data().line_width = 1;
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core.is_animating = false;
  viewer.core.animation_max_fps = 30.;
  cout<<
    "Press '.' to show next weight function."<<endl<<
    "Press ',' to show previous weight function."<<endl<<
    "Press [space] to toggle animation."<<endl;
  viewer.launch();
  return EXIT_SUCCESS;
}


// Copyright 2018-2019 Hans Dembinski
//
// 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/core/lightweight_test.hpp>
#include <boost/histogram/axis/integer.hpp>
#include <boost/histogram/detail/throw_exception.hpp>
#include <boost/histogram/histogram.hpp>
#include <boost/histogram/make_histogram.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/numeric.hpp>

using namespace boost::histogram;
using namespace boost::adaptors;

int main() {
  auto h = make_histogram(axis::integer<>(1, 4));
  h(1, weight(1));
  h(2, weight(2));
  h(3, weight(3));
  h(4, weight(4));

  auto s1 = boost::accumulate(h | filtered([](double x) { return x > 2; }), 0.0);
  BOOST_TEST_EQ(s1, 7);

  return boost::report_errors();
}


#ifndef STAN_MATH_PRIM_MAT_PROB_GAUSSIAN_DLM_OBS_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_GAUSSIAN_DLM_OBS_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/err/check_cov_matrix.hpp>
#include <stan/math/prim/mat/err/check_pos_definite.hpp>
#include <stan/math/prim/mat/err/check_pos_semidefinite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <vector>

namespace stan {
namespace math {
namespace internal {

/**
 * Return a multivariate normal random variate with the given location
 * and covariance using the specified random number generator.
 *
 * No error checking or templating, takes the LDLT directly to avoid
 * recomputation. Can sample from semidefinite covariance matrices.
 *
 * @tparam RNG Type of pseudo-random number generator
 * @param mu location parameter
 * @param S_ldlt Eigen::LDLT of covariance matrix, semidefinite is okay
 * @param rng random number generator
 *
 */
template <class RNG>
inline Eigen::VectorXd multi_normal_semidefinite_rng(
    const Eigen::VectorXd &mu, const Eigen::LDLT<Eigen::MatrixXd> &S_ldlt,
    RNG &rng) {
  using boost::normal_distribution;
  using boost::variate_generator;

  variate_generator<RNG &, normal_distribution<>> std_normal_rng(
      rng, normal_distribution<>(0, 1));

  Eigen::VectorXd stddev = S_ldlt.vectorD().array().sqrt().matrix();
  size_t M = S_ldlt.vectorD().size();
  Eigen::VectorXd z(M);
  for (int i = 0; i < M; i++)
    z(i) = stddev(i) * std_normal_rng();

  Eigen::VectorXd Y
      = mu + (S_ldlt.transpositionsP().transpose() * (S_ldlt.matrixL() * z));
  // The inner paranthesis matter, transpositionsP() gives a
  // permutation matrix from pivoting and matrixL() gives a lower
  // triangular matrix. The types cannot be directly multiplied.

  return Y;
}

/**
 * Return a multivariate normal random variate with the given location
 * and covariance using the specified random number generator.
 *
 * No error checking or templating, takes the LLT directly to avoid
 * recomputation.
 *
 * @tparam RNG Type of pseudo-random number generator
 * @param mu location parameter
 * @param S_llt Eigen::LLT of positive definite covariance matrix.
 * @param rng random number generator
 *
 */
template <class RNG>
inline Eigen::VectorXd multi_normal_definite_rng(
    const Eigen::VectorXd &mu, const Eigen::LLT<Eigen::MatrixXd> &S_llt,
    RNG &rng) {
  using boost::normal_distribution;
  using boost::variate_generator;

  variate_generator<RNG &, normal_distribution<>> std_normal_rng(
      rng, normal_distribution<>(0, 1));

  size_t M = S_llt.matrixL().rows();
  Eigen::VectorXd z(M);
  for (int i = 0; i < M; i++)
    z(i) = std_normal_rng();

  Eigen::VectorXd Y = mu + S_llt.matrixL() * z;

  return Y;
}

}  // namespace internal

/**
 * Simulate random draw from Gaussian dynamic linear model (GDLM).
 * This distribution is equivalent to, for \f$t = 1:T\f$,
 * \f{eqnarray*}{
 * y_t & \sim N(F' \theta_t, V) \\
 * \theta_t & \sim N(G \theta_{t-1}, W) \\
 * \theta_0 & \sim N(m_0, C_0)
 * \f}
 *
 * @tparam RNG Type of pseudo-random number generator.
 * @param F A n x r matrix. The design matrix.
 * @param G A n x n matrix. The transition matrix.
 * @param V A r x r matrix. The observation covariance matrix.
 * @param W A n x n matrix. The state covariance matrix.
 * @param m0 A n x 1 matrix. The mean vector of the distribution
 * of the initial state.
 * @param C0 A n x n matrix. The covariance matrix of the distribution
 * of the initial state.
 * @param T a positive integer, how many timesteps to simulate.
 * @param rng Pseudo-random number generator.
 * @return A r x T matrix of simulated observations. Rows are
 * variables, columns are observations. First column is the state
 * after the first transition. Last column is the state after the last
 * transition. Initial state not returned.
 * @throw std::domain_error if a matrix is not symmetric or not
 * positive semi-definite. Or throw std::invalid_argument if a size is
 * wrong or any input is NaN or non-finite, or if T is not
 * positive. Require C0 in particular to be strictly positive
 * definite. V and W can be semidefinite.
 *
 */
template <class RNG>
inline Eigen::MatrixXd gaussian_dlm_obs_rng(const Eigen::MatrixXd &F,
                                            const Eigen::MatrixXd &G,
                                            const Eigen::MatrixXd &V,
                                            const Eigen::MatrixXd &W,
                                            const Eigen::VectorXd &m0,
                                            const Eigen::MatrixXd &C0,
                                            const int T, RNG &rng) {
  static const char *function = "gaussian_dlm_obs_rng";

  int r = F.cols();  // number of variables
  int n = G.rows();  // number of states

  check_size_match(function, "rows of F", F.rows(), "rows of G", G.rows());
  check_finite(function, "F", F);
  check_square(function, "G", G);
  check_finite(function, "G", G);
  check_size_match(function, "rows of V", V.rows(), "cols of F", F.cols());
  check_finite(function, "V", V);
  check_positive(function, "V rows", V.rows());
  check_symmetric(function, "V", V);
  check_size_match(function, "rows of W", W.rows(), "rows of G", G.rows());
  check_finite(function, "W", W);
  check_positive(function, "W rows", W.rows());
  check_symmetric(function, "W", W);
  check_size_match(function, "rows of W", W.rows(), "rows of G", G.rows());
  check_size_match(function, "size of m0", m0.size(), "rows of G", G.rows());
  check_finite(function, "m0", m0);
  check_size_match(function, "rows of C0", C0.rows(), "rows of G", G.rows());
  check_finite(function, "C0", C0);
  check_positive(function, "C0 rows", C0.rows());
  check_symmetric(function, "C0", C0);
  check_positive(function, "T", T);

  Eigen::LDLT<Eigen::MatrixXd> V_ldlt = V.ldlt();
  check_pos_semidefinite(function, "V", V_ldlt);
  Eigen::LDLT<Eigen::MatrixXd> W_ldlt = W.ldlt();
  check_pos_semidefinite(function, "W", W_ldlt);
  // Require strictly positive definite C0 for consistency with lpdf.
  Eigen::LLT<Eigen::MatrixXd> C0_llt = C0.llt();
  check_pos_definite(function, "C0", C0_llt);

  Eigen::MatrixXd y(r, T);
  Eigen::VectorXd theta_t
      = internal::multi_normal_definite_rng(m0, C0_llt, rng);
  for (int t = 0; t < T; ++t) {
    theta_t = internal::multi_normal_semidefinite_rng(
        Eigen::VectorXd(G * theta_t), W_ldlt, rng);
    y.col(t) = internal::multi_normal_semidefinite_rng(
        Eigen::VectorXd(F.transpose() * theta_t), V_ldlt, rng);
  }
  return y;
}

template <class RNG>
inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
gaussian_dlm_obs_rng(const Eigen::MatrixXd &F, const Eigen::MatrixXd &G,
                     const Eigen::VectorXd &V, const Eigen::MatrixXd &W,
                     const Eigen::VectorXd &m0, const Eigen::MatrixXd &C0,
                     const int T, RNG &rng) {
  return gaussian_dlm_obs_rng(F, G, Eigen::MatrixXd(V.asDiagonal()), W, m0, C0,
                              T, rng);
}

}  // namespace math
}  // namespace stan
#endif