/*
 Copyright 2011 Mario Mulansky
 Copyright 2012 Karsten Ahnert

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

/*
 * Example of a 2D simulation of nonlinearly coupled oscillators.
 * Program just prints final energy which should be close to the initial energy (1.0).
 * No parallelization is employed here.
 * Run time on a 2.3GHz Intel Core-i5: about 10 seconds for 100 steps.
 * Compile simply via bjam or directly:
 * g++ -O3 -I${BOOST_ROOT} -I../../../../.. spreading.cpp
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <cstdlib>
#include <sys/time.h>

#include <boost/ref.hpp>
#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp>

// we use a vector< vector< double > > as state type,
// for that some functionality has to be added for odeint to work
#include "nested_range_algebra.hpp"
#include "vector_vector_resize.hpp"

// defines the rhs of our dynamical equation
#include "lattice2d.hpp"
/* dynamical equations (Hamiltonian structure):
dqdt_{i,j} = p_{i,j}
dpdt_{i,j} = - omega_{i,j}*q_{i,j} - \beta*[ (q_{i,j} - q_{i,j-1})^3
                                            +(q_{i,j} - q_{i,j+1})^3
                                            +(q_{i,j} - q_{i-1,j})^3
                                            +(q_{i,j} - q_{i+1,j})^3 ]
*/

using namespace std;

static const int MAX_N = 1024;//2048;

static const size_t KAPPA = 2;
static const size_t LAMBDA = 4;
static const double W = 1.0;
static const double gap = 0.0;
static const size_t steps = 100;
static const double dt = 0.1;

double initial_e = 1.0;
double beta = 1.0;
int realization_index = 0;

//the state type
typedef vector< vector< double > > state_type;

//the stepper, choose a symplectic one to account for hamiltonian structure
//use nested_range_algebra for calculations on vector< vector< ... > >
typedef boost::numeric::odeint::symplectic_rkn_sb3a_mclachlan<
    state_type , state_type , double , state_type , state_type , double ,
    nested_range_algebra< boost::numeric::odeint::range_algebra > ,
    boost::numeric::odeint::default_operations > stepper_type;

double time_diff_in_ms( timeval &t1 , timeval &t2 )
{ return (t2.tv_sec - t1.tv_sec)*1000.0 + (t2.tv_usec - t1.tv_usec)/1000.0 + 0.5; }

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

    srand( time(NULL) );

    lattice2d< KAPPA , LAMBDA > lattice( beta );

    lattice.generate_pot( W , gap , MAX_N );

    state_type q( MAX_N , vector< double >( MAX_N , 0.0 ) );

    state_type p( q );

    state_type energy( q );

    p[MAX_N/2][MAX_N/2] = sqrt( 0.5*initial_e );
    p[MAX_N/2+1][MAX_N/2] = sqrt( 0.5*initial_e );
    p[MAX_N/2][MAX_N/2+1] = sqrt( 0.5*initial_e );
    p[MAX_N/2+1][MAX_N/2+1] = sqrt( 0.5*initial_e );

    cout.precision(10);

    lattice.local_energy( q , p , energy );
    double e=0.0;
    for( size_t i=0 ; i<energy.size() ; ++i )
        for( size_t j=0 ; j<energy[i].size() ; ++j )
        {
            e += energy[i][j];
        }

    cout << "initial energy: " << lattice.energy( q , p ) << endl;

    timeval elapsed_time_start , elapsed_time_end;
    gettimeofday(&elapsed_time_start , NULL);

    stepper_type stepper;

    for( size_t step=0 ; step<=steps ; ++step )
    {
        stepper.do_step( boost::ref( lattice ) ,
                         make_pair( boost::ref( q ) , boost::ref( p ) ) ,
                         0.0 , 0.1 );
    }

    gettimeofday(&elapsed_time_end , NULL);
    double elapsed_time = time_diff_in_ms( elapsed_time_start , elapsed_time_end );
    cout << steps << " steps in " << elapsed_time/1000 << " s (energy: " << lattice.energy( q , p ) << ")" << endl;
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Cholesky>
#include <Eigen/Sparse>
#include <limits>
using namespace Rcpp;
using namespace RcppEigen;
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;

// [[Rcpp::depends(RcppEigen)]]

void updateS(Eigen::MatrixXd &S, Eigen::MatrixXd &S2, Eigen::MatrixXd &S0, Eigen::MatrixXd &M, Eigen::MatrixXd &delta0, Eigen::MatrixXd &delta1, const Eigen::VectorXd &x, int iter){
  delta0 = x - M; // delta new point wrt old mean
  M+= delta0/(double)iter;     // sample mean
  delta1= x - M;      // delta new point wrt new mean
  if (iter>1){
    S2 +=(iter-1)/(double)(iter*iter)*(delta0*delta0.transpose())+(delta1*delta1.transpose());
    S0=S;
    S = S2/(double)(iter-1);           // sample covariance
  }
}

// [[Rcpp::interfaces(r,cpp)]]

//' Complex Polytope Gibbs Sampling
//' This function draw uniform samples in a convex polytope with inequality constraints
//'
//' @param N the number of samples to generate
//' @param A a matrix
//' @param b a vector of length equals to nrow(A)
//' @param x0 a vector of length equals to nrcol(A) that should be in the polytope, for example returned by \code{\link{chebycenter}}
//' @param thin thinning interval
//'
//' @section Details:
//' This function is based on an initial matlab code developped called CPRND
//' (https://ch.mathworks.com/matlabcentral/fileexchange/34208-uniform-distribution-over-a-convex-polytope)
//' It generates samples within the complex polytope defined by \eqn{A \cdot x \leqslant   b}
//'
//' @return a matrix with one row per sample and one column per parameter
//' @examples
//' n <- 20
//' A1 <- -diag(n)
//' b1 <- as.matrix(rep(0,n))
//' A2 <- diag(n)
//' b2 <- as.matrix(rep(1,n))
//' A <- rbind(A1,A2)
//' b <- rbind(b1,b2)
//' X0 <- chebycenter(A,b)
//' x <- cpgs(1000,A,b,X0)
//' @export
//' @useDynLib cpgsR
// [[Rcpp::export]]

Eigen::MatrixXd cpgs(const int N, const Eigen::MatrixXd &A ,const Eigen::VectorXd &b,const Eigen::VectorXd &x0, const int thin=1) {
  int p=A.cols();
  int m=A.rows();
  double inf = std::numeric_limits<double>::max();

  // Check input arguments
  if (m < (p+1) || b.size()!=m || x0.size()!=p){
    throw std::range_error("dimensions mismatch");
  }
  // Initialisation
  Eigen::MatrixXd X(N,p);
  Eigen::MatrixXd x(p,1);
  Eigen::MatrixXd y(p,1);
  x=x0;

  // Initialize variables for keeping track of sample mean, covariance
  // and isotropic transform.
  Eigen::MatrixXd M(p,1);
  M.setZero();
  Eigen::MatrixXd S2(p,p);
  S2.setZero();

  // outer products.
  Eigen::MatrixXd S(p,p);
  Eigen::MatrixXd S0(p,p);
  S.setIdentity();

  IntegerVector index=Rcpp::seq(0,p-1);

  Eigen::MatrixXd T1(p,p);
  Eigen::MatrixXd T2(p,p);
  T1.setIdentity();
  Eigen::MatrixXd W(m,p);

  W = A;
  bool adapt=true;
  bool updatingS=true;
  Eigen::MatrixXd d(m,1);
  Eigen::MatrixXd d2(m,1);
  Eigen::MatrixXd delta0(p,1);
  Eigen::MatrixXd delta1(p,1);
  Eigen::MatrixXd z(m,1);
  Eigen::MatrixXd L(p,p);
  Eigen::MatrixXd D(p,p);
  Eigen::VectorXd Dtmp(p);
  Eigen::VectorXd Dzero=VectorXd::Constant(p,1.0e-16);
  Eigen::LDLT<MatrixXd> ldltOfS(S.cols());
  int runup=0; //number of adapt
  int discard=0; //number of discard
  int isample=0; //number of sample
  int n=0; //total number of iterations
  int stage=0; //0 adapting phase, 1 discarding phase, 2 sampling
  int runupmax= 10*p*(p+1);
  int sampleit=0; //total number of iteration during sampling, useful for thin
  int discardmax=runupmax;
  double crit=0;
  while (isample<N){               //sampling loop
    //std::random_shuffle(index.begin(), index.end()); //we change the order to
    //limit the influence of initial ordering
    index=sample(index,p,false);
    y=x;
    NumericVector alea2=runif(p);
    // compute approximate stochastic transformation
    if ((stage==1 && discard==0) || (stage>0 && updatingS==true)){ //first true
      //sample, we now make the isotropic transformation
      ldltOfS.compute(S.transpose());
      D=ldltOfS.vectorD().cwiseMax(Dzero).asDiagonal();
      L=ldltOfS.matrixL();
      T1=ldltOfS.transpositionsP().transpose()*L*D.sqrt();
      T2=T1.inverse();
      W = A*T1;
    }
    if (stage>0) y=T2*y; //otherwise y=I^-1 * y=y

    // choose p new components
    for (int ip=0;ip<p;++ip){
      int i=index[ip];
      //Find points where the line with the (p-1) components x_i
      //fixed intersects the bounding polytope.
      z = W.col(i); //prevent any divisions by 0
      if (ip==0)
        d2=(b - W*y);
      d=d2.cwiseQuotient(z);
      double tmin=-inf;
      double tmax=inf;
      for (int j=0;j<m;++j){
        if (z(j)<0 && tmin<d(j)) tmin=d(j);
        if (z(j)>0 && tmax>d(j)) tmax=d(j);
     }
      tmin=std::min(0.0, tmin);
      tmax=std::max(0.0, tmax);

      double delta = -y(i);
      y(i) += (tmin+(tmax-tmin)*alea2(i));
      y(i)=std::min(std::max(y(i),-inf),inf);
      //Rcout<<tmin<<" "<<tmax<<" "<<y(i)<<std::endl;
      delta += y(i);
      d2 =d2- W.col(i)*delta; //we do this to avoid making a matrix
      //multiplication for each parameter (we just update the value of the
      //constraint with the delta of parameter)
}
    x=T1*y;

    if (stage==0){//still in updating phase
      ++runup;
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, runup);
      crit=(S0-S).norm()/S0.norm();
      if(runup>p & crit<0.5){
        stage=1;
        updatingS=false;
        Rcout<<"########adapation successful after "<<runup<<" iterations"<<std::endl;
        discardmax=runup;
      } else if (runup==runupmax){
        stage=1;
        M.setZero();
        S2.setZero();
        S.setIdentity();
        Rcout<<"########adapation unsuccessful after "<<runup<<" iterations"<<std::endl;
      }
    } else if (stage==1){ //we are in adapting phase
      ++discard;
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, discard);
      crit=(S0-S).norm()/S0.norm();
      if (crit < 0.5) {
        if (updatingS) Rcout<<"##stop updating S during discarding phase"<<std::endl;
        updatingS=false;
      }
      if (discard==discardmax){
        stage=2;
        M.setZero();
        S.setIdentity();
        S2.setZero();
        Rcout<<"#######end of discarding phase"<<std::endl;
        if (updatingS) Rcout<<"S still updated"<<std::endl;
      }
    } else{ //we are in sampling phase
      if ((sampleit % thin) == 0){
        X.row(isample)=x.col(0);
        ++isample;
      }
      if (updatingS) updateS(S, S2, S0, M, delta0, delta1, x, isample);
      double crit=(S0-S).norm()/S0.norm();
      if (crit < 0.5) {
        if (updatingS) Rcout<<"##stop updating S during sampling phase"<<std::endl;
        updatingS=false;
      }
      ++sampleit;
    }
    if (n % 100 == 0) Rcout<<"##iteration "<<n<<" stage "<<stage<<" crit "<<crit<<" S0.norm "<<S0.norm()<<" S-S0 "<<(S-S0).norm()<<std::endl;
    ++n;
  }
  return X;
}

