// ///////////////////////////////////////////////////
// Roy Burstein and Aaron Osgood-Zimmerman
// August 2017
// Template file for space-time-Z GPR model.
// Used for fitting IHME Geospatial MBG models
// ///////////////////////////////////////////////////

// ///////////////////////////////////////////////////
// NOTES:
// 1. Type `Type` is a special TMB type that should be used for all variables, except `int` can be used as well
// 2. In our nomenclature, Z is a third interaction (ie age) which defaults to AR1
// 3. Requires same space mesh for all time-Z points
// 4. Anything in the density namespace (ie SEPARABLE) returns the negative log likelihood and is thus added to accumulator
//    also, other density function such as dnorm and dbinom return positive log likelihood and are thus subtracted away.
// 5. ref https://github.com/nmmarquez/re_simulations/blob/master/inla/sta.cpp
//        https://github.com/nmmarquez/re_simulations/blob/master/inla/SPDEAR1AR1.R
// ///////////////////////////////////////////////////

// include libraries
#include <TMB.hpp>
#include <Eigen/Sparse>
#include <vector>
using namespace density;
using Eigen::SparseMatrix;

// helper function to make sparse SPDE precision matrix
// Inputs:
//    logkappa: log(kappa) parameter value
//    logtau: log(tau) parameter value
//    M0, M1, M2: these sparse matrices are output from R::INLA::inla.spde2.matern()$param.inla$M*
template<class Type>
SparseMatrix<Type> spde_Q(Type logkappa, Type logtau, SparseMatrix<Type> M0,
                          SparseMatrix<Type> M1, SparseMatrix<Type> M2) {
    SparseMatrix<Type> Q;
    Type kappa2 = exp(2. * logkappa);
    Type kappa4 = kappa2*kappa2;
    Q = pow(exp(logtau), 2.)  * (kappa4*M0 + Type(2.0)*kappa2*M1 + M2);
    return Q;
}

// helper function for detecting NAs in the data supplied from R
template<class Type>
bool isNA(Type x){
  return R_IsNA(asDouble(x));
}

// Robust Inverse Logit that sets min and max values to avoid numerical instability
template<class Type>
Type invlogit_robust(Type x){
  if (x < -20.723){
    x = -20.723; // corresponds to p=1e-9
  } else if ( x > 20.723 ){
    x = 20.723;  // cooresponds to p=1-1e-9
  }
  return 1 / (1 + exp( -1.0 * x ));
}

// AR funtion from neal m
template<class Type>
SparseMatrix<Type> ar_Q(int N, Type rho, Type sigma) {
  SparseMatrix<Type> Q(N,N);
  Q.insert(0,0) = (1.) / pow(sigma, 2.);
  for (size_t n = 1; n < N; n++) {
    Q.insert(n,n) = (1. + pow(rho, 2.)) / pow(sigma, 2.);
    Q.insert(n-1,n) = (-1. * rho) / pow(sigma, 2.);
    Q.insert(n,n-1) = (-1. * rho) / pow(sigma, 2.);
  }
  Q.coeffRef(N-1,N-1) = (1.) / pow(sigma, 2.);
  return Q;
}

// Corresponding list object on the C++ side
template<class Type>
struct option_list {
  int use_priors;
  int adreport_off;
  int nugget;
  int country_random;
  int NID_random;
  int useGP;
  // Way easier to read these in as vectors of integers and then index the first
  option_list(SEXP x){
    use_priors = asVector<int>(getListElement(x,"use_priors"))[0];
    adreport_off = asVector<int>(getListElement(x,"adreport_off"))[0];
    nugget = asVector<int>(getListElement(x,"nugget"))[0];
    country_random = asVector<int>(getListElement(x,"country_random"))[0];
    NID_random = asVector<int>(getListElement(x,"NID_random"))[0];
    useGP = asVector<int>(getListElement(x,"useGP"))[0];
  }
};

// how to read in an object that is used as a prior for standard devs of res
template<class Type>
struct prior_type_sigma {
  std::string name;
  Type par1;
  Type par2;

  prior_type_sigma(SEXP x){
    name = CHAR(STRING_ELT(getListElement(x,"type"), 0));
    par1 = asVector<float>(getListElement(x,"par1"))[0];
    par2 = asVector<float>(getListElement(x,"par2"))[0];
  }
};

// how to read in an object that is used as a prior for Matern hyperparameters
template<class Type>
struct prior_type_matern {
  std::string name;
  Type par1a; //mean logtau / rho0
  Type par1b; //prec logtau / alpha_rho
  Type par2a; //mean logkappa / sigma0
  Type par2b; //prec logkappa / alpha_sigma

  prior_type_matern(SEXP x){
    name = CHAR(STRING_ELT(getListElement(x,"type"), 0));
    par1a = asVector<float>(getListElement(x,"par1"))[0];
    par1b = asVector<float>(getListElement(x,"par1"))[1];
    par2a = asVector<float>(getListElement(x,"par2"))[0];
    par2b = asVector<float>(getListElement(x,"par2"))[1];
  }
};

// evaluate a prior for sigma using the read in object
template<class Type>
Type eval_prior_sigma(prior_type_sigma<Type> prior, Type log_sigma){
  Type penalty;
  // transform log sigma to log tau space to match INLA prior specification
  // NOTE: log tau --> x=log sigma -->
  //    transform: log tau = -2x --> Jacobian: |J| = 2
  // https://becarioprecario.bitbucket.io/inla-gitbook/ch-priors.html#sec:priors
  Type tau = pow(exp(log_sigma), -2.);
  Type logtau = log(tau);

  if(prior.name == "pc.prec") {
    Type lambda = - log(prior.par2) / prior.par1;
    penalty = -lambda * exp(-logtau/Type(2.0)) - logtau/Type(2.0);
  }
  else if(prior.name == "normal") {
    // prior.par2 needs to be in precision space such as in INLA
    // https://inla.r-inla-download.org/r-inla.org/doc/prior/gaussian.pdf
    penalty = dnorm(logtau, prior.par1, pow(prior.par2, -.5), true);
  }
  else { //loggamma
    // prior.par2 gamma function for TMB uses shape and scale
    // https://kaskr.github.io/adcomp/group__R__style__distribution.html#gab0e2205710a698ad6a0ed39e0652c9a3
    // INLA uses shape and rate so we need to transforma
    // https://inla.r-inla-download.org/r-inla.org/doc/prior/prior-loggamma.pdf
    penalty = dlgamma(logtau, prior.par1, 1./prior.par2, true);
  }

  return penalty;
}

// evaluate priors for matern using the read in object
template<class Type>
Type eval_prior_matern(prior_type_matern<Type> prior, Type logtau, Type logkappa){
  Type penalty;

  if(prior.name == "pc") {
    Type d = 2.;
    Type lambda1 = -log(prior.par1b) * pow(prior.par1a, d/2.);
    Type lambda2 = -log(prior.par2b) / prior.par2a;
    Type range   = sqrt(8.0) / exp(logkappa);
    Type sigma   = 1.0 / sqrt(4.0 * 3.14159265359 * exp(2.0 * logtau) * exp(2.0 * logkappa));

    penalty = (-d/2. - 1.) * log(range) - lambda1 * pow(range, -d/2.) - lambda2 * sigma;
    // Note: (rho, sigma) --> (x=log kappa, y=log tau) -->
    //  transforms: rho = sqrt(8)/e^x & sigma = 1/(sqrt(4pi)*e^x*e^y)
    //  --> Jacobian: |J| propto e^(-y -2x)
    Type jacobian = - logtau - 2.0*logkappa;
    penalty += jacobian;
  }
  else { //normal
    penalty = dnorm(logtau, prior.par1a, 1/sqrt(prior.par1b), true) +
      dnorm(logkappa, prior.par2a, 1/sqrt(prior.par2b), true);
  }

  return penalty;
}

// Constrain alpha values
template<class Type>
vector<Type> constrain_pars(vector<Type> alpha, vector<int> constraints){
  int K = alpha.size();
  vector<Type> alpha_c(K);

  for(int k = 0; k < K; k++){
    if(constraints[k] == 1){
      alpha_c[k] = exp(alpha[k]);
    }
    if(constraints[k] == -1){
      alpha_c[k] = -1. * exp(alpha[k]);
    }
    if(constraints[k] == 0){
      alpha_c[k] = alpha[k];
    }
  }

  return alpha_c;
}