using Eigen::FullPivLU;

//' Complex Polytope Gibbs Sampling
//' This function draw uniform samples in a convex polytope with both equality and inequality constraints
//'
//' @param N the number of samples to generate
//' @param A a matrix of coefficients of inequality constants A.x<=b
//' @param b a vector of length equals to nrow(A)
//' @param C a matrix of coefficients of inequality constants C.x=v
//' @param v a vector of length equals to nrow(C)
//' @param x0 a vector of length equals to ncol(A) that should be in the polytope, for example returned by \code{\link{chebycenter}}
//' @param thin the thinning interval
//'
//' @section Details:
//' This function is based on an initial matlab code developped called CPRND
//' (https://ch.mathworks.com/matlabcentral/fileexchange/34208-uniform-distribution-over-a-convex-polytope)
//' It generates samples within the complex polytope defined by \eqn{A \cdot x \leqslant   b}
//'
//' @return a matrix with one row per sample and one column per parameter
//' @examples
//' n <- 20
//' A1 <- -diag(n)
//' b1 <- as.matrix(rep(0,n))
//' A2 <- diag(n)
//' b2 <- as.matrix(rep(1,n))
//' A <- rbind(A1,A2)
//' b <- rbind(b1,b2)
//' C <- rbind(c(1,1,rep(0,n-2)),c(0,0,1,1,rep(0,n-4)))
//' v <- matrix(rep(0.2,2),2)
//' X0 <- rep(0.1,n)
//' x <- cpgsEquality(1000,A,b,C,v,X0)
//' @export
//' @useDynLib cpgsR
// [[Rcpp::export]]

Eigen::MatrixXd cpgsEquality(const int N, const Eigen::MatrixXd &A,
                             const Eigen::VectorXd &b, const Eigen::MatrixXd &C,
                             const Eigen::VectorXd &v, const Eigen::VectorXd &x0,
                             const int thin=1){
  int p=A.cols();
  int m=A.rows();
  int p2=C.cols();
  int m2=C.rows();

  MatrixXd X(N,p);

  // Check input arguments
  if (m < (p+1) || b.size()!=m || x0.size()!=p){
    throw std::range_error("dimensions mismatch");
  }
  if (v.size()!=m2 || x0.size()!=p2){
    throw std::range_error("dimensions mismatch");
  }
  // Initialisation
  FullPivLU<MatrixXd> lu(C);
  MatrixXd Nt = lu.kernel();
  MatrixXd Abis=A*Nt;
  VectorXd bbis=b-A*x0;

  VectorXd x0bis=VectorXd::Zero(Nt.cols());
  MatrixXd x=cpgs(N, Abis, bbis, x0bis, thin);
  for(int i=0;i<N;++i) {
    X.row(i)=Nt*x.row(i).transpose()+x0;
  }
  return X;
}


/**
 * \file boost/numeric/ublasx/operation/pow2.hpp
 *
 * \brief Apply the \c std::pow2 function to a vector or matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_pow2_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_pow2_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

//namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::pow2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T pow2(T x)
{
	return ::std::pow(2,x);
}

/// Auxiliary function used to replace ::std::pow2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
::std::complex<T> pow2(::std::complex<T> x)
{
	// Use the complex exponentiation formula.
	// See:
	// - http://en.wikipedia.org/wiki/Exponentiation#Powers_of_complex_numbers
	// - http://mathworld.wolfram.com/ComplexExponentiation.html

	T c0 = ::std::pow(2,x.real());
	T c1 = x.imag()*::std::log(2);
	return ::std::complex<T>(c0*::std::cos(c1),c0*::std::sin(c1));
}

//} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::pow2 function to a given vector expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::pow2 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_pow2_functor_traits<VectorExprT>::result_type pow2(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::signature_argument_type signature_argument_type;
	typedef typename detail::vector_pow2_functor_traits<VectorExprT>::signature_result_type signature_result_type;

//	return expression_type(ve(), detail::pow2<signature_result_type>);
//	signature_result_type (*)(ptr_pow2_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW2_NS_::pow2);
	typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
	fun_ptr_type ptr_pow2_fun(&detail::pow2);
	return expression_type(ve(), ptr_pow2_fun);
}

/**
 * \brief Applies the \c std::pow2 function to a given matrix expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::pow2 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_pow2_functor_traits<MatrixExprT>::result_type pow2(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::signature_argument_type signature_argument_type;
	typedef typename detail::matrix_pow2_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

//	return expression_type(me(), detail::pow2<signature_result_type>(signature_argument_type));
	typedef signature_result_type(*fun_ptr_type)(signature_argument_type);
	fun_ptr_type ptr_pow2_fun(&detail::pow2);
	return expression_type(me(), ptr_pow2_fun);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_POW2_HPP


/*
 * Simulation of an ensemble of Roessler attractors using NT2 SIMD library
 * This requires the SIMD library headers.
 *
 * Copyright 2014 Mario Mulansky
 *
 * 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 <iostream>
#include <vector>
#include <random>

#include <boost/timer.hpp>
#include <boost/array.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/multiplies.hpp>

namespace odeint = boost::numeric::odeint;
namespace simd = boost::simd;

typedef boost::timer timer_type;

static const size_t dim = 3;  // roessler is 3D

typedef double fp_type;
//typedef float fp_type;

typedef simd::pack<fp_type> simd_pack;
typedef boost::array<simd_pack, dim> state_type;
// use the simd allocator to get properly aligned memory
typedef std::vector< state_type, simd::allocator< state_type > > state_vec;

static const size_t pack_size = simd_pack::static_size;

//---------------------------------------------------------------------------
struct roessler_system {
    const fp_type m_a, m_b, m_c;

    roessler_system(const fp_type a, const fp_type b, const fp_type c)
        : m_a(a), m_b(b), m_c(c)
    {}

    void operator()(const state_type &x, state_type &dxdt, const fp_type t) const
    {
        dxdt[0] = -1.0*x[1] - x[2];
        dxdt[1] = x[0] + m_a * x[1];
        dxdt[2] = m_b + x[2] * (x[0] - m_c);
    }
};

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
if(argc<3)
{
    std::cerr << "Expected size and steps as parameter" << std::endl;
    exit(1);
}
const size_t n = atoi(argv[1]);
const size_t steps = atoi(argv[2]);

const fp_type dt = 0.01;

const fp_type a = 0.2;
const fp_type b = 1.0;
const fp_type c = 9.0;

// random initial conditions on the device
std::vector<fp_type> x(n), y(n), z(n);
std::default_random_engine generator;
std::uniform_real_distribution<fp_type> distribution_xy(-8.0, 8.0);
std::uniform_real_distribution<fp_type> distribution_z(0.0, 20.0);
auto rand_xy = std::bind(distribution_xy, std::ref(generator));
auto rand_z = std::bind(distribution_z, std::ref(generator));
std::generate(x.begin(), x.end(), rand_xy);
std::generate(y.begin(), y.end(), rand_xy);
std::generate(z.begin(), z.end(), rand_z);

state_vec state(n/pack_size);
for(size_t i=0; i<n/pack_size; ++i)
{
    for(size_t p=0; p<pack_size; ++p)
    {
        state[i][0][p] = x[i*pack_size+p];
        state[i][1][p] = y[i*pack_size+p];
        state[i][2][p] = z[i*pack_size+p];
    }
}

std::cout << "Systems: " << n << std::endl;
std::cout << "Steps: " << steps << std::endl;
std::cout << "SIMD pack size: " << pack_size << std::endl;

std::cout << state[0][0] << std::endl;

// Stepper type
odeint::runge_kutta4_classic<state_type, fp_type, state_type, fp_type,
                             odeint::array_algebra, odeint::default_operations,
                             odeint::never_resizer> stepper;

roessler_system sys(a, b, c);

timer_type timer;

fp_type t = 0.0;

for(int step = 0; step < steps; step++)
{
    for(size_t i = 0; i < n/pack_size; ++i)
    {
        stepper.do_step(sys, state[i], t, dt);
    }
    t += dt;
}

std::cout.precision(16);

std::cout << "Integration finished, runtime for " << steps << " steps: ";
std::cout << timer.elapsed() << " s" << std::endl;

// compute some accumulation to make sure all results have been computed
simd_pack s_pack = 0.0;
for(size_t i = 0; i < n/pack_size; ++i)
{
    s_pack += state[i][0];
}

fp_type s = 0.0;
for(size_t p=0; p<pack_size; ++p)
{
    s += s_pack[p];
}

std::cout << state[0][0] << std::endl;
std::cout << s/n << std::endl;

}


#include "socpInterface.hpp"

#include <array>
#include <chrono>
#include <fmt/format.h>

#include <Eigen/Dense>

// This example solves the portfolio optimization problem

int main()
{
    std::vector<double> solve_times;

    // assets, factors pair
    std::vector<std::tuple<size_t, size_t, size_t>> sets = {{100, 5, 10000},
                                                            {300, 10, 5000},
                                                            {500, 20, 1000},
                                                            {1000, 30, 100},
                                                            {2000, 40, 100},
                                                            {4000, 50, 100},
                                                            {7500, 60, 50},
                                                            {10000, 70, 25},
                                                            {20000, 80, 10}};
    for (auto [n, m, repetitions] : sets)
    {
        fmt::print("Running with assets: {}, factors: {}\n", n, m);

        // Set up problem data.
        double gamma = 0.5;      // risk aversion parameter
        Eigen::VectorXd mu(n);   // vector of expected returns
        Eigen::MatrixXd F(n, m); // factor-loading matrix
        Eigen::VectorXd D(n);    // diagonal of idiosyncratic risk

        mu.setRandom();
        F.setRandom();
        D.setRandom();
        mu = mu.cwiseAbs();
        F = F.cwiseAbs().transpose();
        D = D.cwiseAbs().cwiseSqrt();

        // Formulate SOCP.
        auto t0 = std::chrono::high_resolution_clock::now();

        op::SecondOrderConeProgram socp;

        op::Variable x = socp.createVariable("x", n);
        op::Variable t = socp.createVariable("t");
        op::Variable s = socp.createVariable("s");
        op::Variable u = socp.createVariable("u");
        op::Variable v = socp.createVariable("v");

        socp.addConstraint(x >= 0.);
        socp.addConstraint(op::sum(x) == op::Parameter(1.));
        socp.addConstraint(op::norm2(op::Parameter(&D).cwiseProduct(x)) <= u);
        socp.addConstraint(op::norm2(op::Parameter(&F) * x) <= v);
        socp.addConstraint(op::norm2(op::vstack({op::Parameter(1.) + -t, op::Parameter(2.) * u})) <= op::Parameter(1.) + t);
        socp.addConstraint(op::norm2(op::vstack({op::Parameter(1.) + -s, op::Parameter(2.) * v})) <= op::Parameter(1.) + s);

        socp.addMinimizationTerm(-op::Parameter(&mu).transpose() * x);
        socp.addMinimizationTerm(op::Parameter(gamma) * (t + s));

        // Create and initialize the solver instance.
        op::Solver solver(socp);
        solver.initialize();

        // Solve the problem and show solver output.
        double total_time = 0.;
        for (size_t rep = 0; rep < repetitions; rep++)
        {
            fmt::print("Repetition {}/{}\n", rep + 1, repetitions);
            mu.setRandom();
            F.setRandom();
            D.setRandom();
            mu = mu.cwiseAbs();
            F = F.cwiseAbs().transpose();
            D = D.cwiseAbs().cwiseSqrt();

            t0 = std::chrono::high_resolution_clock::now();
            const bool success = solver.solveProblem(false);
            auto t1 = std::chrono::high_resolution_clock::now();
            total_time += std::chrono::duration<double>(t1 - t0).count();
        }
        solve_times.push_back(total_time / repetitions);
    }

    fmt::print("\nAverage times:\n");
    for (size_t i = 0; i < sets.size(); i++)
    {
        fmt::print("{},\n", solve_times[i]);
    }
}


/*--
    PrimitiveFitUtils.cpp

    This file is part of the Cornucopia curve sketching library.
    Copyright (C) 2010 Ilya Baran (baran37@gmail.com)

    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 <http://www.gnu.org/licenses/>.
*/

#include "PrimitiveFitUtils.h"

#include "Line.h"
#include "Arc.h"
#include "Clothoid.h"
#include "Fresnel.h"

#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;
NAMESPACE_Cornu

void LineFitter::addPointW(const Vector2d &pt, double weight)
{
    ++_numPts;
    _totWeight += weight;

    _lastPoint = pt;
    if(_numPts == 1)
        _firstPoint = pt;

    _sum += weight * pt;
    _squaredSum[0] += weight * SQR(pt[0]);
    _squaredSum[1] += weight * SQR(pt[1]);
    _crossSum += weight * pt[0] * pt[1];
}

LinePtr LineFitter::getCurve() const
{
    if(_numPts < 2)
        return LinePtr();

    Matrix2d cov = Matrix2d::Zero();
    double factor = 1. / double(_totWeight);
    cov(0, 0) = _squaredSum[0] - SQR(_sum[0]) * factor;
    cov(1, 1) = _squaredSum[1] - SQR(_sum[1]) * factor;
    cov(0, 1) = cov(1, 0) = _crossSum - _sum[0] * _sum[1] * factor;
    cov *= factor;

    SelfAdjointEigenSolver<Matrix2d> eigenSolver(cov);

    Vector2d eigVs = eigenSolver.eigenvalues();
    Vector2d dir = eigenSolver.eigenvectors().col(1).normalized(); //1 is the index of the larger eigenvalue
    Vector2d pt = _sum * factor;
    Vector2d pt0 = pt + ((_firstPoint - pt).dot(dir)) * dir;
    Vector2d pt1 = pt + ((_lastPoint - pt).dot(dir)) * dir;

    return new Line(pt0, pt1);
}

void ArcFitter::addPointW(const Vector2d &pt, double weight)
{
    _pts.push_back(pt);

    Vector3d pt3(pt[0] - _pts[0][0], pt[1] - _pts[0][1], (pt - _pts[0]).squaredNorm());

    _totWeight += weight;
    _sum += weight * pt3;
    _squaredSum += weight * pt3 * pt3.transpose();
}

ArcPtr ArcFitter::getCurve() const
{
    if((int)_pts.size() < 2)
        return ArcPtr();

    double factor = 1. / _totWeight;
    Vector3d pt = _sum * factor;
    Matrix3d cov = factor * _squaredSum - pt * pt.transpose();

    SelfAdjointEigenSolver<Matrix3d> eigenSolver(cov);
    Vector3d eigVs = eigenSolver.eigenvalues();

    Vector3d dir = eigenSolver.eigenvectors().col(0); //0 is the index of the smallest eigenvalue
    dir /= (1e-16 + dir[2]);

    double dot = dir.dot(pt);
    //circle equation is:
    //dir[0] * x + dir[1] * y + (x^2+y^2) = dot
    Vector2d center = -0.5 * Vector2d(dir[0], dir[1]);
    double radius = sqrt(1e-16 + dot + center.squaredNorm());
    center += _pts[0];

    //TODO: convert code to use AngleUtils
    //Now get the arc
    Vector2d c[3] = { _pts[0], _pts[_pts.size() / 2], _pts.back() };
    double angle[3];
    for(int i = 0; i < 3; ++i) {
        c[i] = (c[i] - center).normalized() * radius;
        angle[i] = atan2(c[i][1], c[i][0]);
    }
    if(angle[2] < angle[0])
        angle[2] += PI * 2.;
    if(angle[1] < angle[0])
        angle[1] += PI * 2.;
    if(angle[1] <= angle[2]) { //OK--CCW arc
        double a = angle[2] - angle[0];
        return new Arc(c[0] + center, angle[0] + PI * 0.5, a * radius, 1. / radius);
    }
    else { //Backwards--CW arc
        for(int i = 0; i < 3; ++i)
            angle[i] = atan2(c[i][1], c[i][0]);
        if(angle[0] <= angle[2])
            angle[0] += PI * 2.;
        if(angle[1] < angle[2])
            angle[1] += PI * 2.;
        assert(angle[1] <= angle[0]);
        double a = angle[0] - angle[2];
        return new Arc(c[0] + center, angle[0] - PI * 0.5, a * radius, -1. / radius);
    }
}

void ClothoidFitter::addPoint(const Vector2d &pt)
{
    _pts.push_back(pt);

    if((int)_pts.size() < 2)
        return;

    const Vector2d &prevPt = _pts[_pts.size() - 2];
    double segmentLength = (pt - prevPt).norm();
    _centerOfMass += (pt + prevPt) * (0.5 * segmentLength);

    double angle = atan2(pt[1] - prevPt[1], pt[0] - prevPt[0]);
    if(angle < _prevAngle) //make sure it's not far from the previous angle
        angle += TWOPI * int(0.5 + (_prevAngle - angle) / TWOPI);
    else
        angle -= TWOPI * int(0.5 + (angle - _prevAngle) / TWOPI);
    _prevAngle = angle;

    double x0 = _totalLength;
    double x1 = (_totalLength += segmentLength);

    double y = angle;
    double z = _angleIntegral - y * x0;
    _rhs += _getRhs(x1, y, z) - _getRhs(x0, y, z);

    _angleIntegral += segmentLength * angle;
}

ClothoidPtr ClothoidFitter::getCurve() const
{
    Matrix4d lhs;

    lhs = _getLhs(_totalLength);

    Vector4d abcd = lhs.inverse() * _rhs;
    return getClothoidWithParams(abcd);
}

ClothoidPtr ClothoidFitter::getCurveWithZeroCurvature(double param) const
{
    Matrix<double, 5, 5> lhs;
    Matrix<double, 5, 1> rhs;

    Vector4d constraint;
    constraint << 6 * param, 2, 0, 0; // second derivative of ax^3+bx^2+cx+d is 6ax+2b

    //For constrained least squares,
    //lhs is now [A^T A    C]
    //           [ C^T     0]
    lhs << _getLhs(_totalLength),   constraint,
           constraint.transpose(),  0;
    rhs << _rhs, 0;

    Vector4d abcd = (lhs.inverse() * rhs).head<4>();
    return getClothoidWithParams(abcd);
}

ClothoidPtr ClothoidFitter::getClothoidWithParams(const Eigen::Vector4d &abcd) const
{
    Vector3d abc = Vector3d(abcd[0] * 3, abcd[1] * 2, abcd[2]);

    double startAngle = abc[2];
    double startCurvature = abc[1];
    double endCurvature = 2 * abc[0] * _totalLength + abc[1];

    //now compute the center of mass of the clothoid at 0
    double x, y;

    if(fabs(abc[0]) > 1e-8) //if it's a real clothoid
    {
        //The following comes from the expression that Mathematica generates with:
        //Integrate[Integrate[{Cos[a x^2 + b x + c], Sin[a x^2 + b x + c]}, {x, 0, t}], {t, 0, s}]/s
        bool negative = false;
        if(abc[0] < 0)
        {
            negative = true;
            abc = -abc;
        }
        double a = abc[0], b = abc[1], c = abc[2];
        double s = _totalLength;

        double invRtApi2 = 1. / sqrt(0.5 * PI * a);

        double f1s, f2s, f1c, f2c;
        fresnelApprox(b * invRtApi2 * 0.5, &f1s, &f1c);
        fresnelApprox((b + 2 * a * s) * invRtApi2 * 0.5, &f2s, &f2c);

        double disc = b * b / (4 * a) - c;
        double sind = sin(disc), cosd = cos(disc);

        y = (cos(c + s * (b + a * s)) - cos(c)) / (2. * a);
        y -= (b + 2 * a * s) * PI * 0.25 * (cosd * (f1s - f2s) + sind * (f2c - f1c)) * invRtApi2 / a;
        if(negative)
            y = -y;

        x = (sin(c) - sin(c + s * (b + a * s))) / (2. * a);
        x += (b + 2 * a * s) * PI * 0.25 * (-sind * (f1s - f2s) + cosd * (f2c - f1c)) * invRtApi2 / a;

        x /= s;
        y /= s;
    }
    else
    {
        double b = abc[1], c = abc[2];
        double s = _totalLength;

        if(fabs(b) < 1e-8) //line
        {
            x = cos(c) * s * 0.5;
            y = sin(c) * s * 0.5;
        }
        else //arc
        {
            double c1 = cos(c), s1 = sin(c);
            double c2 = cos(c + b * s), s2 = sin(c + b * s);

            x = (c1 - c2) / (b * b * s) - s1 / b;
            y = (s1 - s2) / (b * b * s) + c1 / b;
        }
    }

    return new Clothoid(Vector2d(_centerOfMass[0] / _totalLength - x, _centerOfMass[1] / _totalLength - y),
                        startAngle, _totalLength, startCurvature, endCurvature);
}

Matrix4d ClothoidFitter::_getLhs(double x)
{
    double xp[8] = {1, x, 0, 0, 0, 0, 0, 0 }; //powers of totalLength
    for(int i = 2; i < 8; ++i)
        xp[i] = xp[i - 1] * x;

    Matrix4d out;

    for(int i = 0; i < 4; ++i) for(int j = 0; j < 4; ++j)
    {
        int p = 7 - i - j;
        out(i, j) = (2. / double(p)) * xp[p];
    }

    return out;
}

Vector4d ClothoidFitter::_getRhs(double x, double y, double z)
{
    double xp[6] = {1, x, 0, 0, 0, 0 }; //powers of totalLength
    for(int i = 2; i < 6; ++i)
        xp[i] = xp[i - 1] * x;

    Vector4d out;

    out[0] = (2. / 5.) * y * xp[5] + 0.5 * z * xp[4];
    out[1] = 0.5 * y * xp[4] + (2. / 3.) * z * xp[3];
    out[2] = (2. / 3.) * y * xp[3] + z * xp[2];
    out[3] = y * xp[2] + 2. * z * xp[1];

    return out;
}