// objective function (ie the likelihood function for the model), returns the evaluated negative log likelihood
template<class Type>
Type objective_function<Type>::operator() ()
{

  // ////////////////////////////////////////////////////////////////////////////
  // INPUTS
  // ////////////////////////////////////////////////////////////////////////////
  DATA_INTEGER(flag); // flag=0 => only prior

  // Indices
  DATA_INTEGER(num_i);       // number of datapts in space-time-Z (aka STZ)
  DATA_INTEGER(num_s);       // number of mesh pts in space mesh
  DATA_INTEGER(num_t);       // number of time periods
  DATA_INTEGER(num_z);       // number of Z groups

  // Data (each, excpect for X_ij is a vector of length num_i)
  DATA_VECTOR(y_i);          // obs successes per binomial experiment at point i (aka cluster)
  DATA_VECTOR(n_i);          // trials per cluster
  DATA_IVECTOR(t_i);         // time period of the data point
  DATA_IVECTOR(c_re_i);      // country identifiers
  DATA_IVECTOR(nid_re_i);    // NID identifiers
  DATA_IVECTOR(w_i);         // weights for observations
  DATA_MATRIX(X_ij);         // covariate design matrix (num_i by number of fixed effects matrix)
  DATA_IVECTOR(fconstraints); // constraints of fixed effects

  // instructions for likelihood asessment
  DATA_VECTOR(lik_gaussian_i); // data likelihood for each row
  DATA_VECTOR(lik_binomial_i); // data likelihood for each row
  DATA_VECTOR(sd_i);           // crossalked standard deviation (set to zero if non-existant)

  // SPDE objects
  DATA_SPARSE_MATRIX(M0);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(M1);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(M2);    // used to make gmrf precision
  DATA_SPARSE_MATRIX(Aproj); // used to project from spatial mesh to data locations

  // Boolean vector of options to be used to select different models/modelling
  //   options: see above
  DATA_STRUCT(options, option_list);

  // Prior specifications
  DATA_STRUCT(prior_log_nugget_sigma, prior_type_sigma);
  DATA_STRUCT(prior_log_cre_sigma, prior_type_sigma);
  DATA_STRUCT(prior_log_nidre_sigma, prior_type_sigma);
  DATA_STRUCT(prior_matern, prior_type_matern);

  // Parameters
  PARAMETER_VECTOR(alpha_j);   // fixed effect coefs, including intercept as first index
  PARAMETER(logtau);           // log of INLA tau param (precision of space-time covariance mat)
  PARAMETER(logkappa);         // log of INLA kappa - related to spatial correlation and range
  PARAMETER(trho);             // temporal autocorrelation parameter for AR1, natural scale
  PARAMETER(zrho);             // Z autocorrelation parameter for AR1, natural scale
  PARAMETER(log_nugget_sigma); // log of the standard deviation of the normal error nugget term
  PARAMETER(log_cre_sigma);    // log of the standard deviation of the country random effect (later do as vec if using Random SLOPE TODO)
  PARAMETER(log_nidre_sigma);  // log of the standard deviation of the nid random effect
  PARAMETER(log_gauss_sigma);  // log of sigma for any gaussian observations

  // Random effects
  PARAMETER_ARRAY(Epsilon_stz);  // Random effects for each STZ mesh location. Should be 3D array of dimensions num_s by num_t by num_z
  PARAMETER_VECTOR(nug_i);       // Random effects of the nugget
  PARAMETER_VECTOR(cntry_re);    // Random effects values for country intercept
  PARAMETER_VECTOR(nid_re);      // Random effects values for nid intercept

  printf("Epsilon_stz size: %ld \n", Epsilon_stz.size());

  // ////////////////////////////////////////////////////////////////////////////
  // LIKELIHOOD
  // ////////////////////////////////////////////////////////////////////////////

  // Define the joint-negative log-likelihood as a parallel_accumulator
  // this allows us to add or subtract numbers to the object in parallel
  // parallel_accumulator<Type> jnll(this);
  Type jnll = 0;

  // print parallel info
  max_parallel_regions = omp_get_max_threads();

  // Make spatial precision matrix
  SparseMatrix<Type> Q_ss   = spde_Q(logkappa, logtau, M0, M1, M2);
  printf("Q_ss size: %ld \n", Q_ss.size());

  // Make transformations of some of our parameters
  Type range         = sqrt(8.0) / exp(logkappa);
  Type sigma         = 1.0 / sqrt(4.0 * 3.14159265359 * exp(2.0 * logtau) * exp(2.0 * logkappa));
  Type trho_trans    = (exp(trho) - 1) / (exp(trho) + 1); // TRANSOFRM from -inf, inf to -1, 1.. //log((1.1 + trho) / (1.1 - trho));
  Type zrho_trans    = (exp(zrho) - 1) / (exp(zrho) + 1); //TRANSOFRM from -inf, inf to -1, 1.. // log((1.1 + zrho) / (1.1 - zrho));
  Type nugget_sigma  = exp(log_nugget_sigma);
  Type cre_sigma     = exp(log_cre_sigma);
  Type nidre_sigma   = exp(log_nidre_sigma);
  Type gauss_sigma   = exp(log_gauss_sigma);

  // Define objects for derived values
  vector<Type> fe_i(num_i);                         // main effect X_ij %*% t(alpha_j)
  vector<Type> epsilon_stz(num_s * num_t * num_z);  // Epsilon_stz unlisted into a vector for easier matrix multiplication
  vector<Type> projepsilon_i(num_i);                // value of gmrf at data points
  vector<Type> prob_i(num_i);                       // Logit estimated prob for each point i

  // Latent field/Random effect contribution to likelihood.
  // Possibilities of Kronecker include: S, ST, SZ, and STZ
  if (num_t == 1 & num_z == 1)  {
    printf("GP FOR SPACE  ONLY \n");
    PARALLEL_REGION jnll += GMRF(Q_ss,false)(epsilon_stz);
  } else if(num_t > 1 & num_z == 1) {
    printf("GP FOR SPACE-TIME \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(trho_trans),GMRF(Q_ss,false))(Epsilon_stz);
  } else if (num_t == 1 & num_z > 1) {
    printf("GP FOR SPACE-Z \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(zrho_trans),GMRF(Q_ss,false))(Epsilon_stz);
  } else if (num_t > 1 & num_z > 1) {
    printf("GP FOR SPACE-TIME-Z \n");
    PARALLEL_REGION jnll += SEPARABLE(AR1(zrho_trans),SEPARABLE(AR1(trho_trans),GMRF(Q_ss,false)))(Epsilon_stz);
  }

  // nugget contribution to the likelihood
  if(options.nugget == 1){
    printf("Nugget \n");
    for (int i = 0; i < num_i; i++){
      // binomial models with sd_i, the additional variance gets put into the nugget.
      // for gaussian models nuggets seem unidentifiable so, the sd_i is in the data likelihood
      PARALLEL_REGION jnll -= dnorm(nug_i(i), Type(0.0), sqrt( pow(sd_i(i),2) + pow(nugget_sigma,2) ), true);
    }
  }

  // country random intercept
  if(options.country_random == 1){
    printf("Country RE \n");
    for(int i=0; i<cntry_re.size(); i++){
      PARALLEL_REGION jnll -= dnorm(cntry_re(i), Type(0.0), cre_sigma, true);
    }
  }

  // nid random intercept
  if(options.NID_random == 1){
    printf("NID RE \n");
    for(int i=0; i<nid_re.size(); i++){
      PARALLEL_REGION jnll -= dnorm(nid_re(i), Type(0.0), nidre_sigma, true);
    }
  }

  // Transform GMRFs and make vector form
  printf("Transform GMRF \n");
  for(int s = 0; s < num_s; s++){
    for(int t = 0; t < num_t; t++){
      if(num_z == 1) {
        epsilon_stz[(s + num_s * t )] = Epsilon_stz(s,t);
      } else {
        for(int z = 0; z < num_z; z++){
          epsilon_stz[(s + num_s * t + num_s * num_t * z)] = Epsilon_stz(s,t,z);
        }
      }
    }
  }

  // Project from mesh points to data points in order to eval likelihood at each data point
  printf("Project Epsilon \n");
  projepsilon_i = Aproj * epsilon_stz.matrix();

  // evaluate fixed effects for alpha_j values
  vector<Type> calpha_j = constrain_pars(alpha_j, fconstraints);
  fe_i = X_ij * calpha_j.matrix();

  // Return un-normalized density on request
  if (flag == 0) return jnll;

  // Prior contribution to likelihood. Values are defaulted.
  // Only run if options.use_priors==1
  if(options.use_priors == 1) {
    PARALLEL_REGION jnll -= eval_prior_matern(prior_matern, logtau, logkappa);
    if(num_t > 1) {
      // N(0,2.58^2) prior on log((1+rho)/(1-rho))
      PARALLEL_REGION jnll -= dnorm(trho, Type(0.0), Type(2.58), true);
    }
    if(num_z > 1) {
      // N(0,2.58^2) prior on log((1+rho)/(1-rho))
      PARALLEL_REGION jnll -= dnorm(zrho, Type(0.0), Type(2.58), true);
    }
    for(int j = 0; j < alpha_j.size(); j++){
      // N(0,3) prior for fixed effects.
      PARALLEL_REGION jnll -= dnorm(alpha_j(j), Type(0.0), Type(3.0), true);
    }
    // if using nugget (option in 3rd index)
    if(options.nugget == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_nugget_sigma, log_nugget_sigma);
    }
    // if using country (option in 4th index)
    if(options.country_random == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_cre_sigma, log_cre_sigma);
    }
    // if using nid (option in 5th index)
    if(options.NID_random == 1){
      PARALLEL_REGION jnll -= eval_prior_sigma(prior_log_nidre_sigma, log_nidre_sigma);
    }
  }

  // Likelihood contribution from each datapoint i
  printf("Data likelihood \n");
  for (int i = 0; i < num_i; i++){

    // mean model
    if(options.useGP==1){
      prob_i(i) = fe_i(i) + projepsilon_i(i) + nug_i(i) + cntry_re(c_re_i(i)) + nid_re(nid_re_i(i));
    } else {
      prob_i(i) = fe_i(i) +  nug_i(i) + cntry_re(c_re_i(i)) + nid_re(nid_re_i(i));
    }

    if(!isNA(y_i(i))){

      if(lik_binomial_i(i) == 1){
        PARALLEL_REGION jnll -= dbinom( y_i(i), n_i(i), invlogit_robust(prob_i(i)), true ) * w_i(i);
      }
      if(lik_gaussian_i(i) == 1){
        // this includes any crosswalked sd (sd_i), other variance (gauss_sigma), and is scaled by sample size (n_i)
        PARALLEL_REGION jnll -= dnorm( y_i(i), prob_i(i),  sqrt( (pow(sd_i(i),2) + (1/n_i(i) * pow(gauss_sigma,2)) ) ), true ) * w_i(i);
      }

    }
  }

  // Report estimates
  if(options.adreport_off == 0){
    ADREPORT(alpha_j);
  }

  return jnll;
}


//  (C) Copyright Nick Thompson 2018
//  (C) Copyright Matt Borland 2020
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP
#define BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP

#include <boost/math/tools/config.hpp>
#include <boost/math/tools/assert.hpp>
#include <tuple>
#include <iterator>
#include <type_traits>
#include <cmath>
#include <algorithm>
#include <valarray>
#include <stdexcept>
#include <functional>
#include <vector>

#ifdef BOOST_HAS_THREADS
#include <future>
#include <thread>
#endif

namespace boost { namespace math { namespace statistics { namespace detail {

template<typename ReturnType, typename ForwardIterator>
ReturnType mean_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    const std::size_t elements {static_cast<std::size_t>(std::distance(first, last))};
    std::valarray<ReturnType> mu {0, 0, 0, 0};
    std::valarray<ReturnType> temp {0, 0, 0, 0};
    ReturnType i {1};
    const ForwardIterator end {std::next(first, elements - (elements % 4))};
    ForwardIterator it {first};

    while(it != end)
    {
        const ReturnType inv {ReturnType(1) / i};
        temp = {static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++), static_cast<ReturnType>(*it++)};
        temp -= mu;
        mu += (temp *= inv);
        i += 1;
    }

    const ReturnType num1 {ReturnType(elements - (elements % 4))/ReturnType(4)};
    const ReturnType num2 {num1 + ReturnType(elements % 4)};

    while(it != last)
    {
        mu[3] += (*it-mu[3])/i;
        i += 1;
        ++it;
    }

    return (num1 * std::valarray<ReturnType>(mu[std::slice(0,3,1)]).sum() + num2 * mu[3]) / ReturnType(elements);
}

// Higham, Accuracy and Stability, equation 1.6a and 1.6b:
// Calculates Mean, M2, and variance
template<typename ReturnType, typename ForwardIterator>
ReturnType variance_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    Real M = *first;
    Real Q = 0;
    Real k = 2;
    Real M2 = 0;
    std::size_t n = 1;

    for(auto it = std::next(first); it != last; ++it)
    {
        Real tmp = (*it - M) / k;
        Real delta_1 = *it - M;
        Q += k*(k-1)*tmp*tmp;
        M += tmp;
        k += 1;
        Real delta_2 = *it - M;
        M2 += delta_1 * delta_2;
        ++n;
    }

    return std::make_tuple(M, M2, Q/(k-1), Real(n));
}

// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Higher-order_statistics
template<typename ReturnType, typename ForwardIterator>
ReturnType first_four_moments_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using Size = typename std::tuple_element<4, ReturnType>::type;

    Real M1 = *first;
    Real M2 = 0;
    Real M3 = 0;
    Real M4 = 0;
    Size n = 2;
    for (auto it = std::next(first); it != last; ++it)
    {
        Real delta21 = *it - M1;
        Real tmp = delta21/n;
        M4 = M4 + tmp*(tmp*tmp*delta21*((n-1)*(n*n-3*n+3)) + 6*tmp*M2 - 4*M3);
        M3 = M3 + tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
        M2 = M2 + tmp*(n-1)*delta21;
        M1 = M1 + tmp;
        n += 1;
    }

    return std::make_tuple(M1, M2, M3, M4, n-1);
}

#ifdef BOOST_HAS_THREADS

// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Higher-order_statistics
// EQN 3.1: https://www.osti.gov/servlets/purl/1426900
template<typename ReturnType, typename ForwardIterator>
ReturnType first_four_moments_parallel_impl(ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    const auto elements = std::distance(first, last);
    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // Threading is faster for: 10 + 5.13e-3 N/j <= 5.13e-3N => N >= 10^4j/5.13(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(5.13*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/5.13; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(elements < parallel_lower_bound)
    {
        return detail::first_four_moments_sequential_impl<ReturnType>(first, last);
    }
    else if(elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(5.13*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<ReturnType>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(elements) / num_threads);

    auto it = first;
    for(std::size_t i {}; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, elements_per_thread]() -> ReturnType
        {
            return first_four_moments_sequential_impl<ReturnType>(it, std::next(it, elements_per_thread));
        }));
        it = std::next(it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, last]() -> ReturnType
    {
        return first_four_moments_sequential_impl<ReturnType>(it, last);
    }));

    auto temp = future_manager[0].get();
    Real M1_a = std::get<0>(temp);
    Real M2_a = std::get<1>(temp);
    Real M3_a = std::get<2>(temp);
    Real M4_a = std::get<3>(temp);
    Real range_a = std::get<4>(temp);

    for(std::size_t i = 1; i < future_manager.size(); ++i)
    {
        temp = future_manager[i].get();
        Real M1_b = std::get<0>(temp);
        Real M2_b = std::get<1>(temp);
        Real M3_b = std::get<2>(temp);
        Real M4_b = std::get<3>(temp);
        Real range_b = std::get<4>(temp);

        const Real n_ab = range_a + range_b;
        const Real delta = M1_b - M1_a;

        M1_a = (range_a * M1_a + range_b * M1_b) / n_ab;
        M2_a = M2_a + M2_b + delta * delta * (range_a * range_b / n_ab);
        M3_a = M3_a + M3_b + (delta * delta * delta) * range_a * range_b * (range_a - range_b) / (n_ab * n_ab)
               + Real(3) * delta * (range_a * M2_b - range_b * M2_a) / n_ab;
        M4_a = M4_a + M4_b + (delta * delta * delta * delta) * range_a * range_b * (range_a * range_a - range_a * range_b + range_b * range_b) / (n_ab * n_ab * n_ab)
               + Real(6) * delta * delta * (range_a * range_a * M2_b + range_b * range_b * M2_a) / (n_ab * n_ab)
               + Real(4) * delta * (range_a * M3_b - range_b * M3_a) / n_ab;
        range_a = n_ab;
    }

    return std::make_tuple(M1_a, M2_a, M3_a, M4_a, elements);
}

#endif // BOOST_HAS_THREADS

// Follows equation 1.5 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<typename ReturnType, typename ForwardIterator>
ReturnType skewness_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    using std::sqrt;
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute skewness.");

    ReturnType M1 = *first;
    ReturnType M2 = 0;
    ReturnType M3 = 0;
    ReturnType n = 2;

    for (auto it = std::next(first); it != last; ++it)
    {
        ReturnType delta21 = *it - M1;
        ReturnType tmp = delta21/n;
        M3 += tmp*((n-1)*(n-2)*delta21*tmp - 3*M2);
        M2 += tmp*(n-1)*delta21;
        M1 += tmp;
        n += 1;
    }

    ReturnType var = M2/(n-1);

    if (var == 0)
    {
        // The limit is technically undefined, but the interpretation here is clear:
        // A constant dataset has no skewness.
        return ReturnType(0);
    }

    ReturnType skew = M3/(M2*sqrt(var));
    return skew;
}

template<typename ReturnType, typename ForwardIterator>
ReturnType gini_coefficient_sequential_impl(ForwardIterator first, ForwardIterator last)
{
    ReturnType i = 1;
    ReturnType num = 0;
    ReturnType denom = 0;

    for(auto it = first; it != last; ++it)
    {
        num += *it*i;
        denom += *it;
        ++i;
    }

    // If the l1 norm is zero, all elements are zero, so every element is the same.
    if(denom == 0)
    {
        return ReturnType(0);
    }
    else
    {
        return ((2*num)/denom - i)/(i-1);
    }
}

template<typename ReturnType, typename ForwardIterator>
ReturnType gini_range_fraction(ForwardIterator first, ForwardIterator last, std::size_t starting_index)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;

    std::size_t i = starting_index + 1;
    Real num = 0;
    Real denom = 0;

    for(auto it = first; it != last; ++it)
    {
        num += *it*i;
        denom += *it;
        ++i;
    }

    return std::make_tuple(num, denom, i);
}

#ifdef BOOST_HAS_THREADS

template<typename ReturnType, typename ExecutionPolicy, typename ForwardIterator>
ReturnType gini_coefficient_parallel_impl(ExecutionPolicy&&, ForwardIterator first, ForwardIterator last)
{
    using range_tuple = std::tuple<ReturnType, ReturnType, std::size_t>;

    const auto elements = std::distance(first, last);
    const unsigned max_concurrency = std::thread::hardware_concurrency() == 0 ? 2u : std::thread::hardware_concurrency();
    unsigned num_threads = 2u;

    // Threading is faster for: 10 + 10.12e-3 N/j <= 10.12e-3N => N >= 10^4j/10.12(j-1).
    const auto parallel_lower_bound = 10e4*max_concurrency/(10.12*(max_concurrency-1));
    const auto parallel_upper_bound = 10e4*2/10.12; // j = 2

    // https://lemire.me/blog/2020/01/30/cost-of-a-thread-in-c-under-linux/
    if(elements < parallel_lower_bound)
    {
        return gini_coefficient_sequential_impl<ReturnType>(first, last);
    }
    else if(elements >= parallel_upper_bound)
    {
        num_threads = max_concurrency;
    }
    else
    {
        for(unsigned i = 3; i < max_concurrency; ++i)
        {
            if(parallel_lower_bound < 10e4*i/(10.12*(i-1)))
            {
                num_threads = i;
                break;
            }
        }
    }

    std::vector<std::future<range_tuple>> future_manager;
    const auto elements_per_thread = std::ceil(static_cast<double>(elements) / num_threads);

    auto it = first;
    for(std::size_t i {}; i < num_threads - 1; ++i)
    {
        future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, elements_per_thread, i]() -> range_tuple
        {
            return gini_range_fraction<range_tuple>(it, std::next(it, elements_per_thread), i*elements_per_thread);
        }));
        it = std::next(it, elements_per_thread);
    }

    future_manager.emplace_back(std::async(std::launch::async | std::launch::deferred, [it, last, num_threads, elements_per_thread]() -> range_tuple
    {
        return gini_range_fraction<range_tuple>(it, last, (num_threads - 1)*elements_per_thread);
    }));

    ReturnType num = 0;
    ReturnType denom = 0;

    for(std::size_t i = 0; i < future_manager.size(); ++i)
    {
        auto temp = future_manager[i].get();
        num += std::get<0>(temp);
        denom += std::get<1>(temp);
    }

    // If the l1 norm is zero, all elements are zero, so every element is the same.
    if(denom == 0)
    {
        return ReturnType(0);
    }
    else
    {
        return ((2*num)/denom - elements)/(elements-1);
    }
}

#endif // BOOST_HAS_THREADS

template<typename ForwardIterator, typename OutputIterator>
OutputIterator mode_impl(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    using Z = typename std::iterator_traits<ForwardIterator>::value_type;
    using Size = typename std::iterator_traits<ForwardIterator>::difference_type;

    std::vector<Z> modes {};
    modes.reserve(16);
    Size max_counter {0};

    while(first != last)
    {
        Size current_count {0};
        ForwardIterator end_it {first};
        while(end_it != last && *end_it == *first)
        {
            ++current_count;
            ++end_it;
        }

        if(current_count > max_counter)
        {
            modes.resize(1);
            modes[0] = *first;
            max_counter = current_count;
        }

        else if(current_count == max_counter)
        {
            modes.emplace_back(*first);
        }

        first = end_it;
    }

    return std::move(modes.begin(), modes.end(), output);
}
}}}}

#endif // BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_DETAIL_SINGLE_PASS_HPP


// Standard
#include <iostream> // cout, endl
#include <cmath> // exp, abs
#include <vector>
// Thirdparties
#include <Eigen/Dense> // Eigen
// Lib
#include "s0s/euler.h" // s0s
// Simple
#include "func.h" // Func

constexpr unsigned int DIM = 10;
using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;

using TypeFunction = Func<TypeVector>;

int main () {
    // Parameters
    TypeScalar dt = 1e-4;
    TypeScalar tMax = 1e0;
    std::size_t nt = tMax / dt;
    TypeFunction f;
    s0s::SolverEuler<TypeVector, TypeView> solver;
    // Init
    TypeVector x = TypeVector::Constant(1.0);
    TypeScalar t = 0.0;
    // computation
    for(unsigned int i = 0; i < nt; i++) {
        solver(f, x.data(), x.size(), t, dt);
        t += dt;
    }
    // out
    std::cout << "\n";
    std::cout << "Solver solved exp(" << 0.0 << " -> " << tMax << ") = " << std::endl;
    std::cout << "\n";
    std::cout << x.transpose() << "\n";
    std::cout << "\n";
    std::cout << std::endl;
}


//System includes
#include <iostream>
#include <chrono>

//Eigen includes
#include <Eigen/Sparse>
#include <Eigen/Core>
#include <unsupported/Eigen/Splines>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Interpolation_traits_2.h>
#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/interpolation_functions.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_2<K> Delaunay_triangulation;
typedef CGAL::Interpolation_traits_2<K> Traits;
typedef K::FT Coord_type;
typedef K::Point_2 Point;

int main(){

    typedef Eigen::Spline<double, 1, 1> spline_type;
    typedef typename spline_type::PointType point_type;
    typedef typename spline_type::ControlPointVectorType cpv_type;

    const Eigen::VectorXd xvals = (Eigen::VectorXd(9) << 0, 0, 0, 1, 1, 1, 2, 2, 2).finished();
    const Eigen::VectorXd yvals = (Eigen::VectorXd(9) << 0, 1, 2, 0, 1, 2, 0, 1, 2).finished();
    cpv_type nodes(2,9);
    nodes.row(0)=xvals;
    nodes.row(1)=yvals;

    const Eigen::VectorXd zvals = xvals.array().square()+yvals.array().square();

    const spline_type spline = Eigen::SplineFitting<spline_type>::Interpolate( zvals.transpose(), 1, xvals.transpose());

    Delaunay_triangulation T;

    std::map<Point, Coord_type, K::Less_xy_2> function_values;

    typedef CGAL::Data_access< std::map<Point, Coord_type, K::Less_xy_2 > > Value_access;

    Coord_type a(0.25), bx(1.3), by(-0.7);

    for (int y=0 ; y<3 ; y++)
	for (int x=0 ; x<3 ; x++){
	    K::Point_2 p(x,y);
	    T.insert(p);
	    function_values.insert(std::make_pair(p,a + bx* x+ by*y));
	}

    //coordinate computation
    K::Point_2 p(1.3,0.34);

    std::vector< std::pair< Point, Coord_type > > coords;

    Coord_type norm = CGAL::natural_neighbor_coordinates_2 (T, p,std::back_inserter(coords)).second;
    Coord_type res = CGAL::linear_interpolation(coords.begin(), coords.end(), norm, Value_access(function_values));

    std::cout << " Tested interpolation on " << p << " interpolation: " << res << " exact: " << a + bx* p.x()+ by* p.y()<< std::endl;
    std::cout << "done" << std::endl;

     return 0;
}


#include <state_estimation/filters/ukf.h>
#include <state_estimation/utilities/logging.h>
#include <Eigen/Dense>

namespace state_estimation {

UKF::UKF(system_models::NonlinearSystemModel* system_model)
    : FilterBase::FilterBase(system_model) {
    initializeSigmaPointParameters();
}

UKF::UKF(system_models::NonlinearSystemModel* system_model, const Eigen::VectorXd& x,
         const Eigen::MatrixXd& cov, double timestamp)
    : FilterBase::FilterBase(system_model, x, cov, timestamp) {
    initializeSigmaPointParameters();
}

void UKF::setSigmaPointParameters(double alpha, double kappa, double beta) {
    uint32_t n = system_model_->g().size();
    num_sigma_pts_ = 2 * system_model_->stateSize() + 1;

    // Compute our lambda value
    lambda_ = pow(alpha, 2) * (n + kappa) - n;

    // Compute our weight vectors
    w_mean_.resize(num_sigma_pts_);
    w_cov_.resize(num_sigma_pts_);

    const double init_w = 0.5 / (n + lambda_);
    w_mean_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);
    w_cov_ = Eigen::VectorXd::Constant(num_sigma_pts_, init_w);