END_NAMESPACE_Cornu


/*
 * Copyright (c) 2015 Claus Christmann <hcc |ä| gatech.edu>.
 *
 * Licensed under the Apache Lice*nse, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "gis.h"

#include <assert.h>
#include <cmath>
#include "units.hpp"
#include "commoncoordinatesystems.h"
#include <boost/concept_check.hpp>

// Anoymous namespace to hide the chosen implementation for ecef2geodetic()
namespace {

using namespace AMG::GIS;

void ecef2geodetic_iterativ(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  // NOTE: this function is a replication of ecef2geodetic.m from Matlab
  //
  //   function [phi, lambda, h] = ecef2geodetic(x, y, z, ellipsoid)
  //   %ECEF2GEODETIC Convert geocentric (ECEF) to geodetic coordinates
  //   %
  //   %   [PHI, LAMBDA, H] = ECEF2GEODETIC(X, Y, Z, ELLIPSOID) converts point
  //   %   locations in geocentric Cartesian coordinates, stored in the
  //   %   coordinate arrays X, Y, Z, to geodetic coordinates PHI (geodetic
  //   %   latitude in radians), LAMBDA (longitude in radians), and H (height
  //   %   above the ellipsoid). The geodetic coordinates refer to the
  //   %   reference ellipsoid specified by ELLIPSOID (a row vector with the
  //   %   form [semimajor axis, eccentricity]). X, Y, and Z must use the same
  //   %   units as the semimajor axis;  H will also be expressed in these
  //   %   units.  X, Y, and Z must have the same shape; PHI, LAMBDA, and H
  //   %   will have this shape also.
  //   %
  //   %   For a definition of the geocentric system, also known as
  //   %   Earth-Centered, Earth-Fixed (ECEF), see the help for GEODETIC2ECEF.
  //   %
  //   %   See also ECEF2LV, GEODETIC2ECEF, GEOCENTRIC2GEODETICLAT, LV2ECEF.
  //
  //   % Copyright 2005-2009 The MathWorks, Inc.
  //   % $Revision: 1.1.6.4 $  $Date: 2009/04/15 23:34:43 $
  //
  //   % Reference
  //   % ---------
  //   % Paul R. Wolf and Bon A. Dewitt, "Elements of Photogrammetry with
  //   % Applications in GIS," 3rd Ed., McGraw-Hill, 2000 (Appendix F-3).
  //
  //   % Implementation Notes from Rob Comer
  //   % -----------------------------------
  //   % The implementation below follows Wolf and DeWitt quite literally,
  //   % with a few important exceptions required to ensure good numerical
  //   % behavior:
  //   %
  //   % 1) I used ATAN2 rather than ATAN in the formulas for beta and phi.  This
  //   %    avoids division by zero (or a very small number) for points on (or
  //   %    near) the Z-axis.
  //   %
  //   % 2) Likewise, I used ATAN2 instead of ATAN when computing beta from phi
  //   %    (conversion from geodetic to parametric latitude), ensuring
  //   %    stability even for points at very high latitudes.
  //   %
  //   % 3) Finally, I avoided dividing by cos(phi) -- also problematic at high
  //   %    latitudes -- in the calculation of h, the height above the ellipsoid.
  //   %    Wold and Dewitt give
  //   %
  //   %                   h = sqrt(X^2 + Y^2)/cos(phi) - N.
  //   %
  //   %    The trick is to notice an alternative formula that involves division
  //   %    by sin(phi) instead of cos(phi), then take a linear combination of the
  //   %    two formulas weighted by cos(phi)^2 and sin(phi)^2, respectively. This
  //   %    eliminates all divisions and, because of the identity cos(phi)^2 +
  //   %    sin(phi)^2 = 1 and the fact that both formulas give the same h, the
  //   %    linear combination is also equal to h.
  //   %
  //   %    To obtain the alternative formula, we simply rearrange
  //   %
  //   %                   Z = [N(1 - e^2) + h]sin(phi)
  //   %    into
  //   %                   h = Z/sin(phi) - N(1 - e^2).
  //   %
  //   %    The linear combination is thus
  //   %
  //   %        h = (sqrt(X^2 + Y^2)/cos(phi) - N) cos^2(phi)
  //   %            + (Z/sin(phi) - N(1 - e^2))sin^2(phi)
  //   %
  //   %    which simplifies to
  //   %
  //   %      h = sqrt(X^2 + Y^2)cos(phi) + Zsin(phi) - N(1 - e^2sin^2(phi)).
  //   %
  //   %    From here it's not hard to verify that along the Z-axis we have
  //   %    h = Z - b and in the equatorial plane we have h = sqrt(X^2 + Y^2) - a.

  // % Ellipsoid constants
  // WGS84::a   % Semimajor axis
  // WGS84::e2  % Square of first eccentricity
  // WGS84::ep2 % Square of second eccentricity
  // WGS84::f   % Flattening
  // WGS84::b   % Semiminor axis

  // Longitude
  lambda = std::atan2(y,x);

  // Distance from the z-axis
  double rho = std::hypot(x,y);

  // Bowring's formula for initial parametric (beta) and geodetic (phi) latitudes
  double beta = std::atan2(z, (1 - WGS84::f) * rho);
  phi = std::atan2(z   + WGS84::b*WGS84::ep2 *std::pow(std::sin(beta),3) ,
                   rho - WGS84::a*WGS84::e2  *std::pow(std::cos(beta),3) );

  // Fixed-point iteration with Bowring's formula
  // (typically converges within two or three iterations)
  double betaNew = std::atan2((1 - WGS84::f)*std::sin(phi), std::cos(phi));
  int count = 0;
  while( beta != betaNew && count < 10 )
  {
    beta = betaNew;
    phi = std::atan2(z   + WGS84::b * WGS84::ep2 * std::pow(std::sin(beta),3) ,
                     rho - WGS84::a * WGS84::e2  * std::pow(std::cos(beta),3) );
    betaNew = std::atan2((1 - WGS84::f)*std::sin(phi), std::cos(phi));
    ++count;
  }

  // Calculate ellipsoidal height from the final value for latitude
  double sinphi = std::sin(phi);
  double N = WGS84::a / std::sqrt(1.0 - WGS84::e2 * sinphi*sinphi);
  h = rho * std::cos(phi) + (z + WGS84::e2 * N * sinphi) * sinphi - N;

}

void ecef2geodetic_closedForm(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  //NOTE: This method is taken from
  //
  // Markku Heikkinen, "Geschlossene Formeln zur Berechnung räumlicher
  // geodätischer Koordinaten aud rechtwinkeligen Koordinaten", Zeitschrift für
  // Vermessungswesen, Vol. 107, 5/1982, p. 207--2011, in German

  //NOTE: This method is only valid if the radius r3 = sqrt(x*x+y*y+z*z) is
  // larger than ~43 km. (See the above mentioned paper for the related details.)
  assert( std::sqrt(x*x+y*y+z*z) > 45e3 );

  //
  // Ellipsoidal constants
  //
  long double a = WGS84::a;
  long double b = WGS84::b;
  long double e2 = WGS84::e2; // pos. definite
  long double ep2 = WGS84::ep2;// pos. definite
  long double E2 = e2*a*a; // alternative E2 = a*a-b*b; // pos. definite

  //
  // intermediaries
  //

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

  if( 0.0L == r ) // on N-S axis.
  {
    lambda = 0.0;

    phi =  static_cast<double>(M_PI_2l);
    if( z < 0 )
    { phi *= -1.0; }

    h = std::abs(z)-b;
    return;
  }

  long double F  = 54.0L*(b*b)*(z*z); // pos. definite
  long double G  = (r*r)+(1.0L-e2)*(z*z)-e2*E2; // pos. definite
  long double c  = (e2*e2)*F*(r*r)/(G*G*G)  ;
  long double s  = std::pow( 1.0L+c+std::sqrt((c*c)+2.0L*c) ,(1.0L/3.0L)); // 3rd root
  long double P  = F/( 3.0L*(1.0L+s+1.0L/s)*(1.0L+s+1.0L/s)*(G*G) );
  long double Q  = std::sqrt( 1.0L+2.0L*(e2*e2)*P );
  long double r0 = ((P*e2*r)/(1.0L+Q))
    +std::sqrt( 0.5L*(a*a)*(1.0L+1.0L/Q)-(P*(1.0L-e2)*(z*z))/(Q*(1.0L+Q))-0.5L*P*(r*r) );
  long double U  = std::sqrt( (z*z)+(r-e2*r0)*(r-e2*r0) );
  long double V  = std::sqrt( (r-e2*r0)*(r-e2*r0)+(1.0L-e2)*(z*z) );
  long double z0 = ((b*b)*z)/(a*V);

  //NOTE: I introduced these 3 variables in order to save some typing
  long double W  = z+ep2*z0;
  long double sinphi = W/std::sqrt( (r*r)+(W*W) );
  long double cosphi = r/std::sqrt( (r*r)+(W*W) );

  //
  // final computations
  //
  h = double( U*(1.0L-(b*b)/(a*V)) );

  //NOTE: as both, sinphi and cosphi are given in the algorithm, I decided to
  // invert the lower value, hoping that the higher slope in that region gives
  // a better numerical result. Obviously, this is just a gutt feel, no real
  // scienece involved...
  if( sinphi<=cosphi )
  {
    phi = double(std::asin(sinphi));
  }
  else
  {
    phi = double(std::acos(cosphi));
  }

  //NOTE: same argument as above... Claus
  if( x<=y )
  {
    lambda = double(std::acos(x/r));
  }
  else
  {
    lambda = double(std::asin(y/r));
  }

}

}

namespace AMG { namespace GIS {

std::string convertToString ( const CompassPoint& cp )
{
  using CP=AMG::GIS::CompassPoint;
  switch( cp )
  {
    case CP::N    : return "N"   ; //"North";
    case CP::NbE  : return "NbE" ; //"North by East";
    case CP::NNE  : return "NNE" ; //"North-northeast";
    case CP::NEbN : return "NEbN"; //"Northeast by North";
    case CP::NE   : return "NE"  ; //"Northeast";
    case CP::NEbE : return "NEbE"; //"Northeast by East";
    case CP::ENE  : return "ENE" ; //"East-northeast";
    case CP::EbN  : return "EbN" ; //"East by North";
    case CP::E    : return "E"   ; //"East";
    case CP::EbS  : return "EbS" ; //"South";
    case CP::ESE  : return "ESE" ; //"East-southeast";
    case CP::SEbE : return "SEbE"; //"Southeast by East";
    case CP::SE   : return "SE"  ; //"Southeast";
    case CP::SEbS : return "SEbS"; //"Southeast by South";
    case CP::SSE  : return "SSE" ; //"South-southeast";
    case CP::SbE  : return "SbE" ; //"South by East";
    case CP::S    : return "S"   ; //"South";
    case CP::SbW  : return "SbW" ; //"South by West";
    case CP::SSW  : return "SSW" ; //"South-southwest";
    case CP::SWbS : return "SWbS"; //"Southwest by South";
    case CP::SW   : return "SW"  ; //"Southwest";
    case CP::SWbW : return "SWbW"; //"Southwest by West";
    case CP::WSW  : return "WSW" ; //"West-southwest";
    case CP::WbS  : return "WbS" ; //"West by South";
    case CP::W    : return "W"   ; //"West";
    case CP::WbN  : return "WbN" ; //"West by North";
    case CP::WNW  : return "WNW" ; //"West-northwest";
    case CP::NWbW : return "NWbW"; //"Northwest by West";
    case CP::NW   : return "NW"  ; //"Northwest";
    case CP::NWbN : return "NWbN"; //"Northwest by North";
    case CP::NNW  : return "NNW" ; //"North-norhtwest";
    case CP::NbW  : return "NbW" ; //"North by West";

    default: return std::string();
  }
  return std::string();
}

double ensureHeading ( const double& heading_deg )
{
 // check for circular multiplicity, i.e. reduce heading to within one circle
 double hdg_deg = std::fmod(heading_deg,360.0);

 if( hdg_deg > 180.0 )
 {hdg_deg -=360.0;}
 else if( hdg_deg <= -180.0 )
 {hdg_deg += 360; }

 return hdg_deg;
}

double opposingHeading ( const double& heading_deg )
{
  return ensureHeading(heading_deg+180.0);
}

double compassPointToHeading ( CompassPoint const & cp )
{
  double center_deg = static_cast<double>(cp)/100;
  return ensureHeading(center_deg);
}

bool isInLeftHalfCircle ( const double& reference_deg, const double& heading_deg )
{
  double ref_deg  = ensureHeading(reference_deg );
  double test_deg = ensureHeading(heading_deg);

  //NOTE: remember: left  is [ref-180,ref), i.e. the opposite heading is in
  // both, the left and the right half circle!

  if( ref_deg == test_deg )
  { return false; }

  if( ref_deg == 0.0 )
  { return test_deg < 0  or test_deg == 180.0; }

  if( ref_deg > 0.0 )
  { return  (ref_deg-180.0)<=test_deg && test_deg < ref_deg; }

  if( ref_deg < 0.0 )
  { return !( ref_deg<test_deg && test_deg <(ref_deg+180.0) ) ; }
  //NOTE: ---------------------------------^
  // This is not a "<=" as we defined left as [ref-180,ref) !

  if( test_deg == opposingHeading (ref_deg) )
  { return true; }

  // catch all that shouldn't be reached.
  return false;
}

bool isInRightHalfCircle ( const double& reference_deg, const double& heading_deg )
{
  double ref_deg  = ensureHeading(reference_deg );
  double test_deg = ensureHeading(heading_deg);

  //NOTE: remember: right  is (ref,ref+180], i.e. the opposite heading is in
  // both, the left and the right half circle!

  if( ref_deg == test_deg )
  { return false; }

  if( ref_deg == 0.0 )
  { return test_deg > 0  or test_deg == 180.0; }

  if( ref_deg > 0.0 )
  { return  !( (ref_deg-180.0)<test_deg && test_deg < ref_deg ); }
  //NOTE: --------------------^
  // This is not a "<=" as we defined right as (ref,ref+180] !

  if( ref_deg < 0.0 )
  { return ref_deg<test_deg && test_deg <=(ref_deg+180.0) ; }

  if( test_deg == opposingHeading (ref_deg) )
  { return true; }

  // catch all that shouldn't be reached.
  return false;
}

CompassPoint headingToCompassPoint ( const double& heading_deg, int resolution )
{
  if( resolution != 4 or resolution != 8 /*or resolution != 16*/ )
  { resolution = 8; }

  double hdg_deg = ensureHeading(heading_deg);

  using CP=AMG::GIS::CompassPoint;
  CP cp = CP::N;

  auto inbetween = [hdg_deg](CP left,CP right)
  { return compassPointToHeading(left)<hdg_deg && hdg_deg <= compassPointToHeading(right); };

  if( inbetween(CP::SW, CP ::NW ) )
  { cp = CP::W;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::SSW, CP::WSW) )
    { cp = CP::SW; }
    else if( inbetween(CP::WNW,CP::NNW) )
    { cp = CP::NW; }

  }
  else if( inbetween(CP::NW, CP::NE) )
  { cp = CP::N;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::WNW,CP::NNW) )
    { cp = CP::NW; }
    else if( inbetween(CP::NNE,CP::ENE) )
    { cp = CP::NE; }

  }
  else if( inbetween(CP::NE,CP::SE) )
  { cp = CP::E;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::NNE,CP::ENE) )
    { cp = CP::NE; }
    else if( inbetween(CP::ESE,CP::SSE) )
    { cp = CP::SE; }

  }
  else
  { cp = CP::S;

    if( resolution == 4 ){ /*break;*/ }
    else if( inbetween(CP::ESE,CP::SSE) )
    { cp = CP::SE; }
    else if( inbetween(CP::SSW,CP::WSW) )
    { cp = CP::SW; }

  }

  return cp;
}

std::string geodeticPositionString ( const AMG::Vector& position )
{
  CoordinateTupel ecefCoords = position.absoluteCoordsIn( AMG::CoSy::getECEF() );

  double lat_rad,lon_rad,alt_m;
  GIS::ecef2geodetic(ecefCoords[0],ecefCoords[1],ecefCoords[2],
                          lon_rad,lat_rad,alt_m );

  CompassPoint northSouth = CompassPoint::N;
  if ( lat_rad < 0)
  { // Southern hemisphere
    northSouth = CompassPoint::S;
          lat_rad *= -1.0;
  }

  CompassPoint eastWest = CompassPoint::E;
  if ( lon_rad < 0)
  { // Western hemisphere
    eastWest = CompassPoint::W;
          lon_rad *= -1.0;
  }

  std::ostringstream stream;
  stream << AMG::Units::radian2degree( lat_rad ) << "°" << convertToString(northSouth) << ", "
         << AMG::Units::radian2degree( lon_rad ) << "°" << convertToString(eastWest)   << ", "
         << alt_m << " m WGS84";
  return stream.str();
}

void ecef2geodetic(const double& x, const double& y, const double& z, double& lambda, double& phi, double& h)
{
  ecef2geodetic_iterativ(x,y,z,lambda,phi,h);
//   ecef2geodetic_closedForm(x,y,z,lambda,phi,h); // forward call to closed form algorithm.
}

void geodetic2ecef(const double& lambda, const double& phi, const double& h, double& x, double& y, double& z)
{
  // NOTE: this function is a replication of ecef2geodetic.m from Matlab
  //
  //   function [x, y, z] = geodetic2ecef(phi, lambda, h, ellipsoid)
  //   %GEODETIC2ECEF Convert geodetic to geocentric (ECEF) coordinates
  //   %
  //   %   [X, Y, Z] = GEODETIC2ECEF(PHI, LAMBDA, H, ELLIPSOID) converts geodetic
  //   %   point locations specified by the coordinate arrays PHI (geodetic
  //   %   latitude in radians), LAMBDA (longitude in radians), and H (ellipsoidal
  //   %   height) to geocentric Cartesian coordinates X, Y, and Z.  The geodetic
  //   %   coordinates refer to the reference ellipsoid specified by ELLIPSOID (a
  //   %   row vector with the form [semimajor axis, eccentricity]).  H must use
  //   %   the same units as the semimajor axis;  X, Y, and Z will be expressed in
  //   %   these units also.
  //   %
  //   %   The geocentric Cartesian coordinate system is fixed with respect to the
  //   %   Earth, with its origin at the center of the ellipsoid and its X-, Y-,
  //   %   and Z-axes intersecting the surface at the following points:
  //   %
  //   %                PHI  LAMBDA
  //   %      X-axis:    0     0      (Equator at the Prime Meridian)
  //   %      Y-axis:    0   pi/2     (Equator at 90-degrees East)
  //   %      Z-axis:  pi/2    0      (North Pole)
  //   %
  //   %   A common synonym is Earth-Centered, Earth-Fixed coordinates, or ECEF.
  //   %
  //   %   See also ECEF2GEODETIC, ECEF2LV, GEODETIC2GEOCENTRICLAT, LV2ECEF.
  //
  //   % Copyright 2004-2009 The MathWorks, Inc.
  //   % $Revision: 1.1.6.4 $  $Date: 2009/04/15 23:34:46 $
  //
  //   % Reference
  //   % ---------
  //   % Paul R. Wolf and Bon A. Dewitt, "Elements of Photogrammetry with
  //   % Applications in GIS," 3rd Ed., McGraw-Hill, 2000 (Appendix F-3).

  // % Ellipsoid constants
  // WGS84::a   % Semimajor axis
  // WGS84::e2  % Square of first eccentricity
  // WGS84::ep2 % Square of second eccentricity
  // WGS84::f   % Flattening
  // WGS84::b   % Semiminor axis

  double sinphi = std::sin(phi);
  double cosphi = std::cos(phi);

  double N  = WGS84::a / std::sqrt(1.0 - WGS84::e2 * sinphi*sinphi);
//   double N  = WGS84::e2 ;
//   N *= sinphi;
//   N *= sinphi;
//   N = 1.0 - N;
//   N = std::sqrt(N);
//   N = WGS84::a / N;

  x = (N + h) * cosphi * std::cos(lambda);
  y = (N + h) * cosphi * std::sin(lambda);
  z = (N*(1 - WGS84::e2) + h) * sinphi;
}

void GudermannFunction(const double& yGudermann, double& phiGeocentric)
{
  phiGeocentric = std::asin( std::tanh( yGudermann ) );
}

void InverseGudermannFunction(const double& phiGeocentric, double& yGudermann)
{
  assert( (-M_PI_2l < phiGeocentric) && (phiGeocentric < M_PI_2l) );
  yGudermann = std::asinh( std::tan( phiGeocentric ) );
}

void geocentric2mercator(const double& lambdaGeocentric,
                                    const double& phiGeocentric,
                                    const double& hGeocentric,
                                          double& xGudermann,
                                          double& yGudermann,
                                          double& zGudermann)
{
  // longitude
  xGudermann = lambdaGeocentric;
  // latitude
  InverseGudermannFunction(phiGeocentric,yGudermann);
  // elevation
  zGudermann = hGeocentric;
}

void mercator2geocentric(const double& xGudermann,
                                    const double& yGudermann,
                                    const double& zGudermann,
                                          double& lambdaGeocentric,
                                          double& phiGeocentric,
                                          double& hGeocentric)
{
  // longitude
  lambdaGeocentric = xGudermann;
  // latitude
  GudermannFunction(yGudermann,phiGeocentric);
  // elevation
  hGeocentric=zGudermann;
}

void cartesian2spherical(const double& x,
                                   const double& y,
                                   const double& z,
                                         double& azimuth,
                                         double& elevation,
                                         double& distance)
{
  double xy = std::hypot(x,y); // length of the projection in the xy-plane
  distance  = std::hypot(z,xy);
  azimuth   = std::atan2(y,x);
  elevation = std::atan2(z,xy);
}

void spherical2cartesian(const double& azimuth,
                                   const double& elevation,
                                   const double& distance,
                                         double& x,
                                         double& y,
                                         double& z)
{
  assert(-M_PIl <= azimuth && azimuth <= M_PIl);
  assert(-M_PI_2l <= elevation && elevation <= M_PI_2l);
  assert( 0.0 <= distance );

  double xy = std::cos(elevation) * distance; // length of the projection in the xy-plane
  z = std::sin(elevation) * distance;
  y = std::sin(azimuth) * xy;
  x = std::cos(azimuth) * xy;
}