    w_mean_(0) = lambda_ / (n + lambda_);
    w_cov_(0) = w_mean_(0) + 1 - pow(alpha, 2) + beta;

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF sigma point initialization" << std::endl
              << "alpha=" << alpha << std::endl
              << "kappa=" << kappa << std::endl
              << "lambda=" << lambda_ << std::endl
              << "Initialized mean weights to [" << w_mean_.transpose() << "]" << std::endl
              << "Initialized covariance weights to [" << w_cov_.transpose() << "]" << std::endl;
#endif
}

void UKF::initializeSigmaPointParameters() {
    setSigmaPointParameters(0.001, 0, 2);
}

void UKF::myPredict(const Eigen::VectorXd& u, double dt) {
    // Generate the sigma points and run them through the system model
    const Eigen::MatrixXd sigma_offset =
        ((system_model_->stateSize() + lambda_) * filter_state_.covariance).llt().matrixL();
    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);

    system_model_->update(filter_state_.x, u, dt);
    Eigen::VectorXd x = system_model_->g();
    sigma_pts.col(0) = system_model_->g();

    for (uint32_t i = 0; i < system_model_->stateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->stateSize();

        const Eigen::VectorXd x_high =
            system_model_->addVectors(filter_state_.x, sigma_offset.col(i));
        system_model_->update(x_high, u, dt);
        sigma_pts.col(i_high) = system_model_->g();

        const Eigen::VectorXd x_low =
            system_model_->subtractVectors(filter_state_.x, sigma_offset.col(i));
        system_model_->update(x_low, u, dt);
        sigma_pts.col(i_low) = system_model_->g();
    }

    // Compute the weighted mean
    filter_state_.x = system_model_->weightedSum(w_mean_, sigma_pts);

    // Compute the weighted covariance
    filter_state_.covariance =
        system_model_->P() * system_model_->Rp() * system_model_->P().transpose() +
        system_model_->V() * system_model_->Rc() * system_model_->V().transpose();
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx =
            system_model_->subtractVectors(sigma_pts.col(i), filter_state_.x);
        filter_state_.covariance += w_cov_(i) * dx * dx.transpose();
    }

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF predicition update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "P=" << std::endl
              << printMatrix(system_model_->P()) << std::endl
              << "V=" << std::endl
              << printMatrix(system_model_->V()) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

void UKF::myCorrect(const Eigen::VectorXd& z, measurement_models::NonlinearMeasurementModel* model,
                    double dt) {
    // Generate the sigma points and run them through the measurement model
    Eigen::MatrixXd sigma_offset =
        ((system_model_->stateSize() + lambda_) * filter_state_.covariance).llt().matrixL();
    Eigen::MatrixXd sigma_pts(system_model_->stateSize(), num_sigma_pts_);
    Eigen::MatrixXd observed_sigma_pts(model->measurementSize(), num_sigma_pts_);

    sigma_pts.col(0) = filter_state_.x;
    model->update(filter_state_.x, dt);
    observed_sigma_pts.col(0) = model->h();

    for (uint32_t i = 0; i < system_model_->stateSize(); ++i) {
        const uint32_t i_high = i + 1;
        const uint32_t i_low = i + 1 + system_model_->stateSize();

        sigma_pts.col(i_high) = system_model_->addVectors(filter_state_.x, sigma_offset.col(i));
        model->update(sigma_pts.col(i_high), dt);
        observed_sigma_pts.col(i_high) = model->h();

        sigma_pts.col(i_low) = system_model_->subtractVectors(filter_state_.x, sigma_offset.col(i));
        model->update(sigma_pts.col(i_low), dt);
        observed_sigma_pts.col(i_low) = model->h();
    }

    // Compute the weighted mean for the predicted measurement
    Eigen::VectorXd z_pred = model->weightedSum(w_mean_, observed_sigma_pts);

    // Compute the gain
    Eigen::MatrixXd S = model->covariance();
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dz = model->subtractVectors(observed_sigma_pts.col(i), z_pred);
        S += w_cov_(i) * dz * dz.transpose();
    }

    Eigen::MatrixXd cross_covariance =
        Eigen::MatrixXd::Zero(model->stateSize(), model->measurementSize());
    for (uint32_t i = 0; i < num_sigma_pts_; ++i) {
        const Eigen::VectorXd dx =
            system_model_->subtractVectors(sigma_pts.col(i), sigma_pts.col(0));
        const Eigen::VectorXd dz = model->subtractVectors(observed_sigma_pts.col(i), z_pred);

        cross_covariance += w_cov_(i) * dx * dz.transpose();
    }

    const Eigen::MatrixXd K = cross_covariance * S.inverse();

    // Perform the mean and covariance updates
    const Eigen::VectorXd dx = K * model->subtractVectors(z, z_pred);
    filter_state_.x = system_model_->addVectors(filter_state_.x, dx);
    filter_state_.covariance -= K * S * K.transpose();

#ifdef DEBUG_STATE_ESTIMATION
    std::cout << "UKF measurement update:" << std::endl
              << "Sigma offsets=" << std::endl
              << printMatrix(sigma_offset) << std::endl
              << "Sigma points=" << std::endl
              << printMatrix(sigma_pts) << std::endl
              << "Observed sigma points=" << std::endl
              << printMatrix(observed_sigma_pts) << std::endl
              << "z_pred=" << printMatrix(z_pred) << std::endl
              << "Q=" << std::endl
              << printMatrix(model->covariance()) << std::endl
              << "S=" << std::endl
              << printMatrix(S) << std::endl
              << "Cross Covariance=" << std::endl
              << printMatrix(cross_covariance) << std::endl
              << "K=" << std::endl
              << printMatrix(K) << std::endl
              << "x=" << printMatrix(filter_state_.x) << std::endl
              << "Covariance=" << std::endl
              << printMatrix(filter_state_.covariance) << std::endl;
#endif
}

}  // namespace state_estimation


#include <cmath>
#include <fstream>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include "cmdline.h"
#include "vcf.h"

using std::size_t;

// TODO
// Chi-squared test for independance
// void calc_chisq(const std::vector<allele_t> &x, const std::vector<allele_t> &y);

namespace {

struct Parameter
{
    std::string vcf;
    std::string out;
    std::string loc;
    double rsq = 0.5;
    int maxdist = 500000;
} par ;

// LD (D')  -  Lewontin, R.C. (1964). Genetics 49(1), 49-67.
// LD (r^2) -  Hill, W.G., and Robertson, A. (1968). Theor Appl Genet 38(6), 226-231.

void calc_dprime_rsq_kernel(double pa, double pb, double pab, double &dprime, double &rsq)
{
    auto D = pab - pa*pb;
    auto Dmax = D > 0 ? std::min(pa*(1-pb),(1-pa)*pb) : std::min(pa*pb,(1-pa)*(1-pb));
    dprime = std::fabs(D) / Dmax;  // fabs is required for multi-allelic D'
    rsq = D*D / (pa * (1-pa) * pb * (1-pb));
}

void calc_dprime_rsq_hap2(const std::vector<allele_t> &x, const std::vector<allele_t> &y, double &dprime, double &rsq)
{
    static const allele_t a = 1, b = 1;

    auto n = x.size();
    int nt = 0, na = 0, nb = 0, nab = 0;

    for (size_t i = 0; i < n; ++i) {
        if (x[i] && y[i]) {
            ++nt;
            if (x[i] == a) {
                ++na;
                if (y[i] == b)
                    ++nab;
            }
            if (y[i] == b)
                ++nb;
        }
    }

    dprime = rsq = std::numeric_limits<double>::quiet_NaN();

    if (nt == 0)
        return;

    double fnt = nt;

    calc_dprime_rsq_kernel(na/fnt, nb/fnt, nab/fnt, dprime, rsq);
}

void calc_dprime_rsq_hap(const std::vector<allele_t> &x, const std::vector<allele_t> &y, double &dprime, double &rsq)
{
    auto n = x.size();

    auto xmax = * std::max_element(x.begin(), x.end());
    auto ymax = * std::max_element(y.begin(), y.end());

    dprime = rsq = std::numeric_limits<double>::quiet_NaN();

    if (xmax < 2 || ymax < 2)
        return;

    if (xmax == 2 && ymax == 2) {
        calc_dprime_rsq_hap2(x, y, dprime, rsq);
        return;
    }

    std::vector<size_t> idx;
    idx.reserve(n);
    for (size_t i = 0; i < n; ++i) {
        if (x[i] && y[i])
            idx.push_back(i);
    }

    if ( idx.empty() )
        return;

    dprime = rsq = 0.0;

    for (allele_t a = 1; a <= xmax; ++a) {
        for (allele_t b = 1; b <= ymax; ++b) {
            int na = 0, nb = 0, nab = 0;

            for (auto i : idx) {
                if (x[i] == a) {
                    ++na;
                    if (y[i] == b)
                        ++nab;
                }
                if (y[i] == b)
                    ++nb;
            }

            if (na == 0 || nb == 0)
                continue;

            double nt = idx.size();
            double pa = na / nt;
            double pb = nb / nt;
            double pab = nab / nt;

            double t1 = 0.0, t2 = 0.0;
            calc_dprime_rsq_kernel(pa, pb, pab, t1, t2);

            pab = pa * pb;
            dprime += pab * t1;
            rsq += pab * t2;
        }
    }
}

// Estimates haplotype frequencies via the EM algorithm
// AB, Ab, aB, ab, AaBb
void calc_hap_prob_EM(int n11, int n12, int n21, int n22, int ndh, double &p11, double &p12, double &p21, double &p22)
{
    static const int maxit = 1000;
    static const double tol = 1e-10;

    double n = n11 + n12 + n21 + n22 + ndh * 2;
    p11 = n11 / n;
    p12 = n12 / n;
    p21 = n21 / n;
    p22 = n22 / n;

    if (ndh == 0)
        return;

    auto cp11 = p11;
    auto cp12 = p12;
    auto cp21 = p21;
    auto cp22 = p22;

    auto h = ndh / n;
    auto x = h / 2;
    auto y = h - x;

    for (int i = 0; i < maxit; ++i) {
        p11 = cp11 + x;
        p12 = cp12 + y;
        p21 = cp21 + y;
        p22 = cp22 + x;
        auto z = h * p11 * p22 / (p11 * p22 + p12 * p21);
        if (std::fabs(x - z) < tol)
            break;
        x = z;
        y = h - x;
    }
}

void calc_dprime_rsq_dip2(const std::vector<allele_t> &x, const std::vector<allele_t> &y, double &dprime, double &rsq)
{
    auto n = x.size() / 2;

    int nt = 0;
    int f[3][3] = { { 0,0,0 }, { 0,0,0 }, { 0,0,0 } };

    for (size_t i = 0; i < n; ++i) {
        auto j = i*2, k = i*2+1;
        if (x[j] && x[k] && y[j] && y[k]) {
            auto a = x[j] + x[k] - 2;
            auto b = y[j] + y[k] - 2;
            ++f[a][b];
            ++nt;
        }
    }

    dprime = rsq = std::numeric_limits<double>::quiet_NaN();

    if (nt == 0)
        return;

    int n11 = f[0][0] * 2 + f[0][1] + f[1][0];
    int n12 = f[0][2] * 2 + f[0][1] + f[1][2];
    int n21 = f[2][0] * 2 + f[1][0] + f[2][1];
    int n22 = f[2][2] * 2 + f[2][1] + f[1][2];
    int ndh = f[1][1];

    double p11 = 0.0, p12 = 0.0, p21 = 0.0, p22 = 0.0;
    calc_hap_prob_EM(n11, n12, n21, n22, ndh, p11, p12, p21, p22);

    calc_dprime_rsq_kernel(p11 + p12, p11 + p21, p11, dprime, rsq);
}

// presuming that gametic phase is known
void calc_dprime_rsq_dip(const std::vector<allele_t> &x, const std::vector<allele_t> &y, double &dprime, double &rsq)
{
    auto n = x.size() / 2;

    auto xmax = * std::max_element(x.begin(), x.end());
    auto ymax = * std::max_element(y.begin(), y.end());

    dprime = rsq = std::numeric_limits<double>::quiet_NaN();

    if (xmax < 2 || ymax < 2)
        return;

    if (xmax == 2 && ymax == 2) {
        calc_dprime_rsq_dip2(x, y, dprime, rsq);
        return;
    }

    std::vector<size_t> idx;
    idx.reserve(n);
    for (size_t i = 0; i < n; ++i) {
        auto j = i*2, k = i*2+1;
        if (x[j] && x[k] && y[j] && y[k])
            idx.push_back(i);
    }

    if ( idx.empty() )
        return;

    dprime = rsq = 0.0;

    for (allele_t a = 1; a <= xmax; ++a) {
        for (allele_t b = 1; b <= ymax; ++b) {
            int na = 0, nb = 0, nab = 0;

            for (auto i : idx) {
                auto j = i*2, k = i*2+1;
                if (x[j] == a) {
                    ++na;
                    if (y[j] == b)
                        ++nab;
                }

                if (x[k] == a) {
                    ++na;
                    if (y[k] == b)
                        ++nab;
                }

                if (y[j] == b)
                    ++nb;

                if (y[k] == b)
                    ++nb;
            }

            if (na == 0 || nb == 0)
                continue;

            double nt = idx.size() * 2;
            auto pa = na / nt;
            auto pb = nb / nt;
            auto pab = nab / nt;

            double t1 = 0.0, t2 = 0.0;
            calc_dprime_rsq_kernel(pa, pb, pab, t1, t2);

            pab = pa * pb;
            dprime += pab * t1;
            rsq += pab * t2;
        }
    }
}

std::vector<std::string> read_string_list(const std::string &filename)
{
    std::vector<std::string> vs;

    std::ifstream ifs(filename);

    if ( ! ifs )
        std::cerr << "ERROR: can't open file for reading: " << filename << "\n";
    else
        std::copy(std::istream_iterator<std::string>(ifs), std::istream_iterator<std::string>(),
                  std::back_inserter(vs));

    return vs;
}

int ldstat_list(const Genotype &gt)
{
    auto loc = read_string_list(par.loc);

    std::sort(loc.begin(), loc.end());

    std::ofstream ofs(par.out + ".sub");
    if ( ! ofs ) {
        std::cerr << "ERROR: can't open file: " << par.out << ".list\n";
        return 1;
    }

    ofs << "Locus1\tChromosome1\tPosition1\tLocus2\tChromosome2\tPosition2\tDPrime\tRSquare\n";

    auto m = gt.loc.size();

    for (size_t i = 0; i < m; ++i) {
        if ( ! std::binary_search(loc.begin(), loc.end(), gt.loc[i]) )
            continue;

        std::cerr << "INFO: locus " << gt.loc[i] << "\n";

        for (size_t j = 0; j < m; ++j) {
            if (gt.loc[j] == gt.loc[i])
                continue;

            auto n1 = gt.allele[i].size();
            auto n2 = gt.allele[j].size();

            if (n1 < 2 || n2 < 2)
                continue;

            auto dprime = std::numeric_limits<double>::quiet_NaN();
            auto rsq = std::numeric_limits<double>::quiet_NaN();

            if (n1 == 2 && n2 == 2) {
                if (gt.ploidy == 1)
                    calc_dprime_rsq_hap2(gt.dat[i], gt.dat[j], dprime, rsq);
                else
                    calc_dprime_rsq_dip2(gt.dat[i], gt.dat[j], dprime, rsq);
            }
            else {
                if (gt.ploidy == 1)
                    calc_dprime_rsq_hap(gt.dat[i], gt.dat[j], dprime, rsq);
                else
                    calc_dprime_rsq_dip(gt.dat[i], gt.dat[j], dprime, rsq);
            }

            if ( ! std::isfinite(dprime) || ! std::isfinite(rsq) )
                continue;

            if (rsq < par.rsq)
                continue;

            ofs << gt.loc[i] << "\t" << gt.chr[i] << "\t" << gt.pos[i] << "\t"
                << gt.loc[j] << "\t" << gt.chr[j] << "\t" << gt.pos[j] << "\t"
                << dprime << "\t" << rsq << "\n";
        }
    }

    return 0;
}

} // namespace