void ecef2geocentric(const double& x, const double& y, const double& z, double& lambdaGeocentric, double& phiGeocentric, double& hGeocentric)
{
  // treat it as a regular cartesian2spherical conversion
  double radialDistance(0.0);
  cartesian2spherical(x,y,z,lambdaGeocentric,phiGeocentric,radialDistance);

  // subtract the raddius of the referecen sphere from the distance
  hGeocentric = radialDistance-WGS84::a;
}

void geocentric2ecef(const double& lambdaGeocentric, const double& phiGeocentric, const double& hGeocentric, double& x, double& y, double& z)
{
  double radialDistance = hGeocentric+WGS84::a;
  spherical2cartesian(lambdaGeocentric,phiGeocentric,radialDistance,x,y,z);
}

void geocentric2geodetic(const double& lambdaGeocentric, const double& phiGeocentric, const double& hGeocentric, double& lambda, double& phi, double& h)
{
 double xTemp,yTemp,zTemp;

 geocentric2ecef(lambdaGeocentric,phiGeocentric,hGeocentric,xTemp,yTemp,zTemp);
 ecef2geodetic(xTemp,yTemp,zTemp,lambda,phi,h);
}

void geodetic2geocentric(const double& lambda, const double& phi, const double& h, double& lambdaGeocentric, double& phiGeocentric, double& hGeocentric)
{
  double xTemp,yTemp,zTemp;

  geodetic2ecef(lambda, phi, h, xTemp, yTemp, zTemp);
  ecef2geocentric(xTemp,yTemp,zTemp,lambdaGeocentric,phiGeocentric,hGeocentric);
}

void mercator2ecef(const double& xGudermann, const double& yGudermann, const double& zGudermann, double& x, double& y, double& z)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  mercator2geocentric(xGudermann,yGudermann,zGudermann,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2ecef(lambdaGeocentric,phiGeocentric,hGeocentric,x,y,z);

}

void ecef2mercator(const double& x, const double& y, const double& z, double& xGudermann, double& yGudermann, double& zGudermann)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  ecef2geocentric(x,y,z,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2mercator(lambdaGeocentric,phiGeocentric,hGeocentric,xGudermann,yGudermann,zGudermann);
}

void mercator2geodetic(const double& xGudermann, const double& yGudermann, const double& zGudermann, double& lambda, double& phi, double& h)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  mercator2geocentric(xGudermann,yGudermann,zGudermann,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2geodetic(lambdaGeocentric,phiGeocentric,hGeocentric,lambda,phi,h);

}

void geodetic2mercator(const double& lambda, const double& phi, const double& h, double& xGudermann, double& yGudermann, double& zGudermann)
{
  double lambdaGeocentric, phiGeocentric, hGeocentric;

  geodetic2geocentric(lambda,phi,h,lambdaGeocentric,phiGeocentric,hGeocentric);
  geocentric2mercator(lambdaGeocentric,phiGeocentric,hGeocentric,xGudermann,yGudermann,zGudermann);
}

} // end namespace GIS
} // end namespace AMG

std::ostream& operator<< ( std::ostream& out, const CompassPoint& cp )
{
  return out << AMG::GIS::convertToString(cp);
}


#include <boost/graph/spectrum.hpp>
#include "graph_types.hpp"
#include <boost/graph/python/graph.hpp>
#include <boost/python.hpp>
#include <vector>

namespace boost { namespace graph { namespace python {

template <typename Graph>
boost::python::tuple
spectrum
  (Graph& g,
   int first_eigenvector_index,
   int num_eigenvectors,
   double rel_tol = 100,
   double abs_tol = 1000)
{
  using std::vector;
  using boost::python::object;

  typedef typename property_map<Graph, vertex_index_t>::const_type VertexIndexMap;
  typedef vector_property_map<vertex_index_t, VertexIndexMap> IndexMap;
  typedef std::vector<double> Vector;

  int N = num_vertices(g);
  std::vector<Vector> evecs(num_eigenvectors);
  for (int i = 0; i < num_eigenvectors; i++)
    evecs[i] = *(new Vector(N));
  std::vector<double> evals(num_eigenvectors);

  boost::spectrum<Graph, std::vector<Vector> >(g, first_eigenvector_index, num_eigenvectors, evecs, evals, rel_tol, abs_tol);

  boost::python::list *evec;

  boost::python::list eigenvectors;
  for(int i = 0; i < num_eigenvectors; i++) {
    evec = new boost::python::list();
    for (int j = 0; j < N; j++)
      evec->append(evecs[i][j]);
    eigenvectors.append(*evec);
  }

  boost::python::list eigenvalues;
  for(int i = 0; i < num_eigenvectors; i++) {
      eigenvalues.append(evals[i]);
  }

  return boost::python::make_tuple(eigenvalues, eigenvectors);
}

void export_spectrum()
{
  using boost::python::arg;
  using boost::python::def;
  using boost::python::object;

  def("spectrum",
      &spectrum<Graph>,
      (arg("graph"),
       arg("first_eigenvector_index") = (int)1,
       arg("num_eigenvectors") = (int)1,
       arg("rel_tol") = (double)100,
       arg("abs_tol") = (double)1000)
      );

}

} } } // end namespace boost::graph::python


#include "isd.h"
#include <algorithm>
#include <boost/math/tools/roots.hpp>

using boost::math::tools::bisect;

const double tol = 1e-5;
const double epsilon = 1e-5;
bool quantum = false;

InformationSetDecoding::InformationSetDecoding(unsigned int as, double cr, double w,
                                               const std::string& m, const std::string& a)
{
    if(AlphabetSizeCheck(as))
        alphabetSize = as;
    else
        throw std::invalid_argument("Alphabet size needs to be greater or equal to 2.");

    if(MetricCheck(m))
        metric = m;
    else
        throw std::invalid_argument("This metric is not offered. Allowed metrics are hamming and lee.");

    if(ParamCheck(cr))
        codeRate = cr;
    else
        throw std::invalid_argument("Normalized value needs to be in the interval [0,1].");

    if(ParamCheck(w))
        weight = w;
    else
        throw std::invalid_argument("Normalized value needs to be in the interval [0,1].");

    if(AlgCheck(a))
        algorithm = a;
    else
        throw std::invalid_argument("This algorithm is not offered. Allowed algorithms are prange, dumer and wagner.");

    space = VectorSpace(metric, alphabetSize);
    surfaceW = space.SphereSurfArea(weight);
    if (surfaceW == -1)
    {
        throw std::invalid_argument("Surface area surfaceW is not found.");
    }
}

InformationSetDecoding& InformationSetDecoding::operator=(const InformationSetDecoding& isd)
{
    if (this == &isd)
        return *this;

    alphabetSize = isd.alphabetSize;
    codeRate = isd.codeRate;
    weight = isd.weight;
    metric = isd.metric;
    algorithm = isd.algorithm;
    space = isd.space;
    surfaceW = isd.surfaceW;

    return *this;
}

double InformationSetDecoding::BDayDecCost(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    if (quantum)
    {
        return std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        return std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }
}

double InformationSetDecoding::SolsPerIter(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    double bottomListSize;
    if (quantum)
    {
        bottomListSize = std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        bottomListSize = std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }

    double paramM = paramL - (optLevelNum - 1) * bottomListSize;
    if (quantum)
    {
        return 3 * bottomListSize - paramM;
    }
    else
    {
        return 2 * bottomListSize - paramM;
    }
}

double InformationSetDecoding::IterCost(double paramL, double paramP, double optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    if (quantum)
    {
        return std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        return std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }
}

double InformationSetDecoding::SolsNum() const
{
    return std::max(surfaceW - (1 - codeRate), 0.0);
}

double InformationSetDecoding::PartSolProb(double paramL, double paramP) const
{
    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    return surface2 - surfaceW + paramL;
}

double InformationSetDecoding::AnySolProb(double paramL, double paramP) const
{
    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    return std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW));
}

double InformationSetDecoding::RunTime(double paramL, double paramP, unsigned int& optLevelNum) const
{
    double distance1 = paramP / (codeRate + paramL);
    if(!ParamCheck(distance1))
    {
        if (distance1 >= 1.0 && distance1 <= 1.0 + epsilon)
        {
            distance1 = 1.0;
        }
        else
        {
            std::cout << distance1 << std::endl;
            throw std::runtime_error("Numerical issue on parameter paramP / (codeRate + paramL).");
        }
    }

    double surface1 = (codeRate + paramL) * space.SphereSurfArea(distance1);
    if (surface1 == -1)
    {
        throw std::invalid_argument("Surface area surface1 is not found.");
    }

    double distance2 = (weight - paramP) / (1 - codeRate - paramL);
    if(!ParamCheck(distance2))
    {
        if (distance2 >= 1.0 && distance2 <= 1.0 + epsilon)
        {
            distance2 = 1.0;
        }
        else
        {
            std::cout << distance2 << std::endl;
            throw std::runtime_error("Numerical issue on parameter (weight - paramP) / (1 - codeRate - paramL).");
        }
    }

    double surface2 = (1 - codeRate - paramL) * space.SphereSurfArea(distance2);
    if (surface2 == -1)
    {
        throw std::invalid_argument("Surface area surface2 is not found.");
    }

    auto theorem = [=](unsigned int optLevelNum) -> bool
    {
        return bool(paramL <= optLevelNum / pow(2.0, optLevelNum) * surface1);
    };

    if (!algorithm.compare("prange") || !algorithm.compare("dumer"))
    {
        optLevelNum = 1;
    }
    else
    {
        optLevelNum = 2;
        while (theorem(optLevelNum))
        {
            optLevelNum++;
        }
        optLevelNum--;
    }

    double bottomListSize;
    if (quantum)
    {
        bottomListSize = std::min(surface1 / (pow(2, optLevelNum) + 1), paramL / optLevelNum);
    }
    else
    {
        bottomListSize = std::min(surface1 / pow(2, optLevelNum), paramL / optLevelNum);
    }

    double paramM = paramL - (optLevelNum - 1) * bottomListSize;
    if (quantum)
    {
        return bottomListSize - 0.5 * std::min(0.0, std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW)) + 3 * bottomListSize - paramM);
    }
    else
    {
        return bottomListSize - std::min(0.0, std::min(0.0, surface2 + paramL - std::min(1 - codeRate, surfaceW)) + 2 * bottomListSize - paramM);
    }
}

double InformationSetDecoding::GoldenSectionSearch(double& paramP, unsigned int& optLevelNum)
{
    double paramPLow = std::max(0.0, weight - (1 - codeRate));
    double paramPHigh = std::min(weight, codeRate);
    std::function<double(double)> runTimeFun = [&](double p) {return RunTime(0.0, p, optLevelNum); };
    paramP = ::GoldenSectionSearch(paramPLow, paramPHigh, tol, runTimeFun);

    return log2(alphabetSize) * RunTime(0.0, paramP, optLevelNum);
}