int ldstat(int argc, char *argv[])
{
    std::cerr << "LDSTAT (Built on " __DATE__ " " __TIME__ ")\n";

    CmdLine cmd;

    cmd.add("--vcf", "VCF file", "");
    cmd.add("--loc", "locus list file", "");
    cmd.add("--loc-min-r2", "minimum LD (r2) threshold for locus list", "0.5");
    cmd.add("--out", "output file", "ldstat.out");
    cmd.add("--maxdist", "maximum inter-variant distance", "500000");

    cmd.parse(argc, argv);

    if (argc < 2) {
        cmd.show();
        return 1;
    }

    par.vcf = cmd.get("--vcf");
    par.loc = cmd.get("--loc");
    par.out = cmd.get("--out");
    par.rsq = std::stod(cmd.get("--loc-min-r2"));
    par.maxdist = std::stoi(cmd.get("--maxdist"));

    Genotype gt;

    std::cerr << "INFO: reading genotype file...\n";
    if (read_vcf(par.vcf, gt) != 0)
        return 2;
    std::cerr << "INFO: " << gt.ind.size() << " individuals, " << gt.loc.size() << " loci\n";

    if ( ! par.loc.empty() )
        return ldstat_list(gt);

    std::ofstream ofs1(par.out + ".all");
    if ( ! ofs1 ) {
        std::cerr << "ERROR: can't open file: " << par.out << ".all\n";
        return 1;
    }

    ofs1 << "Chromosome\tLocus1\tPosition1\tLocus2\tPosition2\tDistance\tDPrime\tRSquare\n";

    int n = par.maxdist / 1000;
    using namespace boost::accumulators;
    std::vector< accumulator_set<double, stats<tag::count, tag::mean> > > acc1(n), acc2(n);

    auto m = gt.loc.size();

    for (size_t i = 0; i < m; ++i) {
        for (size_t j = i + 1; j < m; ++j) {
            if (gt.chr[j] != gt.chr[i])
                continue;

            auto dist = std::abs(gt.pos[j] - gt.pos[i]);
            if (dist > par.maxdist)
                continue;

            auto n1 = gt.allele[i].size();
            auto n2 = gt.allele[j].size();

            if (n1 < 2 || n2 < 2)
                continue;

            auto dprime = std::numeric_limits<double>::quiet_NaN();
            auto rsq = std::numeric_limits<double>::quiet_NaN();

            if (n1 == 2 && n2 == 2) {
                if (gt.ploidy == 1)
                    calc_dprime_rsq_hap2(gt.dat[i], gt.dat[j], dprime, rsq);
                else
                    calc_dprime_rsq_dip2(gt.dat[i], gt.dat[j], dprime, rsq);
            }
            else {
                if (gt.ploidy == 1)
                    calc_dprime_rsq_hap(gt.dat[i], gt.dat[j], dprime, rsq);
                else
                    calc_dprime_rsq_dip(gt.dat[i], gt.dat[j], dprime, rsq);
            }

            if ( ! std::isfinite(dprime) || ! std::isfinite(rsq) )
                continue;

            int k = dist / 1000;
            if (dist % 1000 == 0)
                --k;

            acc1[k](dprime);
            acc2[k](rsq);

            ofs1 << gt.chr[i] << "\t"
                 << gt.loc[i] << "\t" << gt.pos[i] << "\t"
                 << gt.loc[j] << "\t" << gt.pos[j] << "\t"
                 << dist << "\t" << dprime << "\t" << rsq << "\n";
        }
    }

    std::ofstream ofs2(par.out + ".sum");
    if ( ! ofs2 ) {
        std::cerr << "ERROR: can't open file: " << par.out << ".sum\n";
        return 1;
    }

    ofs2 << "Group\tCount\tDPrime\tRSquare\n";

    for (int i = 0; i < n; ++i) {
        auto c = count(acc1[i]);
        ofs2 << (i + 1) * 1000 << "\t" << c << "\t";
        if (c != 0)
            ofs2 << mean(acc1[i]) << "\t" << mean(acc2[i]) << "\n";
        else
            ofs2 << "NA\tNA\n";
    }

    return 0;
}


/*
 * StokesM2L.cpp
 *
 *  Created on: Oct 12, 2016
 *      Author: wyan
 */

#include "SVD_pvfmm.hpp"

#include <Eigen/Dense>

#include <chrono>
#include <iomanip>
#include <iostream>

#define DIRECTLAYER 2
#define PI314 3.1415926535897932384626433

namespace Stokes3D3D {

inline double ERFC(double x) { return std::erfc(x); }
inline double ERF(double x) { return std::erf(x); }

/*
 * def AEW(xi,rvec):
 r=np.sqrt(rvec.dot(rvec))
 A = 2*(xi*np.exp(-(xi**2)*(r**2))/(np.sqrt(np.pi)*r**2)+ss.erfc(xi*r)/(2*r**3))
 *(r*r*np.identity(3)+np.outer(rvec,rvec)) -
 4*xi/np.sqrt(np.pi)*np.exp(-(xi**2)*(r**2))*np.identity(3)
 return A
 *
 * */
inline Eigen::Matrix3d AEW(const double xi, const Eigen::Vector3d &rvec) {
    const double r = rvec.norm();
    Eigen::Matrix3d A = 2 * (xi * exp(-(xi * xi) * (r * r)) / (sqrt(PI314) * r * r) + erfc(xi * r) / (2 * r * r * r)) *
                            (r * r * Eigen::Matrix3d::Identity() + (rvec * rvec.transpose())) -
                        4 * xi / sqrt(PI314) * exp(-(xi * xi) * (r * r)) * Eigen::Matrix3d::Identity();
    return A;
}

/*
 *
 def BEW(xi,kvec):
 k=np.sqrt(kvec.dot(kvec))
 B =
 8*np.pi*(1+k*k/(4*(xi**2)))*((k**2)*np.identity(3)-np.outer(kvec,kvec))/(k**4)
 return B*np.exp(-k**2/(4*xi**2))
 *
 * */
inline Eigen::Matrix3d BEW(const double xi, const Eigen::Vector3d &kvec) {
    const double k = kvec.norm();
    Eigen::Matrix3d B = 8 * PI314 * (1 + k * k / (4 * (xi * xi))) *
                        ((k * k) * Eigen::Matrix3d::Identity() - (kvec * kvec.transpose())) / (k * k * k * k);
    B *= exp(-k * k / (4 * xi * xi));
    return B;
}

/*
 * def stokes3DEwald(rvec,force):
 xi = 2
 r=np.sqrt(rvec.dot(rvec))
 real = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 real = real + AEW(xi,rvec+1.0*np.array([i,j,k])).dot(force)
 wave = 0
 N=4
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 kvec=2*np.pi*np.array([i,j,k]) # L = 1
 if(i==0 and j==0 and k==0):
 continue
 else:
 wave = wave + BEW(xi,kvec).dot(force)*np.exp(-complex(0,1)*kvec.dot(rvec))

 return (np.real(wave)+real)

 * */
inline void GkernelEwald(const Eigen::Vector3d &rvec, Eigen::Matrix3d &Gsum) {
    const double xi = 2;
    const double r = rvec.norm();
    Eigen::Matrix3d real = Eigen::Matrix3d::Zero();
    const int N = 5;
    if (r < 1e-14) {
        auto Gself = -4 * xi / sqrt(PI314) * Eigen::Matrix3d::Identity(); // the self term
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    if (i == 0 && j == 0 && k == 0) {
                        continue;
                    }
                    real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, k));
                }
            }
        }
        real += Gself;
    } else {
        for (int i = -N; i < N + 1; i++) {
            for (int j = -N; j < N + 1; j++) {
                for (int k = -N; k < N + 1; k++) {
                    real = real + AEW(xi, rvec + Eigen::Vector3d(i, j, k));
                }
            }
        }
    }
    Eigen::Matrix3d wave = Eigen::Matrix3d::Zero();

    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Eigen::Vector3d kvec(2 * PI314 * i, 2 * PI314 * j, 2 * PI314 * k);
                if (i == 0 and j == 0 and k == 0) {
                    continue;
                } else {
                    wave = wave + BEW(xi, kvec) * cos(kvec.dot(rvec));
                }
            }
        }
    }
    Gsum = real + wave;
}

inline void Gkernel(const Eigen::Vector3d &target, const Eigen::Vector3d &source, Eigen::Matrix3d &answer) {
    auto rst = target - source;
    double rnorm = rst.norm();
    if (rnorm < 1e-14) {
        answer = Eigen::Matrix3d::Zero();
        return;
    }
    auto part2 = rst * rst.transpose() / (rnorm * rnorm * rnorm);
    auto part1 = Eigen::Matrix3d::Identity() / rnorm;
    answer = part1 + part2;
}

/*
 *
 def stokes3DM2L(rvec,force):
 uEwald=stokes3DEwald(rvec,force)
 uNB=0
 N=3
 for i in range(-N,N+1):
 for j in range(-N,N+1):
 for k in range(-N,N+1):
 uNB=uNB+Gkernel(rvec-np.array([i,j,k])).dot(force)
 return uEwald-uNB
 * */
// Out of Layer 1
inline void GkernelEwaldO1(const Eigen::Vector3d &rvec, Eigen::Matrix3d &GsumO1) {
    Eigen::Matrix3d Gfree = Eigen::Matrix3d::Zero();
    GkernelEwald(rvec, GsumO1);
    const int N = DIRECTLAYER;
    for (int i = -N; i < N + 1; i++) {
        for (int j = -N; j < N + 1; j++) {
            for (int k = -N; k < N + 1; k++) {
                Gkernel(rvec, Eigen::Vector3d(i, j, k), Gfree);
                GsumO1 -= Gfree;
            }
        }
    }
}

/**
 * \brief Returns the coordinates of points on the surface of a cube.
 * \param[in] p Number of points on an edge of the cube is (n+1)
 * \param[in] c Coordinates to the centre of the cube (3D array).
 * \param[in] alpha Scaling factor for the size of the cube.
 * \param[in] depth Depth of the cube in the octree.
 * \return Vector with coordinates of points on the surface of the cube in the
 * format [x0 y0 z0 x1 y1 z1 .... ].
 */

template <class Real_t>
std::vector<Real_t> surface(int p, Real_t *c, Real_t alpha, int depth) {
    size_t n_ = (6 * (p - 1) * (p - 1) + 2); // Total number of points.

    std::vector<Real_t> coord(n_ * 3);
    coord[0] = coord[1] = coord[2] = -1.0;
    size_t cnt = 1;
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = -1.0;
            coord[cnt * 3 + 1] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = (2.0 * j - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * i - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = -1.0;
            coord[cnt * 3 + 2] = (2.0 * (j + 1) - p + 1) / (p - 1);
            cnt++;
        }
    for (int i = 0; i < p - 1; i++)
        for (int j = 0; j < p - 1; j++) {
            coord[cnt * 3] = (2.0 * (i + 1) - p + 1) / (p - 1);
            coord[cnt * 3 + 1] = (2.0 * j - p + 1) / (p - 1);
            coord[cnt * 3 + 2] = -1.0;
            cnt++;
        }
    for (size_t i = 0; i < (n_ / 2) * 3; i++)
        coord[cnt * 3 + i] = -coord[i];

    Real_t r = 0.5 * pow(0.5, depth);
    Real_t b = alpha * r;
    for (size_t i = 0; i < n_; i++) {
        coord[i * 3 + 0] = (coord[i * 3 + 0] + 1.0) * b + c[0];
        coord[i * 3 + 1] = (coord[i * 3 + 1] + 1.0) * b + c[1];
        coord[i * 3 + 2] = (coord[i * 3 + 2] + 1.0) * b + c[2];
    }
    return coord;
}