double InformationSetDecoding::GoldenSectionSearch(double& paramL, double& paramP, unsigned int& optLevelNum)
{
    const double paramLLow = 0, paramLHigh = 1 - codeRate;
    double a = paramLLow, b = paramLHigh;
    const double gr = (sqrt(5) + 1) / 2;
    double c = b - (b - a) / gr;
    double d = a + (b - a) / gr;

    while (std::abs(c - d) > tol)
    {
        double paramPLowC = std::max(0.0, weight - (1 - codeRate - c));
        double paramPHighC = std::min(weight, codeRate + c);
        std::function<double(double)> runTimeC = [&](double p) {return RunTime(c, p, optLevelNum); };
        double optC = ::GoldenSectionSearch(paramPLowC, paramPHighC, tol, runTimeC);

        double paramPLowD = std::max(0.0, weight - (1 - codeRate - d));
        double paramPHighD = std::min(weight, codeRate + d);
        std::function<double(double)> runTimeD = [&](double p) {return RunTime(d, p, optLevelNum); };
        double optD = ::GoldenSectionSearch(paramPLowD, paramPHighD, tol, runTimeD);

        if (RunTime(c, optC, optLevelNum) < RunTime(d, optD, optLevelNum))
        {
            b = d;
        }
        else
        {
            a = c;
        }
        c = b - (b - a) / gr;
        d = a + (b - a) / gr;
    }

    paramL = (b + a) / 2;
    std::function<double(double)> runTime = [&](double p) {return RunTime(paramL, p, optLevelNum); };
    paramP = ::GoldenSectionSearch(std::max(0.0, weight - (1 - codeRate - paramL)),
        std::min(weight, codeRate + paramL), tol, runTime);

    return log2(alphabetSize) * RunTime(paramL, paramP, optLevelNum);
}

struct TerminationCondition {
    bool operator() (double min, double max) {
        return std::abs(min - max) <= tol;
    }
};

double AvgSolsNum(const VectorSpace& space, double distance, double codeRate)
{
    double surface = space.SphereSurfArea(distance);
    if (surface == -1)
    {
        throw std::invalid_argument(
            "Surface area surface1 is not found.");
    }
    return surface - (1 - codeRate);
}

double UpperRoot(std::string metric, unsigned int alphabetSize, double codeRate)
{
    VectorSpace space(metric, alphabetSize);
    std::function<double(double)> avgSolsNumCR = [&](double cr) {return AvgSolsNum(space, 1 - epsilon, cr); };
    std::pair<double, double> bracketsCR = bisect(avgSolsNumCR, 0.0, 1.0, TerminationCondition());
    double highestCodeRate = (bracketsCR.first + bracketsCR.second) / 2;

    if (codeRate > highestCodeRate)
        return 1;
    else
    {
        std::function<double(double)> avgSolsNumW = [&](double w) {return AvgSolsNum(space, w, codeRate); };
        std::pair<double, double> bracketsW = bisect(avgSolsNumW, AvgVectorWeight(space, 1.0) / MaxWeight(space), 1.0, TerminationCondition());
        return (bracketsW.first + bracketsW.second) / 2;
    }
}

double LowerRoot(std::string metric, unsigned int alphabetSize, double codeRate)
{
    VectorSpace space(metric, alphabetSize);
    std::function<double(double)> avgSolsNumW = [&](double w) {return AvgSolsNum(space, w, codeRate); };
    std::pair<double, double> bracketsW = bisect(avgSolsNumW, 0.0, AvgVectorWeight(space, 1.0) / MaxWeight(space), TerminationCondition());
    return (bracketsW.first + bracketsW.second) / 2;
}

double RunTime(std::string metric, std::string algorithm, unsigned int alphabetSize, double codeRate,
    double& weight, double& paramL, double& paramP, unsigned int& optLevelNum)
{
    if(!MetricCheck(metric))
        throw std::invalid_argument("This metric is not offered. Allowed metrics are hamming and lee.");

    double runTime;
    if (!metric.compare("hamming") && alphabetSize != 3)
    {
        weight = LowerRoot(metric, alphabetSize, codeRate) - epsilon;
    }
    else
    {
        weight = UpperRoot(metric, alphabetSize, codeRate) - epsilon;
    }

    InformationSetDecoding isd(alphabetSize, codeRate, weight, metric, algorithm);
    if (!algorithm.compare("prange"))
    {
        try
        {
            paramL = 0;
            runTime = isd.GoldenSectionSearch(paramP, optLevelNum);
        }
        catch (std::runtime_error& e)
        {
            std::cout << e.what() << std::endl;
            return -1;
        }
    }
    else if (!algorithm.compare("dumer") || !algorithm.compare("wagner"))
    {
        try
        {
            runTime = isd.GoldenSectionSearch(paramL, paramP, optLevelNum);
        }
        catch (std::runtime_error& e)
        {
            std::cout << e.what() << std::endl;
            return -1;
        }
    }
    else
    {
        std::cout << "This algorithm is not offered. Allowed algorithms are prange, dumer and wagner." << std::endl;
        return -1;
    }
    return -runTime;
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

/**
* @file degree_radian_conversion.hpp
* Provide cartesian<->spherical conversion without imposing particular data types.
* @author Andreas Franck a.franck@soton.ac.uk
*/

#ifndef VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED
#define VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED

#include <boost/math/constants/constants.hpp>

#include <cmath>

namespace visr
{
namespace efl
{

template< typename T >
T degree2radian( T deg )
{
  return boost::math::constants::degree<T>( ) * deg;
}

template< typename T >
T radian2degree( T rad )
{
  return boost::math::constants::radian<T>( ) * rad;
}

} // namespace efl
} // namespace visr

#endif // #ifndef VISR_LIBEFL_DEGREE_RADIAN_CONVERSION_HPP_INCLUDED


/*
 * Test tool for Toy Monte Carlo generation with CRAB submission
 *
 * \author Luca Lista, INFN
 *
 */
#include <boost/program_options.hpp>
#include "RooRandom.h"
#include "RooRealVar.h"
#include "RooGaussian.h"
#include "RooExponential.h"
#include "RooAddPdf.h"
#include "RooDataSet.h"
#include "RooDataHist.h"
#include "RooGlobalFunc.h"
#include "RooChi2Var.h"
#include "RooMinuit.h"
#include "RooPlot.h"
#include "TCanvas.h"
#include "TROOT.h"
#include <string>

static const char * const kHelpOpt = "help";
static const char * const kHelpCommandOpt = "help,h";
static const char * const kSeedOpt = "seed";
static const char * const kSeedCommandOpt = "seed,s";

int main(int argc, char * argv[]) {
  using namespace boost::program_options;
  using namespace std;

  gROOT->SetBatch(kTRUE);
  gROOT->SetStyle("Plain");

  string programName(argv[0]);
  string descString(programName);
  descString += " [options] ";
  options_description desc(descString);

  desc.add_options()
    (kHelpCommandOpt, "produce help message")
    (kSeedCommandOpt, value<unsigned int>(), "random generator seed");

  positional_options_description p;

  p.add(kSeedOpt, -1);

  variables_map vm;
  try {
    store(command_line_parser(argc,argv).options(desc).positional(p).run(), vm);
    notify(vm);
  } catch(const error&) {
    return 7000;
  }

  if(vm.count(kHelpOpt)) {
    cout << desc <<std::endl;
    return 0;
  }

  unsigned int seed = 123456;
  if(vm.count(kSeedOpt)) {
    seed = vm[kSeedOpt].as<unsigned int>();
    cout << "random seed specified by user as: " << seed << endl;
  } else {
    cout << "random seed set by default as: " << seed << endl;
  }

  RooRandom::randomGenerator()->SetSeed(seed);

  RooRealVar x("x", "x", -10, 10);

  RooRealVar mu("mu", "average", 0, -1, 1);
  RooRealVar sigma("sigma", "sigma", 1, 0, 5);
  RooGaussian gauss("gauss","gaussian PDF", x, mu, sigma);

  RooRealVar lambda("lambda", "slope", -0.1, -5., 0.);
  RooExponential expo("expo", "exponential PDF", x, lambda);

  RooRealVar s("s", "signal yield", 1000, 0, 10000);
  RooRealVar b("b", "background yield", 1000, 0, 10000);

  cout << "initial values: " << endl;

  mu.Print();
  sigma.Print();
  lambda.Print();
  s.Print();
  b.Print();

  RooAddPdf sum("sum", "gaussian plus exponential PDF",
		RooArgList(gauss, expo), RooArgList(s, b));

  unsigned int nSample = RooRandom::randomGenerator()->Poisson(s.getVal() + b.getVal());
  RooDataSet * data = sum.generate(x, nSample);
  x.setBins(50);
  RooDataHist hist("hist", "hist", RooArgSet(x), *data);
  RooChi2Var chi2("chi2","chi2", sum, hist, true);
  RooMinuit minuit(chi2);
  minuit.migrad();
  minuit.hesse();

  RooPlot * xFrame = x.frame() ;
  hist.plotOn(xFrame) ;
  sum.plotOn(xFrame) ;
  sum.plotOn(xFrame, RooFit::Components(expo), RooFit::LineStyle(kDashed)) ;
  TCanvas c;
  xFrame->Draw();
  c.SaveAs("binnedChi2Fit.eps");

  sum.getVariables()->Print();
  mu.Print();
  sigma.Print();
  lambda.Print();
  s.Print();
  b.Print();

  return 0;
}


/* adcpp_eigen.hpp
 *
 *  Created on: 21 Aug 2019
 *      Author: Fabian Meyer
 *     License: MIT
 */

#ifndef ADCPP_ADCPP_EIGEN_HPP_
#define ADCPP_ADCPP_EIGEN_HPP_

#include <adcpp/adcpp.hpp>
#include <Eigen/Core>

#define ADCPP_GEN_NUMTRAITS(T) \
    template<>\
    struct NumTraits<T>\
    {\
        using ValueType = T::Scalar;\
        using Real = T;\
        using NonInteger = T;\
        using Nested = T;\
        using Literal = T;\
        enum {\
            IsInteger = std::is_integral<ValueType>::value ? 1 : 0,\
            IsSigned = std::is_signed<ValueType>::value ? 1 : 0,\
            IsComplex = 0,\
            RequireInitialization = 1,\
            ReadCost = 1,\
            AddCost = 3,\
            MulCost = 3\
        };\
        static Real epsilon()\
        {\
            return Real(std::numeric_limits<ValueType>::epsilon());\
        }\
        static Real highest()\
        {\
            return Real(std::numeric_limits<ValueType>::max());\
        }\
        static Real lowest()\
        {\
            return Real(std::numeric_limits<ValueType>::min());\
        }\
        static Real min_exponent()\
        {\
            return Real(std::numeric_limits<ValueType>::min_exponent);\
        }\
        static Real max_exponent()\
        {\
            return Real(std::numeric_limits<ValueType>::max_exponent);\
        }\
        static Real digits()\
        {\
            return Real(std::numeric_limits<ValueType>::digits);\
        }\
        static Real digits10()\
        {\
            return Real(std::numeric_limits<ValueType>::digits10);\
        }\
    }

namespace Eigen
{
    ADCPP_GEN_NUMTRAITS(adcpp::fwd::Double);
    ADCPP_GEN_NUMTRAITS(adcpp::fwd::Float);

    ADCPP_GEN_NUMTRAITS(adcpp::bwd::Double);
    ADCPP_GEN_NUMTRAITS(adcpp::bwd::Float);
}

namespace adcpp
{
namespace fwd
{
    typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
    typedef Eigen::Matrix<Double, 2, 2> Matrix2d;
    typedef Eigen::Matrix<Double, 3, 3> Matrix3d;
    typedef Eigen::Matrix<Double, 4, 4> Matrix4d;
    typedef Eigen::Matrix<Double, 5, 5> Matrix5d;

    typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> VectorXd;
    typedef Eigen::Matrix<Double, 2, 1> Vector2d;
    typedef Eigen::Matrix<Double, 3, 1> Vector3d;
    typedef Eigen::Matrix<Double, 4, 1> Vector4d;
    typedef Eigen::Matrix<Double, 5, 1> Vector5d;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
    typedef Eigen::Matrix<Float, 2, 2> Matrix2f;
    typedef Eigen::Matrix<Float, 3, 3> Matrix3f;
    typedef Eigen::Matrix<Float, 4, 4> Matrix4f;
    typedef Eigen::Matrix<Float, 5, 5> Matrix5f;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, 1> VectorXf;
    typedef Eigen::Matrix<Float, 2, 1> Vector2f;
    typedef Eigen::Matrix<Float, 3, 1> Vector3f;
    typedef Eigen::Matrix<Float, 4, 1> Vector4f;
    typedef Eigen::Matrix<Float, 5, 1> Vector5f;
}

namespace bwd
{
    typedef Eigen::Matrix<Double, Eigen::Dynamic, Eigen::Dynamic> MatrixXd;
    typedef Eigen::Matrix<Double, 2, 2> Matrix2d;
    typedef Eigen::Matrix<Double, 3, 3> Matrix3d;
    typedef Eigen::Matrix<Double, 4, 4> Matrix4d;
    typedef Eigen::Matrix<Double, 5, 5> Matrix5d;

    typedef Eigen::Matrix<Double, Eigen::Dynamic, 1> VectorXd;
    typedef Eigen::Matrix<Double, 2, 1> Vector2d;
    typedef Eigen::Matrix<Double, 3, 1> Vector3d;
    typedef Eigen::Matrix<Double, 4, 1> Vector4d;
    typedef Eigen::Matrix<Double, 5, 1> Vector5d;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
    typedef Eigen::Matrix<Float, 2, 2> Matrix2f;
    typedef Eigen::Matrix<Float, 3, 3> Matrix3f;
    typedef Eigen::Matrix<Float, 4, 4> Matrix4f;
    typedef Eigen::Matrix<Float, 5, 5> Matrix5f;

    typedef Eigen::Matrix<Float, Eigen::Dynamic, 1> VectorXf;
    typedef Eigen::Matrix<Float, 2, 1> Vector2f;
    typedef Eigen::Matrix<Float, 3, 1> Vector3f;
    typedef Eigen::Matrix<Float, 4, 1> Vector4f;
    typedef Eigen::Matrix<Float, 5, 1> Vector5f;

    template<typename Scalar, typename DerivedA, typename DerivedB>
    inline void gradient(const Eigen::MatrixBase<DerivedA> &x,
        const Number<Scalar> &f,
        Eigen::MatrixBase<DerivedB> &grad)
    {
        assert(grad.size() == x.size());

        typename bwd::Number<Scalar>::DerivativeMap derivative;
        f.derivative(derivative);

        grad.setZero();
        for(long int i = 0; i < x.size(); ++i)
        {
            if(derivative.contains(x(i)))
                grad(i) = derivative(x(i));
        }
    }

    template<typename DerivedA, typename DerivedB, typename DerivedC>
    inline void jacobian(const Eigen::MatrixBase<DerivedA> &x,
        const Eigen::MatrixBase<DerivedB> &f,
        Eigen::MatrixBase<DerivedC> &jac)
    {
        assert(jac.rows() == f.size());
        assert(jac.cols() == x.size());

        typename Eigen::MatrixBase<DerivedB>::Scalar::DerivativeMap derivative;
        jac.setZero();
        for(long int i = 0; i < f.size(); ++i)
        {
            f(i).derivative(derivative);
            for(long int j = 0; j < x.size(); ++j)
            {
                if(derivative.contains(x(j)))
                    jac(i, j) = derivative(x(j));
            }
        }
    }
}
}

#endif


/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    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)
=============================================================================*/
#if !defined(PAWN_MAST_HPP)
#define PAWN_MAST_HPP