int main(int argc, char **argv) {
    Eigen::initParallel();
    Eigen::setNbThreads(1);
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    const double scaleLEquiv = 1.05;
    const double scaleLCheck = 2.95;
    const double pCenterLEquiv[3] = {-(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2, -(scaleLEquiv - 1) / 2};
    const double pCenterLCheck[3] = {-(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2, -(scaleLCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    auto pointLEquiv = surface(pEquiv, (double *)&(pCenterLCheck[0]), scaleLCheck,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0
    auto pointLCheck = surface(pCheck, (double *)&(pCenterLEquiv[0]), scaleLEquiv,
                               0); // center at 0.5,0.5,0.5, periodic box 1,1,1, scale 1.05, depth = 0

    //	for (int i = 0; i < pointLEquiv.size() / 3; i++) {
    //		std::cout << pointLEquiv[3 * i] << " " << pointLEquiv[3 * i + 1]
    //<< " " << pointLEquiv[3 * i + 2] << " "
    //				<< std::endl;
    //	}
    //
    //	for (int i = 0; i < pointLCheck.size() / 3; i++) {
    //		std::cout << pointLCheck[3 * i] << " " << pointLCheck[3 * i + 1]
    //<< " " << pointLCheck[3 * i + 2] << " "
    //				<< std::endl;
    //	}

    // const int imageN = 100; // images to sum
    // calculate the operator M2L with least square
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointLCheck.size() / 3;
    Eigen::MatrixXd M2L(3 * equivN, 3 * equivN);
    Eigen::MatrixXd A(3 * checkN, 3 * equivN);
#pragma omp parallel for
    for (int k = 0; k < checkN; k++) {
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const Eigen::Vector3d Lpoint(pointLEquiv[3 * l], pointLEquiv[3 * l + 1], pointLEquiv[3 * l + 2]);
            Gkernel(Cpoint, Lpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    Eigen::MatrixXd ApinvU(A.cols(), A.rows());
    Eigen::MatrixXd ApinvVT(A.cols(), A.rows());
    pinv(A, ApinvU, ApinvVT);

#pragma omp parallel for
    for (int i = 0; i < equivN; i++) {
        const Eigen::Vector3d Mpoint(pointMEquiv[3 * i], pointMEquiv[3 * i + 1], pointMEquiv[3 * i + 2]);
        //		std::cout<<"debug:"<<Mpoint<<std::endl;
        // assemble linear system
        Eigen::MatrixXd f(3 * checkN, 3);
        for (int k = 0; k < checkN; k++) {
            Eigen::Matrix3d temp = Eigen::Matrix3d::Zero();
            Eigen::Vector3d Cpoint(pointLCheck[3 * k], pointLCheck[3 * k + 1], pointLCheck[3 * k + 2]);
            //			std::cout<<"debug:"<<k<<std::endl;
            // sum the images
            // use 3D Ewald subtract the first layer
            GkernelEwaldO1(Cpoint - Mpoint, temp);
            f.block<3, 3>(3 * k, 0) = temp;
        }

        M2L.block(0, 3 * i, 3 * equivN, 3) = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    }
    std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();

    // dump M2L
    for (int i = 0; i < 3 * equivN; i++) {
        for (int j = 0; j < 3 * equivN; j++) {
            std::cout << i << " " << j << " " << std::scientific << std::setprecision(18) << M2L(i, j) << std::endl;
        }
    }

    std::cout << "Precomputing time:" << duration / 1e6 << std::endl;

    /*
     * pointForce=[(np.array([1.0,0,0]),np.array([0.1,0.55,0.2]))
     ,(np.array([-1.0,1.0,1.0]),np.array([0.5,0.1,0.3]))
     ,(np.array([0.0,0.0,-1.0]),np.array([0.8,0.5,0.7]))]
     * */
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> forcePoint(3);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> forceValue(3);
    forcePoint[0] = Eigen::Vector3d(0.1, 0.55, 0.2);
    forceValue[0] = Eigen::Vector3d(1, 0, 0);
    forcePoint[1] = Eigen::Vector3d(0.5, 0.1, 0.3);
    forceValue[1] = Eigen::Vector3d(-1, 1, 1);
    forcePoint[2] = Eigen::Vector3d(0.8, 0.5, 0.7);
    forceValue[2] = Eigen::Vector3d(0, 0, -1);

    // solve M
    A.resize(3 * checkN, 3 * equivN);
    ApinvU.resize(A.cols(), A.rows());
    ApinvVT.resize(A.cols(), A.rows());
    Eigen::VectorXd f(3 * checkN);
    for (int k = 0; k < checkN; k++) {
        Eigen::Vector3d temp = Eigen::Vector3d::Zero();
        Eigen::Matrix3d G = Eigen::Matrix3d::Zero();
        Eigen::Vector3d Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (size_t p = 0; p < forcePoint.size(); p++) {
            Gkernel(Cpoint, forcePoint[p], G);
            temp = temp + G * (forceValue[p]);
        }
        f.block<3, 1>(3 * k, 0) = temp;
        for (int l = 0; l < equivN; l++) {
            Eigen::Vector3d Mpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Gkernel(Cpoint, Mpoint, G);
            A.block<3, 3>(3 * k, 3 * l) = G;
        }
    }
    pinv(A, ApinvU, ApinvVT);
    Eigen::VectorXd Msource = (ApinvU.transpose() * (ApinvVT.transpose() * f));
    // impose net charge equal
    double fx = 0, fy = 0, fz = 0;
    for (int i = 0; i < equivN; i++) {
        fx += Msource[3 * i];
        fy += Msource[3 * i + 1];
        fz += Msource[3 * i + 2];
    }
    std::cout << "fx svd before correction: " << fx << std::endl;
    std::cout << "fy svd before correction: " << fy << std::endl;
    std::cout << "fz svd before correction: " << fz << std::endl;
    double fnetx = 0;
    double fnety = 0;
    double fnetz = 0;
    for (size_t p = 0; p < forcePoint.size(); p++) {
        fnetx += (forceValue[p][0]);
        fnety += (forceValue[p][1]);
        fnetz += (forceValue[p][2]);
    }
    /*
     * fx=(fx-fnet[0])/len(MPoints)
     fy=(fy-fnet[1])/len(MPoints)
     fz=(fz-fnet[2])/len(MPoints)
     * */
    fx = (fx - fnetx) / equivN;
    fy = (fy - fnety) / equivN;
    fz = (fz - fnetz) / equivN;
    for (int i = 0; i < equivN; i++) {
        Msource[3 * i] -= fx;
        Msource[3 * i + 1] -= fy;
        Msource[3 * i + 2] -= fz;
    }
    std::cout << "Msource: " << Msource << std::endl;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> forcePointExt(0);
    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> forceValueExt(0);
    for (size_t p = 0; p < forcePoint.size(); p++) {
        for (int i = -DIRECTLAYER; i < DIRECTLAYER + 1; i++) {
            for (int j = -DIRECTLAYER; j < DIRECTLAYER + 1; j++) {
                for (int k = -DIRECTLAYER; k < DIRECTLAYER + 1; k++) {
                    forcePointExt.push_back(Eigen::Vector3d(i, j, k) + forcePoint[p]);
                    forceValueExt.push_back(forceValue[p]);
                }
            }
        }
    }

    Eigen::VectorXd M2Lsource = M2L * (Msource);

    Eigen::Vector3d samplePoint(0.5, 0.5, 0.5);
    Eigen::Vector3d Usample(0, 0, 0);
    Eigen::Vector3d UsampleSP(0, 0, 0);
    Eigen::Matrix3d G;
    for (size_t p = 0; p < forcePointExt.size(); p++) {
        Gkernel(samplePoint, forcePointExt[p], G);
        Usample = Usample + G * (forceValueExt[p]);
    }
    std::cout << "Usample Direct:" << Usample << std::endl;
    for (int p = 0; p < equivN; p++) {
        Eigen::Vector3d Lpoint(pointLEquiv[3 * p], pointLEquiv[3 * p + 1], pointLEquiv[3 * p + 2]);
        Eigen::Vector3d Fpoint(M2Lsource[3 * p], M2Lsource[3 * p + 1], M2Lsource[3 * p + 2]);
        Gkernel(samplePoint, Lpoint, G);
        UsampleSP = UsampleSP + G * (Fpoint);
    }

    std::cout << "Usample M2L:" << UsampleSP << std::endl;
    std::cout << "Usample M2L total:" << UsampleSP + Usample << std::endl;

    Eigen::Vector3d UsampleDirect = 0 * Usample;
    for (size_t p = 0; p < forcePoint.size(); p++) {
        GkernelEwald(samplePoint - forcePoint[p], G);
        UsampleDirect += G * (forceValue[p]);
    }
    std::cout << "Usample Ewald:" << UsampleDirect << std::endl;

    std::cout << "error" << UsampleSP + Usample - UsampleDirect << std::endl;

    return 0;
}

} // namespace Stokes3D3D

#undef PI314
#undef DIRECTLAYER


//  Copyright (c) 2007 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Computes test data for the various bessel functions using
// archived - deliberately naive - version of the code.
// We'll rely on the high precision of boost::math::ntl::RR to get us out of
// trouble and not worry about how long the calculations take.
// This provides a reasonably independent set of test data to
// compare against newly added asymptotic expansions etc.
//
#include <fstream>

#include <boost/math/tools/test_data.hpp>
#include "ntl_rr_lanczos.hpp"

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

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

// Compute J(v, x) and Y(v, x) simultaneously by Steed's method, see
// Barnett et al, Computer Physics Communications, vol 8, 377 (1974)
template <typename T>
int bessel_jy_bare(T v, T x, T* J, T* Y, int kind = need_j|need_y)
{
    // Jv1 = J_(v+1), Yv1 = Y_(v+1), fv = J_(v+1) / J_v
    // Ju1 = J_(u+1), Yu1 = Y_(u+1), fu = J_(u+1) / J_u
    T u, Jv, Ju, Yv, Yv1, Yu, Yu1, fv, fu;
    T W, p, q, gamma, current, prev, next;
    bool reflect = false;
    int n, k, s;

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

    if (v < 0)
    {
        reflect = true;
        v = -v;                             // v is non-negative from here
        kind = need_j|need_y;               // need both for reflection formula
    }
    n = real_cast<int>(v + 0.5L);
    u = v - n;                              // -1/2 <= u < 1/2

    if (x < 0)
    {
       *J = *Y = policies::raise_domain_error<T>("",
          "Real argument x=%1% must be non-negative, complex number result not supported", x, policies::policy<>());
        return 1;
    }
    if (x == 0)
    {
       *J = *Y = policies::raise_overflow_error<T>(
          "", 0, policies::policy<>());
       return 1;
    }

    // x is positive until reflection
    W = T(2) / (x * pi<T>());               // Wronskian
    if (x <= 2)                           // x in (0, 2]
    {
       if(temme_jy(u, x, &Yu, &Yu1, policies::policy<>()))             // Temme series
        {
           // domain error:
           *J = *Y = Yu;
           return 1;
        }
        prev = Yu;
        current = Yu1;
        for (k = 1; k <= n; k++)            // forward recurrence for Y
        {
            next = 2 * (u + k) * current / x - prev;
            prev = current;
            current = next;
        }
        Yv = prev;
        Yv1 = current;
        CF1_jy(v, x, &fv, &s, policies::policy<>());                 // continued fraction CF1
        Jv = W / (Yv * fv - Yv1);           // Wronskian relation
    }
    else                                    // x in (2, \infty)
    {
        // Get Y(u, x):
        CF1_jy(v, x, &fv, &s, policies::policy<>());
        // tiny initial value to prevent overflow
        T init = sqrt(tools::min_value<T>());
        prev = fv * s * init;
        current = s * init;
        for (k = n; k > 0; k--)             // backward recurrence for J
        {
            next = 2 * (u + k) * current / x - prev;
            prev = current;
            current = next;
        }
        T ratio = (s * init) / current;     // scaling ratio
        // can also call CF1() to get fu, not much difference in precision
        fu = prev / current;
        CF2_jy(u, x, &p, &q, policies::policy<>());                  // continued fraction CF2
        T t = u / x - fu;                   // t = J'/J
        gamma = (p - t) / q;
        Ju = sign(current) * sqrt(W / (q + gamma * (p - t)));

        Jv = Ju * ratio;                    // normalization

        Yu = gamma * Ju;
        Yu1 = Yu * (u/x - p - q/gamma);

        // compute Y:
        prev = Yu;
        current = Yu1;
        for (k = 1; k <= n; k++)            // forward recurrence for Y
        {
            next = 2 * (u + k) * current / x - prev;
            prev = current;
            current = next;
        }
        Yv = prev;
    }

    if (reflect)
    {
        T z = (u + n % 2) * pi<T>();
        *J = cos(z) * Jv - sin(z) * Yv;     // reflection formula
        *Y = sin(z) * Jv + cos(z) * Yv;
    }
    else
    {
        *J = Jv;
        *Y = Yv;
    }

    return 0;
}

int progress = 0;

template <class T>
T cyl_bessel_j_bare(T v, T x)
{
   T j, y;
   bessel_jy_bare(v, x, &j, &y);

   std::cout << progress++ << ":   J(" << v << ", " << x << ") = " << j << std::endl;

   if(fabs(j) > 1e30)
      throw std::domain_error("");

   return j;
}

template <class T>
T cyl_bessel_i_bare(T v, T x)
{
   using namespace std;
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_i_bare(v, -x);
         if(tools::real_cast<int>(v) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
            "",
            "Got x = %1%, but we need x >= 0", x, policies::policy<>());
   }
   if(x == 0)
   {
      return (v == 0) ? 1 : 0;
   }
   T I, K;
   boost::math::detail::bessel_ik(v, x, &I, &K, 0xffff, policies::policy<>());

   std::cout << progress++ << ":   I(" << v << ", " << x << ") = " << I << std::endl;

   if(fabs(I) > 1e30)
      throw std::domain_error("");

   return I;
}

template <class T>
T cyl_bessel_k_bare(T v, T x)
{
   using namespace std;
   if(x < 0)
   {
      return policies::raise_domain_error<T>(
         "",
         "Got x = %1%, but we need x > 0", x, policies::policy<>());
   }
   if(x == 0)
   {
      return (v == 0) ? policies::raise_overflow_error<T>("", 0, policies::policy<>())
         : policies::raise_domain_error<T>(
         "",
         "Got x = %1%, but we need x > 0", x, policies::policy<>());
   }
   T I, K;
   bessel_ik(v, x, &I, &K, 0xFFFF, policies::policy<>());

   std::cout << progress++ << ":   K(" << v << ", " << x << ") = " << K << std::endl;

   if(fabs(K) > 1e30)
      throw std::domain_error("");

   return K;
}

template <class T>
T cyl_neumann_bare(T v, T x)
{
   T j, y;
   bessel_jy(v, x, &j, &y, 0xFFFF, policies::policy<>());

   std::cout << progress++ << ":   Y(" << v << ", " << x << ") = " << y << std::endl;

   if(fabs(y) > 1e30)
      throw std::domain_error("");

   return y;
}

template <class T>
T sph_bessel_j_bare(T v, T x)
{
   std::cout << progress++ << ":   j(" << v << ", " << x << ") = ";
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   T r = sqrt(constants::pi<T>() / (2 * x)) * cyl_bessel_j_bare(v+0.5, x);
   std::cout << r << std::endl;
   return r;
}

template <class T>
T sph_bessel_y_bare(T v, T x)
{
   std::cout << progress++ << ":   y(" << v << ", " << x << ") = ";
   if((v < 0) || (floor(v) != v))
      throw std::domain_error("");
   T r = sqrt(constants::pi<T>() / (2 * x)) * cyl_neumann_bare(v+0.5, x);
   std::cout << r << std::endl;
   return r;
}

enum
{
   func_J = 0,
   func_Y,
   func_I,
   func_K,
   func_j,
   func_y
};

int main(int argc, char* argv[])
{
   std::cout << std::setprecision(17) << std::scientific;
   std::cout << sph_bessel_j_bare(0., 0.1185395751953125e4) << std::endl;
   std::cout << sph_bessel_j_bare(22., 0.6540834903717041015625) << std::endl;

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   boost::math::ntl::RR::SetPrecision(1000);
   boost::math::ntl::RR::SetOutputPrecision(40);

   int functype = 0;
   std::string letter = "J";

   if(argc == 2)
   {
      if(std::strcmp(argv[1], "--Y") == 0)
      {
         functype = func_Y;
         letter = "Y";
      }
      else if(std::strcmp(argv[1], "--I") == 0)
      {
         functype = func_I;
         letter = "I";
      }
      else if(std::strcmp(argv[1], "--K") == 0)
      {
         functype = func_K;
         letter = "K";
      }
      else if(std::strcmp(argv[1], "--j") == 0)
      {
         functype = func_j;
         letter = "j";
      }
      else if(std::strcmp(argv[1], "--y") == 0)
      {
         functype = func_y;
         letter = "y";
      }
      else
         assert(0);
   }

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the Bessel " << letter << " function\n\n";
   do{
      get_user_parameter_info(arg1, "v");
      get_user_parameter_info(arg2, "x");
      boost::math::ntl::RR (*fp)(boost::math::ntl::RR, boost::math::ntl::RR);
      if(functype == func_J)
         fp = cyl_bessel_j_bare;
      else if(functype == func_I)
         fp = cyl_bessel_i_bare;
      else if(functype == func_K)
         fp = cyl_bessel_k_bare;
      else if(functype == func_Y)
         fp = cyl_neumann_bare;
      else if(functype == func_j)
         fp = sph_bessel_j_bare;
      else if(functype == func_y)
         fp = sph_bessel_y_bare;
      else
         assert(0);

      data.insert(fp, arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=bessel_j_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "bessel_j_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific;
   write_code(ofs, data, line.c_str());

   return 0;
}


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

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/complex.hpp>
#include <amgcl/adapter/crs_tuple.hpp>

#include <amgcl/mpi/util.hpp>
#include <amgcl/mpi/make_solver.hpp>
#include <amgcl/mpi/preconditioner.hpp>
#include <amgcl/mpi/solver/runtime.hpp>

#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

namespace math = amgcl::math;

//---------------------------------------------------------------------------
ptrdiff_t assemble_poisson3d(amgcl::mpi::communicator comm,
        ptrdiff_t n, int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<std::complex<double>> &val,
        std::vector<std::complex<double>> &rhs)
{
    ptrdiff_t n3 = n * n * n;

    ptrdiff_t chunk = (n3 + comm.size - 1) / comm.size;
    if (chunk % block_size != 0) {
        chunk += block_size - chunk % block_size;
    }
    ptrdiff_t row_beg = std::min(n3, chunk * comm.rank);
    ptrdiff_t row_end = std::min(n3, row_beg + chunk);
    chunk = row_end - row_beg;

    ptr.clear(); ptr.reserve(chunk + 1);
    col.clear(); col.reserve(chunk * 7);
    val.clear(); val.reserve(chunk * 7);

    rhs.resize(chunk);
    std::fill(rhs.begin(), rhs.end(), 1.0);

    const double h2i = (n - 1) * (n - 1);
    ptr.push_back(0);

    for (ptrdiff_t idx = row_beg; idx < row_end; ++idx) {
        ptrdiff_t k = idx / (n * n);
        ptrdiff_t j = (idx / n) % n;
        ptrdiff_t i = idx % n;

        if (k > 0)  {
            col.push_back(idx - n * n);
            val.push_back(-h2i);
        }

        if (j > 0)  {
            col.push_back(idx - n);
            val.push_back(-h2i);
        }

        if (i > 0) {
            col.push_back(idx - 1);
            val.push_back(-h2i);
        }

        col.push_back(idx);
        val.push_back(6 * h2i);

        if (i + 1 < n) {
            col.push_back(idx + 1);
            val.push_back(-h2i);
        }

        if (j + 1 < n) {
            col.push_back(idx + n);
            val.push_back(-h2i);
        }

        if (k + 1 < n) {
            col.push_back(idx + n * n);
            val.push_back(-h2i);
        }

        ptr.push_back( col.size() );
    }

    return chunk;
}

//---------------------------------------------------------------------------
void solve_scalar(
        amgcl::mpi::communicator comm,
        ptrdiff_t chunk,
        const std::vector<ptrdiff_t> &ptr,
        const std::vector<ptrdiff_t> &col,
        const std::vector<std::complex<double>> &val,
        const boost::property_tree::ptree &prm,
        const std::vector<std::complex<double>> &rhs
        )
{
    typedef amgcl::backend::builtin<std::complex<double>> Backend;

    typedef
        amgcl::mpi::make_solver<
            amgcl::runtime::mpi::preconditioner<Backend>,
            amgcl::runtime::mpi::solver::wrapper<Backend>
            >
        Solver;

    using amgcl::prof;

    prof.tic("setup");
    Solver solve(comm, std::tie(chunk, ptr, col, val), prm);
    prof.toc("setup");

    if (comm.rank == 0) {
        std::cout << solve << std::endl;
    }

    std::vector<std::complex<double>> x(chunk);

    int    iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = solve(rhs, x);
    prof.toc("solve");

    if (comm.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << error << std::endl
            << prof << std::endl;
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    amgcl::mpi::init_thread mpi(&argc, &argv);
    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    if (comm.rank == 0)
        std::cout << "World size: " << comm.size << std::endl;

    using amgcl::prof;

    // Read configuration from command line
    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "size,n",
         po::value<ptrdiff_t>()->default_value(128),
         "domain size"
        )
        ("prm-file,P",
         po::value<std::string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< std::vector<std::string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::positional_options_description p;
    p.add("prm", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (comm.rank == 0) std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<std::string>(), prm);
    }

    if (vm.count("prm")) {
        for(const std::string &v : vm["prm"].as<std::vector<std::string> >()) {
            amgcl::put(prm, v);
        }
    }

    ptrdiff_t n;
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<std::complex<double>> val;
    std::vector<std::complex<double>> rhs;

    prof.tic("assemble");
    n = assemble_poisson3d(comm, vm["size"].as<ptrdiff_t>(), 1, ptr, col, val, rhs);
    prof.toc("assemble");

    solve_scalar(comm, n, ptr, col, val, prm, rhs);
}


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

/*!
 Copyright (C) 2008 Allen Kuo

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

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

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

/* This example sets up a callable fixed rate bond with a Hull White pricing
   engine and compares to Bloomberg's Hull White price/yield calculations.
*/

#include <ql/experimental/callablebonds/callablebond.hpp>
#include <ql/experimental/callablebonds/treecallablebondengine.hpp>
#include <ql/models/shortrate/onefactormodels/hullwhite.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/unitedstates.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <vector>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <boost/timer.hpp>

using namespace std;
using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

boost::shared_ptr<YieldTermStructure>
    flatRate(const Date& today,
             const boost::shared_ptr<Quote>& forward,
             const DayCounter& dc,
             const Compounding& compounding,
             const Frequency& frequency) {
    return boost::shared_ptr<YieldTermStructure>(
                                       new FlatForward(today,
                                                       Handle<Quote>(forward),
                                                       dc,
                                                       compounding,
                                                       frequency));
}

boost::shared_ptr<YieldTermStructure>
    flatRate(const Date& today,
             Rate forward,
             const DayCounter& dc,
             const Compounding &compounding,
             const Frequency &frequency) {
    return flatRate(today,
            boost::shared_ptr<Quote>(new SimpleQuote(forward)),
            dc,
            compounding,
            frequency);
}

int main(int, char* [])
{
    try {

        boost::timer timer;

        Date today = Date(16,October,2007);
        Settings::instance().evaluationDate() = today;

        cout <<  endl;
        cout << "Pricing a callable fixed rate bond using" << endl;
        cout << "Hull White model w/ reversion parameter = 0.03" << endl;
        cout << "BAC4.65 09/15/12  ISIN: US06060WBJ36" << endl;
        cout << "roughly five year tenor, ";
        cout << "quarterly coupon and call dates" << endl;
        cout << "reference date is : " << today << endl << endl;

        /* Bloomberg OAS1: "N" model (Hull White)
           varying volatility parameter

           The curve entered into Bloomberg OAS1 is a flat curve,
           at constant yield = 5.5%, semiannual compounding.
           Assume here OAS1 curve uses an ACT/ACT day counter,
           as documented in PFC1 as a "default" in the latter case.
        */

        // set up a flat curve corresponding to Bloomberg flat curve

        Rate bbCurveRate = 0.055;
        DayCounter bbDayCounter = ActualActual(ActualActual::Bond);
        InterestRate bbIR(bbCurveRate,bbDayCounter,Compounded,Semiannual);

        Handle<YieldTermStructure> termStructure(flatRate(today,
                                                          bbIR.rate(),
                                                          bbIR.dayCounter(),
                                                          bbIR.compounding(),
                                                          bbIR.frequency()));

        // set up the call schedule

        CallabilitySchedule callSchedule;
        Real callPrice = 100.;
        Size numberOfCallDates = 24;
        Date callDate = Date(15,September,2006);

        for (Size i=0; i< numberOfCallDates; i++) {
            Calendar nullCalendar = NullCalendar();

            Callability::Price myPrice(callPrice,
                                       Callability::Price::Clean);
            callSchedule.push_back(
                boost::shared_ptr<Callability>(
                    new Callability(myPrice,
                                    Callability::Call,
                                    callDate )));
            callDate = nullCalendar.advance(callDate, 3, Months);
        }

        // set up the callable bond

        Date dated = Date(16,September,2004);
        Date issue = dated;
        Date maturity = Date(15,September,2012);
        Natural settlementDays = 3;  // Bloomberg OAS1 settle is Oct 19, 2007
        Calendar bondCalendar = UnitedStates(UnitedStates::GovernmentBond);
        Real coupon = .0465;
        Frequency frequency = Quarterly;
        Real redemption = 100.0;
        Real faceAmount = 100.0;

        /* The 30/360 day counter Bloomberg uses for this bond cannot
           reproduce the US Bond/ISMA (constant) cashflows used in PFC1.
           Therefore use ActAct(Bond)
        */
        DayCounter bondDayCounter = ActualActual(ActualActual::Bond);

        // PFC1 shows no indication dates are being adjusted
        // for weekends/holidays for vanilla bonds
        BusinessDayConvention accrualConvention = Unadjusted;
        BusinessDayConvention paymentConvention = Unadjusted;

        Schedule sch(dated, maturity, Period(frequency), bondCalendar,
                     accrualConvention, accrualConvention,
                     DateGeneration::Backward, false);

        Size maxIterations = 1000;
        Real accuracy = 1e-8;
        Integer gridIntervals = 40;
        Real reversionParameter = .03;

        // output price/yield results for varying volatility parameter

        Real sigma = QL_EPSILON; // core dumps if zero on Cygwin

        boost::shared_ptr<ShortRateModel> hw0(
                       new HullWhite(termStructure,reversionParameter,sigma));

        boost::shared_ptr<PricingEngine> engine0(
                      new TreeCallableFixedRateBondEngine(hw0,gridIntervals));

        CallableFixedRateBond callableBond(settlementDays, faceAmount, sch,
                                           vector<Rate>(1, coupon),
                                           bondDayCounter, paymentConvention,
                                           redemption, issue, callSchedule);
        callableBond.setPricingEngine(engine0);

        cout << setprecision(2)
             << showpoint
             << fixed
             << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

        cout << "QuantLib price/yld (%)  ";
        cout << callableBond.cleanPrice() << " / "
             << 100. * callableBond.yield(bondDayCounter,
                                          Compounded,
                                          frequency,
                                          accuracy,
                                          maxIterations)
             << endl;

        cout << "Bloomberg price/yld (%) ";
        cout << "96.50 / 5.47"
             << endl
             << endl;

        sigma = .01;

        cout << "sigma/vol (%) = " << 100.*sigma << endl;

        boost::shared_ptr<ShortRateModel> hw1(
                       new HullWhite(termStructure,reversionParameter,sigma));

        boost::shared_ptr<PricingEngine> engine1(
                      new TreeCallableFixedRateBondEngine(hw1,gridIntervals));

        callableBond.setPricingEngine(engine1);

        cout << "QuantLib price/yld (%)  ";
        cout << callableBond.cleanPrice() << " / "
             << 100.* callableBond.yield(bondDayCounter,
                                         Compounded,
                                         frequency,
                                         accuracy,
                                         maxIterations)
             << endl;

        cout << "Bloomberg price/yld (%) ";
        cout << "95.68 / 5.66"
             << endl
             << endl;

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

        sigma = .03;

        boost::shared_ptr<ShortRateModel> hw2(
                     new HullWhite(termStructure, reversionParameter, sigma));

        boost::shared_ptr<PricingEngine> engine2(
                      new TreeCallableFixedRateBondEngine(hw2,gridIntervals));

        callableBond.setPricingEngine(engine2);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

        cout << "QuantLib price/yld (%)  ";
        cout << callableBond.cleanPrice() << " / "
             << 100. * callableBond.yield(bondDayCounter,
                                          Compounded,
                                          frequency,
                                          accuracy,
                                          maxIterations)
             << endl;

        cout << "Bloomberg price/yld (%) ";
        cout << "92.34 / 6.49"
             << endl
             << endl;

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

        sigma = .06;

        boost::shared_ptr<ShortRateModel> hw3(
                     new HullWhite(termStructure, reversionParameter, sigma));

        boost::shared_ptr<PricingEngine> engine3(
                      new TreeCallableFixedRateBondEngine(hw3,gridIntervals));

        callableBond.setPricingEngine(engine3);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

        cout << "QuantLib price/yld (%)  ";
        cout << callableBond.cleanPrice() << " / "
             << 100. * callableBond.yield(bondDayCounter,
                                          Compounded,
                                          frequency,
                                          accuracy,
                                          maxIterations)
             << endl;

        cout << "Bloomberg price/yld (%) ";
        cout << "87.16 / 7.83"
             << endl
             << endl;

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

        sigma = .12;

        boost::shared_ptr<ShortRateModel> hw4(
                     new HullWhite(termStructure, reversionParameter, sigma));

        boost::shared_ptr<PricingEngine> engine4(
                      new TreeCallableFixedRateBondEngine(hw4,gridIntervals));

        callableBond.setPricingEngine(engine4);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

        cout << "QuantLib price/yld (%)  ";
        cout << callableBond.cleanPrice() << " / "
             << 100.* callableBond.yield(bondDayCounter,
                                         Compounded,
                                         frequency,
                                         accuracy,
                                         maxIterations)
             << endl;

        cout << "Bloomberg price/yld (%) ";
        cout << "77.31 / 10.65"
             << endl
             << endl;

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        cout << " \nRun completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s\n" << endl;

        return 0;

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}