#include <boost/config/warning_disable.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>
#include <boost/optional.hpp>

#include <iostream>
#include <list>
#include <map>
#include <vector>

#include <helper.hpp>

namespace client { namespace math { namespace ast
{
    struct nil {};
    struct unary;
    struct expr;

    using variable = std::string;
    using column = unsigned int;

    typedef boost::variant<
            nil
          , double
          , variable
          , column
          , boost::recursive_wrapper<unary>
          , boost::recursive_wrapper<expr>
        >
    operand;

    enum class optoken : int
    {
        plus,
        minus,
        times,
        divide,
        positive,
        negative
    };

    struct unary
    {
        optoken operator_;
        operand operand_;
    };

    struct operation
    {
        optoken operator_;
        operand operand_;
    };

    struct expr
    {
        operand first;
        std::list<operation> rest;
    };

    // print functions for debugging
    inline std::ostream& operator<<(std::ostream& out, nil) { out << "nil"; return out; }
    //inline std::ostream& operator<<(std::ostream& out, variable const& var) { out << var.name; return out; }

    struct printer {
        typedef void result_type;

        void operator()(nil) const {std::cout << '_'; }
        void operator()(double n) const { std::cout << n; }
        void operator()(variable const& x) const { std::cout << '%' << x; }
        void operator()(column const& x) const { std::cout << '$' << x; }

        void operator()(optoken const &o) const {
            switch (o)
            {
                case optoken::plus: std::cout << " add"; break;
                case optoken::minus: std::cout << " subt"; break;
                case optoken::times: std::cout << " mult"; break;
                case optoken::divide: std::cout << " div"; break;
                case optoken::positive: std::cout << " pos"; break;
                case optoken::negative: std::cout << " neg"; break;
            }
        }

        void operator()(unary const& x) const
        {
            boost::apply_visitor(*this, x.operand_);
            (*this)(x.operator_);
        }

        void operator()(operation const& x) const
        {
            boost::apply_visitor(*this, x.operand_);
            (*this)(x.operator_);
        }

        void operator()(expr const& x) const
        {
            boost::apply_visitor(*this, x.first);
            for (const auto& oper : x.rest) {
                std::cout << ' ';
                (*this)(oper);
            }
        }
    };

    struct colsEval {
    private:
      using ColIndices = client::helper::ColIndices;
      using Global = client::helper::Global;
      const ColIndices &_pre;
      const Global &_global;
      bool _isInitial {true};
      std::vector<std::string> _headers;
    public:
        using result_type = std::pair<ColIndices, std::string>;
        colsEval(const ColIndices &v, const Global &g) : _pre{v}, _global{g} {}
        void setHeaders(const std::vector<std::string>& h) { _headers = h; }
        void notInitial() { _isInitial = false; }
        result_type operator()(nil) const { BOOST_ASSERT(0); return result_type{}; }
        result_type operator()(double n) const { return result_type{}; }
        result_type operator()(variable const &x) const {
          if (_global.gVarsN.find(x) != std::end(_global.gVarsN)) return result_type{};
          auto jt = std::find(begin(_headers), end(_headers), x);
          if (jt != std::end(_headers)) return (*this)((unsigned int)(jt - std::begin(_headers)) + 1);
          auto it = std::find(std::begin(_pre.var), std::end(_pre.var), x);
          if (it == std::end(_pre.var)) return std::make_pair(ColIndices{}, "Error: " + x + " used before declaration.");
          return result_type{};
        }
        result_type operator()(column const &x) const {
          if (!_isInitial) return std::make_pair(ColIndices{}, "Can't access new input file columns via column name / index after reduce / zip.");
          ColIndices res;
          res.num.push_back(x);
          return std::make_pair(res, "");
        }
        result_type operator()(unary const& x) const {
          return boost::apply_visitor(*this, x.operand_);
        }
        result_type operator()(operation const& x) const {
            return boost::apply_visitor(*this, x.operand_);
        }
        result_type operator()(expr const& e) const {
            result_type res{};
            auto x = boost::apply_visitor(*this, e.first);
            if (x.second.size() > 0) return x;
            res.first.add(x.first);
            for (const auto& oper : e.rest) {
              x = (*this)(oper);
              if (x.second.size() > 0) return x;
              res.first.add(x.first);
            }
            return res;
        }
    };

    ///////////////////////////////////////////////////////////////////////////
    //  The AST evaluator
    ///////////////////////////////////////////////////////////////////////////
    struct whatever {
      whatever(double x) : _x{x} {}
      auto operator() (const std::vector<double>&) { return _x; }
    private:
      double _x;
    };

    struct evaluator {
    private:
        const helper::positionTeller _index;
        const helper::Global &_global;
    public:
        using retFnT = std::function<double(const std::vector<double>&)>;
        typedef retFnT result_type;

        evaluator(helper::positionTeller p, const helper::Global &g) : _index{p}, _global{g} {}
        retFnT operator()(nil) const { BOOST_ASSERT(0); return whatever{0.0}; }
        retFnT operator()(double n) const { return whatever{n}; }
        retFnT operator()(variable const &x) const {
          auto it = _global.gVarsN.find(x);
          if (it != std::end(_global.gVarsN)) {
            auto y = it->second;
            return [y](const std::vector<double> &v) { return y; };
          }
          auto y = _index.var(x);
          return [y](const std::vector<double> &v) { return v[y]; };
        }
        retFnT operator()(column const &x) const {
          auto y = _index.num(x);
          return [y](const std::vector<double> &v) { return v[y]; };
        }

        retFnT operator()(optoken const &o, retFnT const &lhs, retFnT const &rhs) const {
            switch (o)
            {
                case optoken::plus: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) + rhs(v); };
                case optoken::minus: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) - rhs(v); };
                case optoken::times: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) * rhs(v); };
                case optoken::divide: return [lhs, rhs](const std::vector<double> &v) { return lhs(v) / rhs(v); };
                default: BOOST_ASSERT(0); return rhs;
            }
            BOOST_ASSERT(0);
            return rhs;
        }

        retFnT operator()(optoken const &o, retFnT const &rhs) const {
            switch (o)
            {
                case optoken::positive: return [rhs](const std::vector<double> &v) { return rhs(v); };
                case optoken::negative:  return [rhs](const std::vector<double> &v) { return - rhs(v); };
                default: BOOST_ASSERT(0); return rhs;
            }
            BOOST_ASSERT(0);
            return rhs;
        }

        retFnT operator()(unary const& x) const {
            retFnT rhs = boost::apply_visitor(*this, x.operand_);
            return (*this)(x.operator_, rhs);
        }

        retFnT operator()(operation const& x, retFnT const& lhs) const
        {
            retFnT rhs = boost::apply_visitor(*this, x.operand_);
            return (*this)(x.operator_, lhs, rhs);
        }
        retFnT operator()(expr const& x) const
        {
            retFnT state = boost::apply_visitor(*this, x.first);
            for (const auto& oper : x.rest) {
                state = (*this)(oper, state);
            }
            return state;
        }
    };
}}}

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::unary,
    (client::math::ast::optoken, operator_)
    (client::math::ast::operand, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::operation,
    (client::math::ast::optoken, operator_)
    (client::math::ast::operand, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    client::math::ast::expr,
    (client::math::ast::operand, first)
    (std::list<client::math::ast::operation>, rest)
)
#endif