#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <omp.h>

#include <random>
#include <map>
#include <string>
#include <iomanip>

#include <unistd.h>
#include <string>
#include <algorithm>
#include <random>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>
#include <boost/program_options.hpp>
#include <iterator>

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::LLT;
using Eigen::Lower;
using Eigen::Map;
using Eigen::Upper;
typedef Map<MatrixXd> MapMatd;

boost::random::mt19937 gen(time(0));

//distributions
double runif(double lower, double higher)
{
	boost::random::uniform_real_distribution<> dist(lower, higher);
	return dist(gen);
}

double rnorm(double mean, double sd)
{
	boost::random::normal_distribution<> dist(mean, sd);
	return dist(gen);
}

double rbeta(double alpha, double beta)
{

	boost::math::beta_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rinvchisq(double df, double scale)
{

	boost::math::inverse_chi_squared_distribution<> dist(df, scale);
	double q = quantile(dist, runif(0,1));

	return(q);
}
int rbernoulli(double p)
{
	std::bernoulli_distribution dist(p);
	return dist(gen);
}

//sampling functions
double sample_mu(int N, double Sigma2_e,const VectorXd& Y,const MatrixXd& X,const VectorXd& beta)
{
	double mean=((Y-X*beta).sum())/N;
	double sd=sqrt(Sigma2_e/N);
	double mu=rnorm(mean,sd);
	return(mu);
}

//sample variance of beta
double sample_sigma2_b(const VectorXd& beta,int NZ,double v0B,double s0B){
	double df=v0B+NZ;
	double scale=(beta.squaredNorm()*NZ+v0B*s0B)/(v0B+NZ);
	//cout<<NZ<<"\t"<<beta.squaredNorm()<<"\t"<<df<<"\t"<<scale<<"\t"<<endl;
	double psi2=rinvchisq(df, scale);
	return(psi2);
}

//sample error variance of Y
double sample_sigma2_e(int N,const VectorXd& epsilon,double v0E,double s0E){
	double sigma2=rinvchisq(v0E+N, (epsilon.squaredNorm()+v0E*s0E)/(v0E+N));
	return(sigma2);
}

//sample mixture weight
double sample_w(int M,int NZ){
	double w=rbeta(1+NZ,1+(M-NZ));
	return(w);
}

void ReadFromFile(std::vector<double> &x, const std::string &file_name)
{
	std::ifstream read_file(file_name);
	assert(read_file.is_open());

	std::copy(std::istream_iterator<double>(read_file), std::istream_iterator<double>(),
			std::back_inserter(x));

	read_file.close();
}

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

	po::options_description desc("Options");
	desc.add_options()
		("M", po::value<int>()->required(), "No. of simulated markers")
		("N", po::value<int>()->required(), "No. of simulated individuals")
		("iter", po::value<int>()->default_value(5000), "No. of Gibbs iterations")
		("pNZ", po::value<double>()->default_value(0.5), "Proportion nonzero")
		("input", po::value<std::string>()->default_value("none"),"Input filename")
		("out", po::value<std::string>()->default_value("BayesC_out"),"Output filename")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(argc,argv,desc),vm);
	po::notify(vm);

	int M=vm["M"].as<int>();
	int N=vm["N"].as<int>();
	int iter=vm["iter"].as<int>();
	string input=vm["input"].as<string>();
	string output=vm["out"].as<string>();

	MatrixXd X(N,M);
	VectorXd Y(N);

	//beta coefficients
	VectorXd beta_true(M);
	beta_true.setZero();

	int i,j,k,l,m=0;

	//Was an input matrix given?

	if (input!="none"){ //Either read input tables for X and Y
		ifstream f1(input+".X");
		//f1 >> m >> n;
		for (int i = 0; i < N; i++)
		{
			for (int j = 0; j < M; j++)
			{
				f1 >> X(i,j);
				//cout<<X(i,j)<<endl;
			}
		}
		f1.close();
		cout<<"finished reading matrix X!"<<endl;

		std::vector<double> Y_in;
		ReadFromFile(Y_in, input+".Y");
		double* ptr_Y = &Y_in[0];
		Eigen::Map<Eigen::VectorXd> Y1(ptr_Y, Y_in.size());
		cout<<"finished reading vector Y!"<<endl;
		if(Y_in.size()!=N){cout<<"input Y vector size doesnt much the size indicated in the command line"<<endl;return 0;}
		Y=Y1;
		Y_in.clear();

	}else //or simulate
	{
		double pNZ=vm["pNZ"].as<double>();
		double sigmaY_true=1;
		double sigmab_true=1;
		int MT=pNZ*M;

		//Fill Genotype matrix
		for (i=0;i<N;i++){
			for (j=0;j<M;j++){
				X(i,j)=rnorm(0,1);
			}
		}
		for (i=0;i<MT;i++){
			beta_true[i]=rnorm(0,sigmab_true);
		}

		//error
		VectorXd error(N);
		for (i=0;i<N;i++){
			error[i]=rnorm(0,sigmaY_true);
		}

		//construct phenotypes
		Y=X*beta_true;
		Y+=error;
	}

	//normalize
	RowVectorXd mean = X.colwise().mean();
	RowVectorXd sd = ((X.rowwise() - mean).array().square().colwise().sum() / (X.rows() - 1)).sqrt();
	X = (X.rowwise() - mean).array().rowwise() / sd.array();

	double Emu=0;
	VectorXd vEmu(N);
	vEmu.setOnes();

	VectorXd Ebeta(M);
	Ebeta.setZero();
	VectorXd ny(M);
	ny.setZero();
	double Ew=0.5;
	//residual error
	VectorXd epsilon(N);

	epsilon=Y-X*Ebeta-vEmu*Emu;

	std::vector<int> markerI;
	for (int i=0; i<M; ++i) {
		markerI.push_back(i);
	}
	int marker=0;

	//non-zero variable NZ
	int NZ=0;

	double Sigma2_e=epsilon.squaredNorm()/(N*0.5);
	double Sigma2_b=rbeta(1,1);

	//Standard parameterization of hyperpriors for variances
	//double v0E=0.001,s0E=0.001,v0B=0.001,s0B=0.001;

	// Alternative parameterization of hyperpriors for variances
	double v0E=4,v0B=4;
	double s0B=((v0B-2)/v0B)*Sigma2_b;
	double s0E=((v0E-2)/v0E)*Sigma2_e;

	//pre-computed elements for calculations
	VectorXd el1(M);
	for (int i=0; i<M; ++i) {
		el1[i]=X.col(i).transpose()*X.col(i);
	}

	std::ofstream ofs;
	ofs.open(output+"_estimates.txt");
	for (int i=0; i<M; ++i) {
		ofs << "beta_" <<i<< ' ';
	}
	for (int i=0; i<M; ++i) {
		ofs << "incl_" <<i<< ' ';
	}
	ofs << "Ew" << " ";
	ofs << "Sigma2_b" << " ";
	ofs << "Sigma2_e" << " ";
	ofs << "\n";
	ofs.close();

	//begin GIBBS sampling iterations

	ofs.open (output+"_estimates.txt", std::ios_base::app);
	for (i=0;i<iter;i++){

		Emu=sample_mu(N,Sigma2_e,Y,X,Ebeta);

		//sample effects and probabilities jointly
		std::random_shuffle(markerI.begin(), markerI.end());
		for (j=0;j<M;j++){
			marker=markerI[j];

			epsilon=epsilon+X.col(marker)*Ebeta[marker];

			double Cj=el1[marker]+Sigma2_e/Sigma2_b; //adjusted variance
			double rj=X.col(marker).transpose()*epsilon; // mean

			double ratio=(((exp(-(pow(rj,2))/(2*Cj*Sigma2_e))*sqrt((Sigma2_b*Cj)/Sigma2_e))));
			ratio=Ew/(Ew+ratio*(1-Ew));
			ny[marker]=rbernoulli(ratio);

			if (ny[marker]==0){
				Ebeta[marker]=0;
			}
			else if (ny[marker]==1){
				Ebeta[marker]=rnorm(rj/Cj,Sigma2_e/Cj);
			}

			epsilon=epsilon-X.col(marker)*Ebeta[marker];

		}
		for (j=0;j<M;j++){
			ofs << Ebeta[j] << " ";
		}
		for (j=0;j<M;j++){
			ofs << ny[j] << " ";
		}
		NZ=ny.sum();
		//cout<<NZ<<endl;

		Ew=sample_w(M,NZ);
		epsilon=Y-X*Ebeta-vEmu*Emu;

		Sigma2_b=sample_sigma2_b(Ebeta,NZ,v0B,s0B);
		Sigma2_e=sample_sigma2_e(N,epsilon,v0E,s0E);

		ofs << Ew << " ";
		ofs << Sigma2_b << " ";
		ofs << Sigma2_e << " ";
		ofs << "\n";

	}
	ofs.close();

if (input=="none"){
	//write to files
	ofstream myfile1;
	myfile1.open (output+"_simulated_Y.txt");
	for (i=0;i<N;i++){
		myfile1 << Y[i] << ' ';
	}
	myfile1 << endl;
	myfile1.close();

	ofstream myfile2;
	myfile2.open (output+"_simulated_X.txt");
	for (i=0;i<N;i++){
		for (j=0;j<M;j++){
			myfile2<<X(i,j)<< ' ';
		}
		myfile2<<endl;
	}
	myfile2.close();

	ofstream myfile3;
	myfile3.open (output+"_simulated_betatrue.txt");
	myfile3 << beta_true << ' ';
	myfile3.close();
}

	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2013 - 2014.
// Copyright John Maddock 2013.
// Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// This work is based on an earlier work:
// "Algorithm 910: A Portable C++ Multiple-Precision System for Special-Function Calculations",
// in ACM TOMS, {VOL 37, ISSUE 4, (February 2011)} (C) ACM, 2011. http://doi.acm.org/10.1145/1916461.1916469
//

#include <algorithm>
#include <cstdint>
#include <deque>
#include <functional>
#include <iostream>
#include <limits>
#include <numeric>
#include <vector>
#include <boost/math/constants/constants.hpp>
#include <boost/noncopyable.hpp>

//#define USE_CPP_BIN_FLOAT
#define USE_CPP_DEC_FLOAT
//#define USE_MPFR

#if !defined(DIGIT_COUNT)
#define DIGIT_COUNT 100
#endif

#if !defined(BOOST_NO_CXX11_HDR_CHRONO)
  #include <chrono>
  #define STD_CHRONO std::chrono
#else
  #include <boost/chrono.hpp>
  #define STD_CHRONO boost::chrono
#endif

#if defined(USE_CPP_BIN_FLOAT)
  #include <boost/multiprecision/cpp_bin_float.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<DIGIT_COUNT + 10> > mp_type;
#elif defined(USE_CPP_DEC_FLOAT)
  #include <boost/multiprecision/cpp_dec_float.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<DIGIT_COUNT + 10> > mp_type;
#elif defined(USE_MPFR)
  #include <boost/multiprecision/mpfr.hpp>
  typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<DIGIT_COUNT + 10> > mp_type;
#else
  #error no multiprecision floating type is defined
#endif

template <class clock_type>
struct stopwatch
{
public:
  typedef typename clock_type::duration duration_type;

  stopwatch() : m_start(clock_type::now()) { }

  stopwatch(const stopwatch& other) : m_start(other.m_start) { }

  stopwatch& operator=(const stopwatch& other)
  {
    m_start = other.m_start;
    return *this;
  }

  ~stopwatch() { }

  duration_type elapsed() const
  {
    return (clock_type::now() - m_start);
  }

  void reset()
  {
    m_start = clock_type::now();
  }

private:
  typename clock_type::time_point m_start;
};

namespace my_math
{
  template<class T> T chebyshev_t(const std::int32_t n, const T& x);

  template<class T> T chebyshev_t(const std::uint32_t n, const T& x, std::vector<T>* vp);

  template<class T> bool isneg(const T& x) { return (x < T(0)); }

  template<class T> const T& zero() { static const T value_zero(0); return value_zero; }
  template<class T> const T& one () { static const T value_one (1); return value_one; }
  template<class T> const T& two () { static const T value_two (2); return value_two; }
}

namespace orthogonal_polynomial_series
{
  template<typename T> static inline T orthogonal_polynomial_template(const T& x, const std::uint32_t n, std::vector<T>* const vp = static_cast<std::vector<T>*>(0u))
  {
    // Compute the value of an orthogonal chebyshev polinomial.
    // Use stable upward recursion.

    if(vp != nullptr)
    {
      vp->clear();
      vp->reserve(static_cast<std::size_t>(n + 1u));
    }

    T y0 = my_math::one<T>();

    if(vp != nullptr) { vp->push_back(y0); }

    if(n == static_cast<std::uint32_t>(0u))
    {
      return y0;
    }

    T y1 = x;

    if(vp != nullptr) { vp->push_back(y1); }

    if(n == static_cast<std::uint32_t>(1u))
    {
      return y1;
    }

    T a = my_math::two <T>();
    T b = my_math::zero<T>();
    T c = my_math::one <T>();

    T yk;

    // Calculate higher orders using the recurrence relation.
    // The direction of stability is upward recursion.
    for(std::int32_t k = static_cast<std::int32_t>(2); k <= static_cast<std::int32_t>(n); ++k)
    {
      yk = (((a * x) + b) * y1) - (c * y0);

      y0 = y1;
      y1 = yk;

      if(vp != nullptr) { vp->push_back(yk); }
    }

    return yk;
  }
}

template<class T> T my_math::chebyshev_t(const std::int32_t n, const T& x)
{
  if(my_math::isneg(x))
  {
    const bool b_negate = ((n % static_cast<std::int32_t>(2)) != static_cast<std::int32_t>(0));

    const T y = chebyshev_t(n, -x);

    return (!b_negate ? y : -y);
  }

  if(n < static_cast<std::int32_t>(0))
  {
    const std::int32_t nn = static_cast<std::int32_t>(-n);

    return chebyshev_t(nn, x);
  }
  else
  {
    return orthogonal_polynomial_series::orthogonal_polynomial_template(x, static_cast<std::uint32_t>(n));
  }
}

template<class T> T my_math::chebyshev_t(const std::uint32_t n, const T& x, std::vector<T>* const vp) { return orthogonal_polynomial_series::orthogonal_polynomial_template(x, static_cast<std::int32_t>(n),  vp); }

namespace util
{
  template <class T> float digit_scale()
  {
    const int d = ((std::max)(std::numeric_limits<T>::digits10, 15));
    return static_cast<float>(d) / 300.0F;
  }
}

namespace examples
{
  namespace nr_006
  {
    template<typename T> class hypergeometric_pfq_base : private boost::noncopyable
    {
    public:
      virtual ~hypergeometric_pfq_base() { }

      virtual void ccoef() const = 0;

      virtual T series() const
      {
        using my_math::chebyshev_t;

        // Compute the Chebyshev coefficients.
        // Get the values of the shifted Chebyshev polynomials.
        std::vector<T> chebyshev_t_shifted_values;
        const T z_shifted = ((Z / W) * static_cast<std::int32_t>(2)) - static_cast<std::int32_t>(1);

        chebyshev_t(static_cast<std::uint32_t>(C.size()),
                    z_shifted,
                    &chebyshev_t_shifted_values);

        // Luke: C     ---------- COMPUTE SCALE FACTOR                       ----------
        // Luke: C
        // Luke: C     ---------- SCALE THE COEFFICIENTS                     ----------
        // Luke: C

        // The coefficient scaling is preformed after the Chebyshev summation,
        // and it is carried out with a single division operation.
        bool b_neg = false;

        const T scale = std::accumulate(C.begin(),
                                        C.end(),
                                        T(0),
                                        [&b_neg](T scale_sum, const T& ck) -> T
                                        {
                                          ((!b_neg) ? (scale_sum += ck) : (scale_sum -= ck));
                                          b_neg = (!b_neg);
                                          return scale_sum;
                                        });

        // Compute the result of the series expansion using unscaled coefficients.
        const T sum = std::inner_product(C.begin(),
                                         C.end(),
                                         chebyshev_t_shifted_values.begin(),
                                         T(0));

        // Return the properly scaled result.
        return sum / scale;
      }

    protected:
      const   T             Z;
      const   T             W;
      mutable std::deque<T> C;

      hypergeometric_pfq_base(const T& z,
                              const T& w) : Z(z),
                                            W(w),
                                            C(0u) { }

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 500.0F); }
    };

    template<typename T> class ccoef4_hypergeometric_0f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef4_hypergeometric_0f1(const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              CP(c) { }

      virtual ~ccoef4_hypergeometric_0f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 80.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X1 = N2;

        T C1 = T(1) - CP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          const T DIVFAC = T(1) / X1;

          --X1;

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T term =   (A2 - ((A3 * DIVFAC) * X1))
                         + ((A2 * X1) * ((1 + (C1 + X1)) * Z1))
                         + ((A1 * X1) * ((DIVFAC - (C1 * Z1)) + (X1 * Z1)));

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T CP;
    };

    template<typename T> class ccoef1_hypergeometric_1f0 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef1_hypergeometric_1f0(const T& a,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a) { }

      virtual ~ccoef1_hypergeometric_1f0() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 67.
        const std::int32_t N1 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X1 = N2;

        T V1 = T(1) - AP;

        // Here, we have corrected what appears to be an error in Luke's code.

        // Luke's original code listing has:
        //  AFAC = 2 + FOUR/W
        // But it appears as though the correct form is:
        //  AFAC = 2 - FOUR/W.

        const T AFAC = 2 - (T(4) / hypergeometric_pfq_base<T>::W);

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X1;

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T term = -(((X1 * AFAC) * A1) + ((X1 + V1) * A2)) / (X1 - V1);

          hypergeometric_pfq_base<T>::C.push_front(term);

          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 1600.0F); }
    };

    template<typename T> class ccoef3_hypergeometric_1f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef3_hypergeometric_1f1(const T& a,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              CP(c) { }

      virtual ~ccoef3_hypergeometric_1f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 74.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        std::int32_t X1 = N2;

        T XA  =  X + AP;
        T X3A = (X + 3) - AP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --X1;
          --XA;
          --X3A;

          const T X3A_over_X2 = X3A / static_cast<std::int32_t>(X + 2);

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 =  A1 * (((X + CP) * Z1) - X3A_over_X2);
          const T PART2 =  A2 * (Z1 * ((X + 3) - CP) + (XA / X1));
          const T PART3 =  A3 * X3A_over_X2;

          const T term = (((PART1 + PART2) + PART3) * X1) / XA;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T CP;
    };

    template<typename T> class ccoef6_hypergeometric_1f2 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef6_hypergeometric_1f2(const T& a,
                                const T& b,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              BP(b),
                                              CP(c) { }

      virtual ~ccoef6_hypergeometric_1f2() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 85.
        const std::int32_t N1 = static_cast<std::int32_t>(this->N() + static_cast<std::int32_t>(1));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A4(0);
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        T            PP = X + AP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --PP;

          const std::int32_t TWO_X    = static_cast<std::int32_t>(X * 2);
          const std::int32_t X_PLUS_1 = static_cast<std::int32_t>(X + 1);
          const std::int32_t X_PLUS_3 = static_cast<std::int32_t>(X + 3);
          const std::int32_t X_PLUS_4 = static_cast<std::int32_t>(X + 4);

          const T QQ = T(TWO_X + 3) / static_cast<std::int32_t>(TWO_X + static_cast<std::int32_t>(5));
          const T SS = (X + BP) * (X + CP);

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 =   A1 * (((PP - (QQ * (PP + 1))) * 2) + (SS * Z1));
          const T PART2 =  (A2 * (X + 2)) * ((((TWO_X + 1) * PP) / X_PLUS_1) - ((QQ * 4) * (PP + 1)) + (((TWO_X + 3) * (PP + 2)) / X_PLUS_3) + ((Z1 * 2) * (SS - (QQ * (X_PLUS_1 + BP)) * (X_PLUS_1 + CP))));
          const T PART3 =   A3 * ((((X_PLUS_3 - AP) - (QQ * (X_PLUS_4 - AP))) * 2) + (((QQ * Z1) * (X_PLUS_4 - BP)) * (X_PLUS_4 - CP)));
          const T PART4 = ((A4 * QQ) * (X_PLUS_4 - AP)) / X_PLUS_3;

          const T term = (((PART1 - PART2) + (PART3 - PART4)) * X_PLUS_1) / PP;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A4 = A3;
          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T BP;
      const T CP;
    };

    template<typename T> class ccoef2_hypergeometric_2f1 : public hypergeometric_pfq_base<T>
    {
    public:
      ccoef2_hypergeometric_2f1(const T& a,
                                const T& b,
                                const T& c,
                                const T& z,
                                const T& w) : hypergeometric_pfq_base<T>(z, w),
                                              AP(a),
                                              BP(b),
                                              CP(c) { }

      virtual ~ccoef2_hypergeometric_2f1() { }

      virtual void ccoef() const
      {
        // See Luke 1977 page 59.
        const std::int32_t N1 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(1));
        const std::int32_t N2 = static_cast<std::int32_t>(N() + static_cast<std::int32_t>(2));

        // Luke: C     ---------- START COMPUTING COEFFICIENTS USING         ----------
        // Luke: C     ---------- BACKWARD RECURRENCE SCHEME                 ----------
        // Luke: C
        T A3(0);
        T A2(0);
        T A1(boost::math::tools::root_epsilon<T>());

        hypergeometric_pfq_base<T>::C.resize(1u, A1);

        std::int32_t X  = N1;
        std::int32_t X1 = N2;
        std::int32_t X3 = static_cast<std::int32_t>((X * 2) + 3);

        T X3A = (X + 3) - AP;
        T X3B = (X + 3) - BP;

        const T Z1 = T(4) / hypergeometric_pfq_base<T>::W;

        for(std::int32_t k = static_cast<std::int32_t>(0); k < N1; ++k)
        {
          --X;
          --X1;
          --X3A;
          --X3B;
          X3 -= 2;

          const std::int32_t X_PLUS_2 = static_cast<std::int32_t>(X + 2);

          const T XAB = T(1) / ((X + AP) * (X + BP));

          // The terms have been slightly re-arranged resulting in lower complexity.
          // Parentheses have been added to avoid reliance on operator precedence.
          const T PART1 = (A1 * X1) * (2 - (((AP + X1) * (BP + X1)) * ((T(X3) / X_PLUS_2) * XAB)) + ((CP + X) * (XAB * Z1)));
          const T PART2 = (A2 * XAB) * ((X3A * X3B) - (X3 * ((X3A + X3B) - 1)) + (((3 - CP) + X) * (X1 * Z1)));
          const T PART3 = (A3 * X1) * (X3A / X_PLUS_2) * (X3B * XAB);

          const T term = (PART1 + PART2) - PART3;

          hypergeometric_pfq_base<T>::C.push_front(term);

          A3 = A2;
          A2 = A1;
          A1 = hypergeometric_pfq_base<T>::C.front();
        }

        hypergeometric_pfq_base<T>::C.front() /= static_cast<std::int32_t>(2);
      }

    private:
      const T AP;
      const T BP;
      const T CP;

      virtual std::int32_t N() const { return static_cast<std::int32_t>(util::digit_scale<T>() * 1600.0F); }
    };

    template<class T> T luke_ccoef4_hypergeometric_0f1(const T& a, const T& x);
    template<class T> T luke_ccoef1_hypergeometric_1f0(const T& a, const T& x);
    template<class T> T luke_ccoef3_hypergeometric_1f1(const T& a, const T& b, const T& x);
    template<class T> T luke_ccoef6_hypergeometric_1f2(const T& a, const T& b, const T& c, const T& x);
    template<class T> T luke_ccoef2_hypergeometric_2f1(const T& a, const T& b, const T& c, const T& x);
  }
}

template<class T>
T examples::nr_006::luke_ccoef4_hypergeometric_0f1(const T& a, const T& x)
{
  const ccoef4_hypergeometric_0f1<T> hypergeometric_0f1_object(a, x, T(-20));

  hypergeometric_0f1_object.ccoef();

  return hypergeometric_0f1_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef1_hypergeometric_1f0(const T& a, const T& x)
{
  const ccoef1_hypergeometric_1f0<T> hypergeometric_1f0_object(a, x, T(-20));

  hypergeometric_1f0_object.ccoef();

  return hypergeometric_1f0_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef3_hypergeometric_1f1(const T& a, const T& b, const T& x)
{
  const ccoef3_hypergeometric_1f1<T> hypergeometric_1f1_object(a, b, x, T(-20));

  hypergeometric_1f1_object.ccoef();

  return hypergeometric_1f1_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef6_hypergeometric_1f2(const T& a, const T& b, const T& c, const T& x)
{
  const ccoef6_hypergeometric_1f2<T> hypergeometric_1f2_object(a, b, c, x, T(-20));

  hypergeometric_1f2_object.ccoef();

  return hypergeometric_1f2_object.series();
}

template<class T>
T examples::nr_006::luke_ccoef2_hypergeometric_2f1(const T& a, const T& b, const T& c, const T& x)
{
  const ccoef2_hypergeometric_2f1<T> hypergeometric_2f1_object(a, b, c, x, T(-20));

  hypergeometric_2f1_object.ccoef();

  return hypergeometric_2f1_object.series();
}

int main()
{
  stopwatch<STD_CHRONO::high_resolution_clock> my_stopwatch;
  float total_time = 0.0F;

  std::vector<mp_type> hypergeometric_0f1_results(20U);
  std::vector<mp_type> hypergeometric_1f0_results(20U);
  std::vector<mp_type> hypergeometric_1f1_results(20U);
  std::vector<mp_type> hypergeometric_2f1_results(20U);
  std::vector<mp_type> hypergeometric_1f2_results(20U);

  const mp_type a(mp_type(3) / 7);
  const mp_type b(mp_type(2) / 3);
  const mp_type c(mp_type(1) / 4);

  std::int_least16_t i;

  std::cout << "test hypergeometric_0f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric0F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{}, {3/7}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_0f1_results.begin(),
                hypergeometric_0f1_results.end(),
                [&i, &a](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef4_hypergeometric_0f1(a, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric0F1.
  std::for_each(hypergeometric_0f1_results.begin(),
                hypergeometric_0f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f0." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F0.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f0_results.begin(),
                hypergeometric_1f0_results.end(),
                [&i, &a](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef1_hypergeometric_1f0(a, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F0.
  std::for_each(hypergeometric_1f0_results.begin(),
                hypergeometric_1f0_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {2/3}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f1_results.begin(),
                hypergeometric_1f1_results.end(),
                [&i, &a, &b](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef3_hypergeometric_1f1(a, b, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F1.
  std::for_each(hypergeometric_1f1_results.begin(),
                hypergeometric_1f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_1f2." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric1F2.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7}, {2/3, 1/4}, -(i*EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_1f2_results.begin(),
                hypergeometric_1f2_results.end(),
                [&i, &a, &b, &c](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef6_hypergeometric_1f2(a, b, c, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric1F2.
  std::for_each(hypergeometric_1f2_results.begin(),
                hypergeometric_1f2_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "test hypergeometric_2f1." << std::endl;
  i = 1U;
  my_stopwatch.reset();

  // Generate a table of values of Hypergeometric2F1.
  // Compare with the Mathematica command:
  // Table[N[HypergeometricPFQ[{3/7, 2/3}, {1/4}, -(i * EulerGamma)], 100], {i, 1, 20, 1}]
  std::for_each(hypergeometric_2f1_results.begin(),
                hypergeometric_2f1_results.end(),
                [&i, &a, &b, &c](mp_type& new_value)
                {
                  const mp_type x(-(boost::math::constants::euler<mp_type>() * i));

                  new_value = examples::nr_006::luke_ccoef2_hypergeometric_2f1(a, b, c, x);

                  ++i;
                });

  total_time += STD_CHRONO::duration_cast<STD_CHRONO::duration<float> >(my_stopwatch.elapsed()).count();

  // Print the values of Hypergeometric2F1.
  std::for_each(hypergeometric_2f1_results.begin(),
                hypergeometric_2f1_results.end(),
                [](const mp_type& h)
                {
                  std::cout << std::setprecision(DIGIT_COUNT) << h << std::endl;
                });

  std::cout << "Total execution time = " << std::setprecision(3) << total_time << "s" << std::endl;
}


//
// Created by leanne on 11/23/20.
//

#include "utility.h"
#include "VWAP.h"
#include <ctime>
#include <cmath>
#include <random>
#include <algorithm>
#include <cstdio>
#include <boost/timer/timer.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using std::vector;
using std::ofstream;
using namespace boost::accumulators;
using timer = boost::timer::auto_cpu_timer;

void VWAPOption::computefirst2Moments(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<1>,tag::variance(immediate) > > moment1;
    accumulator_set<double, stats<tag::moment<2>,tag::variance(immediate) > > moment2;

    //compute estimate for 1st moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment1(VWAP_T);
    }

    //compute estimate for 2nd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment2(VWAP_T);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP) = " << moment<1>(moment1) << std::endl;
    cout << "E(VWAP^2) = " << moment<2>(moment2) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<1>(moment1) //E(VWAP)
         << ", "
         << sqrt(variance(moment1)) //StdErr(VWAP)
         << ", "
         << moment<2>(moment2) //E(VWAP)
         << ", "
         << sqrt(variance(moment2)) //StdErr(VWAP^2)
         << std::endl;
}

void VWAPOption::computeMoment3(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<3> > > moment3;

    //compute estimate for 3rd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment3(VWAP_T);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(moment3) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<3>(moment3) //E(VWAP^3)
         << std::endl;
}

void VWAPOption::computePrice(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::mean, tag::variance(immediate) > > price;

    //compute estimate for VWAP price
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        if((VWAP_T - K_ ) > 0)
        {
            price(VWAP_T - K_);
        }
        else
        {
            price(0.0);
        }
    }
    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "price = " << mean(price) << std::endl;
    cout << "stdDevPrice = " << sqrt(variance(price)) << std::endl;

    file << sigmaPrice_
         << ", "
         << mean(price) //option price
         << ", "
         << sqrt(variance(price)) << std::endl;

}
void VWAPOption::computeMoment3AndPrice(ofstream& file) {
    timer t;
    accumulator_set<double, stats<tag::moment<3> > > moment3;
    accumulator_set<double, stats<tag::mean, tag::variance(immediate) > > price;

    //compute estimate for 3rd moment
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        moment3(VWAP_T);
    }

    //compute estimate for VWAP price
    for (int i = 0; i < NPaths_; ++i) {
        double VWAP_T = simulateSinglePath();
        if((VWAP_T - K_ ) > 0)
            price(VWAP_T - K_);
        else
            price(0.0);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(2) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(moment3) << std::endl;
    cout << "price = " << mean(price) << std::endl;
    cout << "stdDevPrice = " << sqrt(variance(price)) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<3>(moment3) //E(VWAP^3)
         << ", "
         << mean(price) //option price
         << ", "
         << sqrt(variance(price)) << std::endl;

}

void VWAPOption::computeParameters(ofstream& file) {
    using std::cout;
    timer t;
    for (int i = 0; i < NPaths_; ++i) {

        double VWAP_T = simulateSinglePath();

        VWAPMoments_(VWAP_T);

        if((VWAP_T - K_ ) > 0)
            callOptionPayoffs_(VWAP_T - K_);
        else
            callOptionPayoffs_(0.0);
    }

    cout << std::endl;
    cout << "computation time = " << t.format(3) << std::endl;
    cout << "sigmaPrice = " << sigmaPrice_ << std::endl;
    cout << "Delta = " << delta_ << std::endl;
    cout << "E(VWAP) = " << moment<1>(VWAPMoments_) << std::endl;
    cout << "E(VWAP^2) = " << moment<2>(VWAPMoments_) << std::endl;
    cout << "E(VWAP^3) = " << moment<3>(VWAPMoments_) << std::endl;
    cout << "price = " << mean(callOptionPayoffs_) << std::endl;
    cout << "stdDev = " << sqrt(variance(callOptionPayoffs_)) << std::endl;

    file << sigmaPrice_
         << ", "
         << moment<1>(VWAPMoments_) //E(VWAP)
         << ", "
         << moment<2>(VWAPMoments_) //E(VWAP^2)
         << ", "
         << moment<3>(VWAPMoments_) //E(VWAP^3)
         << ", "
         << mean(callOptionPayoffs_) //option price
         << ", "
         << sqrt(variance(callOptionPayoffs_)) << std::endl;
}

double VWAPOption::GetNormDistrDouble(std::mt19937 &gen)
{
    double val = 1;
    std::normal_distribution<> nd;
    //   val = nd(gen);
    do { val = nd(gen);}
    while(val < 0.4 || val > 2.5);

    return val;
}

//return a realisation of VWAP_T
double VWAPOption::simulateSinglePath() {
    int seed = -107;
    std::mt19937 gen(seed);

    double sumStUt = S0_*X0_*X0_;
    double sumUt   = X0_*X0_;

    double Sti = S0_;
    double Xti = X0_;
    double Uti = 0.0;
    const double muX = a_*(1-exp(-lambda_ * delta_));
    const double sigmaX2 = pow(sigmaVol_,2)/(2*lambda_)*(1-exp(-2*lambda_*delta_));

    //simulate path over [0,T]
    for (int i = 1; i <= numIncrements_; i++) {
        Sti = Sti*exp((mu_-0.5*sigmaPrice_*sigmaPrice_)*delta_
                      + sigmaPrice_*sqrt(delta_))*GetNormDistrDouble(gen);

        Xti = exp(-lambda_*delta_)*Xti + muX + sqrt(sigmaX2)*GetNormDistrDouble(gen);
        Uti = pow(Xti,2);

        sumStUt+= Sti*Uti;
        sumUt += Uti;
    }

    return (sumStUt/sumUt);
}


#include <fstream>
#include <iostream>
#include <experimental/filesystem>
#include <stdio.h>
#include <random>
#include <string>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "../tasks.hh"
#include "../classes/BinnedTypedMatrix.hh"

namespace fs = std::experimental::filesystem;

using namespace EMC;

using namespace boost::numeric::ublas;

/// from  https://stackoverflow.com/questions/440133/how-do-i-create-a-random-alpha-numeric-string-in-c
std::string random_string(std::string::size_type length) {
	static auto& chrs = "0123456789"
			"abcdefghijklmnopqrstuvwxyz"
			"ABCDEFGHIJKLMNOPQRSTUVWXYZ";

	thread_local static std::mt19937 rg { std::random_device { }() };
	thread_local static std::uniform_int_distribution<std::string::size_type> pick(
			0, sizeof(chrs) - 2);

	std::string s;

	s.reserve(length);

	while (length--)
		s += chrs[pick(rg)];

	return s;
}

unsigned int modeCount = 5;

void shiftCenters(std::vector<double>& inputVec, unsigned int mode, double error) {
	switch(mode) {
	case 1: {
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error;
		}
		break;
	}
	case 2: {
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error;
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 3: {
		double half = inputVec.size() / 2.0;
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) += error * (i-half) / inputVec.size();
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 4: {
		double half = inputVec.size() / 2.0;
		for (int i=0; i<inputVec.size(); i++) {
			inputVec.at(i) -= error * (i-half) / inputVec.size();
			if (inputVec.at(i) < 0) {
				inputVec.at(i) = 0;
			}
		}
		break;
	}
	case 0: {
		/// do nothing, mode 0 does not modify bin centers
		break;
	}
	default:
		std::cout << "this should not happen!!" << std::endl;
		break;
	}
}

int EMC::rebin_invert(bool forceOverwrite, std::string input, std::string output,
		int countRate, double minRow, double minCol, double minRowBin,
		double minColBin, double maxRow, double maxCol, double rowError, double colError) {

	if (!forceOverwrite && fs::exists(output)) {
		std::cout << "Output file already exists, use -f to force overwrite." << std::endl;
		return 1;
	}

    std::streambuf* orig_buf = std::cout.rdbuf();
    std::cout.rdbuf(NULL);

	std::string randRebinName = output + "_" + random_string(5) + "_tmp.rebinned.mat";
	rebin(true, input, randRebinName, countRate, minRow, minCol, minRowBin, minColBin, maxRow, maxCol);

	std::ifstream infile(randRebinName);
	BinnedTypedMatrix inputMatrix = BinnedTypedMatrix::readFromFile(infile);
	infile.close();

	fs::remove(randRebinName);

	BinnedTypedMatrix newMat(inputMatrix.columnIndex, inputMatrix.rowIndex, inputMatrix.matType);
	{
		typedef permutation_matrix<std::size_t> pmatrix;
		// create a working copy of the input
		matrix<ValueError> A(inputMatrix.m);
		// create a permutation matrix for the LU-factorization
		pmatrix pm(A.size1());
		// perform LU-factorization
		int res = lu_factorize(A,pm);
		if( res != 0 )
			return false;
		// create identity matrix of "inverse"
		newMat.m.assign(identity_matrix<ValueError>(A.size1()));
		// backsubstitute to get the inverse
		lu_substitute(A, pm, newMat.m);
	}

	std::vector<std::vector<std::vector<real>>> wiggleResults(newMat.rowCount, std::vector<std::vector<real>>(newMat.columnCount, std::vector<real>()));

	for (unsigned int i=0; i<modeCount; i++) {
		for (unsigned int j=0; j<modeCount; j++) {
			if (i==0 && j==0) {
				continue;
			}

		    std::cout.rdbuf(orig_buf);
			std::cout << "mode " << i << " " << j << std::endl;
			std::cout.flush();
		    std::cout.rdbuf(NULL);

			std::string randKey = random_string(5);

		    std::cout.rdbuf(orig_buf);
			std::cout << "shift, ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpShiftedMatrixName = output + "_" + randKey + "_tmp.shifted.mat";
			{ // shift matrix by mode
				std::ifstream infile(input);
				BinnedTypedMatrix inputMat = BinnedTypedMatrix::readFromFile(infile);

				double mult = 0.7071067812;

				if (i==0 || j==0)
					mult=1;

				shiftCenters(inputMat.rowIndex, i, mult*rowError);
				shiftCenters(inputMat.columnIndex, j, mult*colError);

				std::ofstream shiftedMatOutputName(tmpShiftedMatrixName);
				inputMat.writeToFile(shiftedMatOutputName);
			}

		    std::cout.rdbuf(orig_buf);
			std::cout << "rebin, ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpRebinnedMatName = output + "_" + randKey + "_tmp.rebin.mat";
			rebin(true, tmpShiftedMatrixName, tmpRebinnedMatName, countRate, minRow, minCol, minRowBin, minColBin, maxRow, maxCol);

		    std::cout.rdbuf(orig_buf);
			std::cout << "invert ";
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			std::string tmpInvertedMatName = output + "_" + randKey + "_tmp.inverted.mat";
			invert(true, tmpRebinnedMatName, tmpInvertedMatName);

			std::ifstream infile(tmpInvertedMatName);
			BinnedTypedMatrix rebinnedInvertedMatrix = BinnedTypedMatrix::readFromFile(infile);
			infile.close();

			fs::remove(tmpShiftedMatrixName);
			fs::remove(tmpRebinnedMatName);
			fs::remove(tmpInvertedMatName);

		    std::cout.rdbuf(orig_buf);
			std::cout << "and done!" << std::endl;
			std::cout.flush();
		    std::cout.rdbuf(NULL);
			for (unsigned int rowPos = 0; rowPos < newMat.rowCount; rowPos++) {
				for (unsigned int colPos = 0; colPos < newMat.columnCount; colPos++) {
					wiggleResults.at(rowPos).at(colPos).push_back(rebinnedInvertedMatrix.m(rowPos, colPos).value);
				}
			}
		}
	}

	for (unsigned int rowPos = 0; rowPos < newMat.rowCount; rowPos++) {
		for (unsigned int colPos = 0; colPos < newMat.columnCount; colPos++) {
			newMat.m(rowPos, colPos).err_sq = 0;
			for (unsigned int i = 0; i<(modeCount*modeCount-1); i++) {

				// use this (quadratic mean)
				newMat.m(rowPos, colPos).err_sq += pow(wiggleResults.at(rowPos).at(colPos).at(i)-newMat.m(rowPos, colPos).value, 2) / (modeCount*modeCount-1);

				// or this (linear mean)
				/*newMat.m(rowPos, colPos).err_sq += std::fabs(wiggleResults.at(rowPos).at(colPos).at(i)-newMat.m(rowPos, colPos).value) / (maxMode*maxMode);
				newMat.m(rowPos, colPos).err_sq = newMat.m(rowPos, colPos).err_sq * newMat.m(rowPos, colPos).err_sq;*/
			}
		}
	}

    std::ofstream outputF(output);
    newMat.writeToFile(outputF);
    outputF.close();

    return 0;
}


#pragma once

#include <Eigen/Core>

namespace fluid {
namespace algorithm {
namespace _impl {

index incrementalMeanVariance(const Eigen::Ref<Eigen::ArrayXXd> data,
                              index                             lastSampleCount,
                              Eigen::Ref<Eigen::ArrayXd>        mean,
                              Eigen::Ref<Eigen::ArrayXd>        var)
{

  Eigen::VectorXd rowSums = data.isNaN().select(0, data).colwise().sum();
  index          newSampleCount = data.rows();
  Eigen::ArrayXd lastSum = mean * lastSampleCount;

  if (mean.cols() > 0)
  {
    index updatedSampleCount = lastSampleCount + newSampleCount;
    mean = ((mean * lastSampleCount) + rowSums.array()) / updatedSampleCount;

    if (var.rows() > 0)
    {

      Eigen::ArrayXXd tmp  = (data.transpose().colwise() - (rowSums.array() / newSampleCount)).transpose();
      Eigen::ArrayXd correction = tmp.colwise().sum();
      tmp = tmp.square();

      Eigen::ArrayXd newUnnormalisedVar = tmp.colwise().sum();
      newUnnormalisedVar -= correction.square() / newSampleCount;

      Eigen::ArrayXd lastUnormalisedVar = var * lastSampleCount;

      if(lastSampleCount > 0)
      {
          double lastCountOverNewCount = static_cast<double>(lastSampleCount) / newSampleCount;
          var =(
              lastUnormalisedVar
              + newUnnormalisedVar
              + lastCountOverNewCount
                / updatedSampleCount
                * (lastSum / lastCountOverNewCount - rowSums.array()).square()
                );
      }
      var /= updatedSampleCount;
    }
    return updatedSampleCount;
  }
  return lastSampleCount;
}

} // namespace _impl
} // namespace algorithm
} // namespace fluid


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

#include <thread>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>

// #include <opencv2/core/core.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/highgui/highgui.hpp> // needed for verbosity >= 3, DISVISUAL
// #include <opencv2/imgproc/imgproc.hpp> // needed for verbosity >= 3, DISVISUAL

#include <sys/time.h>    // timeof day
#include <stdio.h>

#include "oflow.h"
#include "patchgrid.h"
#include "refine_variational.h"

using std::cout;
using std::endl;
using std::vector;

namespace OFC
{

  OFClass::OFClass(const float ** im_ao_in, const float ** im_ao_dx_in, const float ** im_ao_dy_in, // expects #sc_f_in pointers to float arrays for images and gradients.
                                                                                // E.g. im_ao[sc_f_in] will be used as coarsest coarsest, im_ao[sc_l_in] as finest scale
                                                                                // im_ao[  (sc_l_in-1) : 0 ] can be left as nullptr pointers
                                                                                // IMPORTANT assumption: mod(width,2^sc_f_in)==0  AND mod(height,2^sc_f_in)==0,
                  const float ** im_bo_in, const float ** im_bo_dx_in, const float ** im_bo_dy_in,
                  const int imgpadding_in,
                  float * outflow,
                  const float * initflow,
                  const int width_in, const int height_in,
                  const int sc_f_in, const int sc_l_in,
                  const int max_iter_in, const int min_iter_in,
                  const float  dp_thresh_in,
                  const float  dr_thresh_in,
                  const float res_thresh_in,
                  const int p_samp_s_in,
                  const float patove_in,
                  const bool usefbcon_in,
                  const int costfct_in,
                  const int noc_in,
                  const int patnorm_in,
                  const bool usetvref_in,
                  const float tv_alpha_in,
                  const float tv_gamma_in,
                  const float tv_delta_in,
                  const int tv_innerit_in,
                  const int tv_solverit_in,
                  const float tv_sor_in,
                  const int verbosity_in)
  : im_ao(im_ao_in), im_ao_dx(im_ao_dx_in), im_ao_dy(im_ao_dy_in),
    im_bo(im_bo_in), im_bo_dx(im_bo_dx_in), im_bo_dy(im_bo_dy_in)
{

  #ifdef WITH_OPENMP
    if (verbosity_in>1)
      cout <<  "OPENMP is ON - used in pconst, pinit, potim";
    #ifdef USE_PARALLEL_ON_FLOWAGGR
    if (verbosity_in>1)
      cout << ", cflow ";
    #endif
    if (verbosity_in>1) cout << endl;
  #endif //DWITH_OPENMP

  // Parse optimization parameters
  #if (SELECTMODE==1)
  op.nop = 2;
  #else
  op.nop = 1;
  #endif
  op.p_samp_s = p_samp_s_in;  // patch has even border length, center pixel is at (p_samp_s/2, p_samp_s/2) (ZERO INDEXED!)
  op.outlierthresh = (float)op.p_samp_s/2;
  op.patove = patove_in;
  op.sc_f = sc_f_in;
  op.sc_l = sc_l_in;
  op.max_iter = max_iter_in;
  op.min_iter = min_iter_in;
  op.dp_thresh = dp_thresh_in*dp_thresh_in; // saves the square to compare with squared L2-norm (saves sqrt operation)
  op.dr_thresh = dr_thresh_in;
  op.res_thresh = res_thresh_in;
  op.steps = std::max(1,  (int)floor(op.p_samp_s*(1-op.patove)));
  op.novals = noc_in * (p_samp_s_in)*(p_samp_s_in);
  op.usefbcon = usefbcon_in;
  op.costfct = costfct_in;
  op.noc = noc_in;
  op.patnorm = patnorm_in;
  op.verbosity = verbosity_in;
  op.noscales = op.sc_f-op.sc_l+1;
  op.usetvref = usetvref_in;
  op.tv_alpha = tv_alpha_in;
  op.tv_gamma = tv_gamma_in;
  op.tv_delta = tv_delta_in;
  op.tv_innerit = tv_innerit_in;
  op.tv_solverit = tv_solverit_in;
  op.tv_sor = tv_sor_in;
  op.normoutlier_tmpbsq = (v4sf) {op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier, op.normoutlier*op.normoutlier};
  op.normoutlier_tmp2bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.twos);
  op.normoutlier_tmp4bsq = __builtin_ia32_mulps(op.normoutlier_tmpbsq, op.fours);

  // Variables for algorithm timings
  struct timeval tv_start_all, tv_end_all, tv_start_all_global, tv_end_all_global;
  if (op.verbosity>0)
    gettimeofday(&tv_start_all_global, nullptr);

  // ... per each scale
  double tt_patconstr[op.noscales], tt_patinit[op.noscales], tt_patoptim[op.noscales], tt_compflow[op.noscales], tt_tvopt[op.noscales], tt_all[op.noscales];
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    tt_patconstr[sl-op.sc_l]=0;
    tt_patinit[sl-op.sc_l]=0;
    tt_patoptim[sl-op.sc_l]=0;
    tt_compflow[sl-op.sc_l]=0;
    tt_tvopt[sl-op.sc_l]=0;
    tt_all[sl-op.sc_l]=0;
  }

  if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr);

  // Create grids on each scale
  vector<OFC::PatGridClass*> grid_fw(op.noscales);
  vector<OFC::PatGridClass*> grid_bw(op.noscales); // grid for backward OF computation, only needed if 'usefbcon' is set to 1.
  vector<float*> flow_fw(op.noscales);
  vector<float*> flow_bw(op.noscales);
  cpl.resize(op.noscales);
  cpr.resize(op.noscales);
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    int i = sl-op.sc_l;

    float sc_fct = pow(2,-sl); // scaling factor at current scale
    cpl[i].sc_fct = sc_fct;
    cpl[i].height = height_in * sc_fct;
    cpl[i].width = width_in * sc_fct;
    cpl[i].imgpadding = imgpadding_in;
    cpl[i].tmp_lb = -(float)op.p_samp_s/2;
    cpl[i].tmp_ubw = (float) (cpl[i].width +op.p_samp_s/2-2);
    cpl[i].tmp_ubh = (float) (cpl[i].height+op.p_samp_s/2-2);
    cpl[i].tmp_w = cpl[i].width + 2*imgpadding_in;
    cpl[i].tmp_h = cpl[i].height+ 2*imgpadding_in;
    cpl[i].curr_lv = sl;
    cpl[i].camlr = 0;

    cpr[i] = cpl[i];
    cpr[i].camlr = 1;

    flow_fw[i]   = new float[op.nop * cpl[i].width * cpl[i].height];
    grid_fw[i]   = new OFC::PatGridClass(&(cpl[i]), &(cpr[i]), &op);

    if (op.usefbcon) // for merging forward and backward flow
    {
      flow_bw[i] = new float[op.nop * cpr[i].width * cpr[i].height];
      grid_bw[i] = new OFC::PatGridClass(&(cpr[i]), &(cpl[i]), &op);

      // Make grids known to each other, necessary for AggregateFlowDense();
      grid_fw[i]->SetComplGrid( grid_bw[i] );
      grid_bw[i]->SetComplGrid( grid_fw[i] );
    }
  }

  // Timing, Grid memory allocation
  if (op.verbosity>1)
  {
    gettimeofday(&tv_end_all, nullptr);
    double tt_gridconst = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
    printf("TIME (Grid Memo. Alloc. ) (ms): %3g\n", tt_gridconst);
  }

  // *** Main loop; Operate over scales, coarse-to-fine
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {
    int ii = sl-op.sc_l;

    if (op.verbosity>1) gettimeofday(&tv_start_all, nullptr);

    // Initialize grid (Step 1 in Algorithm 1 of paper)
    grid_fw[ii]->  InitializeGrid(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]);
    grid_fw[ii]->  SetTargetImage(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]);
    if (op.usefbcon)
    {
      grid_bw[ii]->InitializeGrid(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]);
      grid_bw[ii]->SetTargetImage(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]);
    }

    // Timing, Grid construction
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patconstr[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patconstr[ii];
      gettimeofday(&tv_start_all, nullptr);
    }

    // Initialization from previous scale, or to zero at first iteration. (Step 2 in Algorithm 1 of paper)
    if (sl < op.sc_f)
    {
      grid_fw[ii]->InitializeFromCoarserOF(flow_fw[ii+1]); // initialize from flow at previous coarser scale

      // Initialize backward flow
      if (op.usefbcon)
        grid_bw[ii]->InitializeFromCoarserOF(flow_bw[ii+1]);
    }
    else if (sl == op.sc_f && initflow != nullptr) // initialization given input flow
    {
      grid_fw[ii]->InitializeFromCoarserOF(initflow); // initialize from flow at coarser scale
    }

    // Timing, Grid initialization
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patinit[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patinit[ii];
      gettimeofday(&tv_start_all, nullptr);
    }

    // Dense Inverse Search. (Step 3 in Algorithm 1 of paper)
    grid_fw[ii]->Optimize();
    if (op.usefbcon)
      grid_bw[ii]->Optimize();

//     if (op.verbosity==4) // needed for verbosity >= 3, DISVISUAL
//     {
//       grid_fw[ii]->OptimizeAndVisualize(pow(2, sl));
//       if (op.usefbcon)
//         grid_bw[ii]->Optimize();
//     }
//     else
//     {
//       grid_fw[ii]->Optimize();
//       if (op.usefbcon)
//         grid_bw[ii]->Optimize();
//     }

    // Timing, DIS
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_patoptim[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_patoptim[ii];

      gettimeofday(&tv_start_all, nullptr);
    }

    // Densification. (Step 4 in Algorithm 1 of paper)
    float *tmp_ptr = flow_fw[ii];
    if (sl == op.sc_l)
      tmp_ptr = outflow;

    grid_fw[ii]->AggregateFlowDense(tmp_ptr);

    if (op.usefbcon && sl > op.sc_l )  // skip at last scale, backward flow no longer needed
      grid_bw[ii]->AggregateFlowDense(flow_bw[ii]);

    // Timing, Densification
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_compflow[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_compflow[ii];

      gettimeofday(&tv_start_all, nullptr);
    }

    // Variational refinement, (Step 5 in Algorithm 1 of paper)
    if (op.usetvref)
    {
      OFC::VarRefClass varref_fw(im_ao[sl], im_ao_dx[sl], im_ao_dy[sl],
                                im_bo[sl], im_bo_dx[sl], im_bo_dy[sl]
                                ,&(cpl[ii]), &(cpr[ii]), &op, tmp_ptr);

      if (op.usefbcon  && sl > op.sc_l )    // skip at last scale, backward flow no longer needed
          OFC::VarRefClass varref_bw(im_bo[sl], im_bo_dx[sl], im_bo_dy[sl],
                                    im_ao[sl], im_ao_dx[sl], im_ao_dy[sl]
                                    ,&(cpr[ii]), &(cpl[ii]), &op, flow_bw[ii]);
    }

    // Timing, Variational Refinement
    if (op.verbosity>1)
    {
      gettimeofday(&tv_end_all, nullptr);
      tt_tvopt[ii] = (tv_end_all.tv_sec-tv_start_all.tv_sec)*1000.0f + (tv_end_all.tv_usec-tv_start_all.tv_usec)/1000.0f;
      tt_all[ii] += tt_tvopt[ii];
      printf("TIME (Sc: %i, #p:%6i, pconst, pinit, poptim, cflow, tvopt, total): %8.2f %8.2f %8.2f %8.2f %8.2f -> %8.2f ms.\n", sl, grid_fw[ii]->GetNoPatches(), tt_patconstr[ii], tt_patinit[ii], tt_patoptim[ii], tt_compflow[ii], tt_tvopt[ii], tt_all[ii]);
    }

//     if (op.verbosity==3) // Display displacement result of this scale // needed for verbosity >= 3, DISVISUAL
//     {
//       // Display Grid on current scale
//       float sc_fct_tmp = pow(2, sl); // upscale factor
//
//       cv::Mat src(cpl[ii].height+2*cpl[ii].imgpadding, cpl[ii].width+2*cpl[ii].imgpadding, CV_32FC1, (void*) im_ao[sl]);
//       cv::Mat img_ao_mat = src(cv::Rect(cpl[ii].imgpadding, cpl[ii].imgpadding, cpl[ii].width, cpl[ii].height));
//
//       cv::Mat outimg;
//       img_ao_mat.convertTo(outimg, CV_8UC1);
//       cv::cvtColor(outimg, outimg, CV_GRAY2RGB);
//       cv::resize(outimg, outimg, cv::Size(), sc_fct_tmp, sc_fct_tmp, cv::INTER_NEAREST);
//       for (int i = 0; i < grid_fw[ii]->GetNoPatches() ; ++i)
//         DisplayDrawPatchBoundary(outimg, grid_fw[ii]->GetRefPatchPos(i), sc_fct_tmp);
//
//       for (int i = 0; i < grid_fw[ii]->GetNoPatches(); ++i)
//       {
//         // Show displacement vector
//         const Eigen::Vector2f pt_ref = grid_fw[ii]->GetRefPatchPos(i);
//         const Eigen::Vector2f pt_ret = grid_fw[ii]->GetQuePatchPos(i);
//
//         Eigen::Vector2f pta, ptb;
//         cv::line(outimg, cv::Point( (pt_ref[0]+.5)*sc_fct_tmp, (pt_ref[1]+.5)*sc_fct_tmp ), cv::Point( (pt_ret[0]+.5)*sc_fct_tmp, (pt_ret[1]+.5)*sc_fct_tmp ), cv::Scalar(0,255,0),  2);
//       }
//       cv::namedWindow( "Img_ao", cv::WINDOW_AUTOSIZE );
//       cv::imshow( "Img_ao", outimg);
//
//       cv::waitKey(0);
//     }

  }

  // Clean up
  for (int sl=op.sc_f; sl>=op.sc_l; --sl)
  {

    delete[] flow_fw[sl-op.sc_l];
    delete grid_fw[sl-op.sc_l];

    if (op.usefbcon)
    {
      delete[] flow_bw[sl-op.sc_l];
      delete grid_bw[sl-op.sc_l];
    }
  }

  // Timing, total algorithm run-time
  if (op.verbosity>0)
  {
    gettimeofday(&tv_end_all_global, nullptr);
    double tt = (tv_end_all_global.tv_sec-tv_start_all_global.tv_sec)*1000.0f + (tv_end_all_global.tv_usec-tv_start_all_global.tv_usec)/1000.0f;
    printf("TIME (O.Flow Run-Time   ) (ms): %3g\n", tt);
  }

}

// // needed for verbosity >= 3, DISVISUAL
// void OFClass::DisplayDrawPatchBoundary(cv::Mat img, const Eigen::Vector2f pt, const float sc)
// {
//   cv::line(img, cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Point( (pt[0]+.5)*sc, (pt[1]+.5)*sc ), cv::Scalar(0,0,255),  4);
//
//   float lb = -op.p_samp_s/2;
//   float ub = op.p_samp_s/2-1;
//
//   cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+ub)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Scalar(0,0,255),  1);
//   cv::line(img, cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+ub)+.5)*sc ), cv::Point( ((pt[0]+lb)+.5)*sc, ((pt[1]+lb)+.5)*sc ), cv::Scalar(0,0,255),  1);
// }

}


// Copyright Paul A. Bristow 2015

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

// Comparison of finding roots using TOMS748, Newton-Raphson, Halley & Schroder algorithms.
// root_n_finding_algorithms.cpp  Generalised for nth root version.

// http://en.wikipedia.org/wiki/Cube_root

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!
// This program also writes files in Quickbook tables mark-up format.

#include <boost/cstdlib.hpp>
#include <boost/config.hpp>
#include <boost/array.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/tools/roots.hpp>

//using boost::math::policies::policy;
//using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
//using boost::math::tools::bracket_and_solve_root;
//using boost::math::tools::toms748_solve;
//using boost::math::tools::halley_iterate;
//using boost::math::tools::newton_raphson_iterate;
//using boost::math::tools::schroder_iterate;

#include <boost/math/special_functions/next.hpp> // For float_distance.
#include <boost/math/special_functions/pow.hpp> // For pow<N>.
#include <boost/math/tools/tuple.hpp> // for tuple and make_tuple.

#include <boost/multiprecision/cpp_bin_float.hpp> // is binary.
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_bin_float_50;

#include <boost/timer/timer.hpp>
#include <boost/system/error_code.hpp>
#include <boost/preprocessor/stringize.hpp>

// STL
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <limits>
#include <fstream> // std::ofstream
#include <cmath>
#include <typeinfo> // for type name using typid(thingy).name();

#ifdef __FILE__
  std::string sourcefilename = __FILE__;
#else
  std::string sourcefilename("");
#endif

  std::string chop_last(std::string s)
  {
     std::string::size_type pos = s.find_last_of("\\/");
     if(pos != std::string::npos)
        s.erase(pos);
     else if(s.empty())
        abort();
     else
        s.erase();
     return s;
  }

  std::string make_root()
  {
     std::string result;
     if(sourcefilename.find_first_of(":") != std::string::npos)
     {
        result = chop_last(sourcefilename); // lose filename part
        result = chop_last(result);   // lose /example/
        result = chop_last(result);   // lose /math/
        result = chop_last(result);   // lose /libs/
     }
     else
     {
        result = chop_last(sourcefilename); // lose filename part
        if(result.empty())
           result = ".";
        result += "/../../..";
     }
     return result;
  }

  std::string short_file_name(std::string s)
  {
     std::string::size_type pos = s.find_last_of("\\/");
     if(pos != std::string::npos)
        s.erase(0, pos + 1);
     return s;
  }

  std::string boost_root = make_root();

std::string fp_hardware; // Any hardware features like SEE or AVX

const std::string roots_name = "libs/math/doc/roots/";

const std::string full_roots_name(boost_root + "/libs/math/doc/roots/");

const std::size_t nooftypes = 4;
const std::size_t noofalgos = 4;

double digits_accuracy = 1.0; // 1 == maximum possible accuracy.

std::stringstream ss;

std::ofstream fout;

std::vector<std::string> algo_names =
{
  "TOMS748", "Newton", "Halley", "Schr'''ö'''der"
};

std::vector<std::string> names =
{
  "float", "double", "long double", "cpp_bin_float50"
};

uintmax_t iters; // Global as value of iterations is not returned.

struct root_info
{ // for a floating-point type, float, double ...
  std::size_t max_digits10; // for type.
  std::string full_typename; // for type from type_id.name().
  std::string short_typename; // for type "float", "double", "cpp_bin_float_50" ....
  std::size_t bin_digits;  // binary in floating-point type numeric_limits<T>::digits;
  int get_digits; // fraction of maximum possible accuracy required.
  // = digits * digits_accuracy
  // Vector of values (4) for each algorithm, TOMS748, Newton, Halley & Schroder.
  //std::vector< boost::int_least64_t> times;  converted to int.
  std::vector<int> times; // arbitrary units (ticks).
  //boost::int_least64_t min_time = std::numeric_limits<boost::int_least64_t>::max(); // Used to normalize times (as int).
  std::vector<double> normed_times;
  int min_time = (std::numeric_limits<int>::max)(); // Used to normalize times.
  std::vector<uintmax_t> iterations;
  std::vector<long int> distances;
  std::vector<cpp_bin_float_100> full_results;
}; // struct root_info

std::vector<root_info> root_infos;  // One element for each floating-point type used.

inline std::string build_test_name(const char* type_name, const char* test_name)
{
  std::string result(BOOST_COMPILER);
  result += "|";
  result += BOOST_STDLIB;
  result += "|";
  result += BOOST_PLATFORM;
  result += "|";
  result += type_name;
  result += "|";
  result += test_name;
#if defined(_DEBUG) || !defined(NDEBUG)
  result += "|";
  result += " debug";
#else
  result += "|";
  result += " release";
#endif
  result += "|";
  return result;
} // std::string build_test_name

// Algorithms //////////////////////////////////////////////

// No derivatives - using TOMS748 internally.

template <int N, typename T = double>
struct nth_root_functor_noderiv
{ //  Nth root of x using only function - no derivatives.
  nth_root_functor_noderiv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor just stores value a to find root of.
  }
  T operator()(T const& x)
  {
    using boost::math::pow;
    T fx = pow<N>(x) -a; // Difference (estimate x^n - a).
    return fx;
  }
private:
  T a; // to be 'cube_rooted'.
}; // template <int N, class T> struct nth_root_functor_noderiv

template <int N, class T = double>
T nth_root_noderiv(T x)
{ // return Nth root of x using bracket_and_solve (using NO derivatives).
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For bracket_and_solve_root.

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  //T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  //T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  T factor = 2; // How big steps to take when searching.

  const boost::uintmax_t maxit = 50; // Limit to maximum iterations.
  boost::uintmax_t it = maxit; // Initially our chosen max iterations, but updated with actual.
  bool is_rising = true; // So if result if guess^3 is too low, then try increasing guess.
  // Some fraction of digits is used to control how accurate to try to make the result.
  int get_digits = std::numeric_limits<T>::digits - 2;
  eps_tolerance<T> tol(get_digits); // Set the tolerance.
  std::pair<T, T> r;
  r =  bracket_and_solve_root(nth_root_functor_noderiv<N, T>(x), guess, factor, is_rising, tol, it);
  iters = it;
  T result = r.first + (r.second - r.first) / 2;  // Midway between brackets.
  return result;
} // template <class T> T nth_root_noderiv(T x)

// Using 1st derivative only Newton-Raphson

template <int N, class T = double>
struct nth_root_functor_1deriv
{ // Functor also returning 1st derivative.
  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");

  nth_root_functor_1deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of, for example:
  }
  std::pair<T, T> operator()(T const& x)
  { // Return both f(x) and f'(x).
    using boost::math::pow; // // Compile-time integral power.
    T p = pow<N - 1>(x);
    return std::make_pair(p * x - a, N * p); // 'return' both fx and dx.
  }

private:
  T a; // to be 'nth_rooted'.
}; // struct nthroot__functor_1deriv

template <int N, class T = double>
T nth_root_1deriv(T x)
{ // return nth root of x using 1st derivative and Newton_Raphson.
  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For newton_raphson_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  int get_digits = static_cast<int>(digits * 0.6);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(nth_root_functor_1deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;
  return result;
} // T nth_root_1_deriv  Newton-Raphson

// Using 1st and 2nd derivatives with Halley algorithm.

template <int N, class T = double>
struct nth_root_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.
  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");

  nth_root_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { // Constructor stores value a to find root of, for example:
  }

  // using boost::math::tuple; // to return three values.
  std::tuple<T, T, T> operator()(T const& x)
  { // Return f(x), f'(x) and f''(x).
    using boost::math::pow; // Compile-time integral power.
    T p = pow<N - 2>(x);

    return std::make_tuple(p * x * x - a, p * x * N, p * N * (N - 1)); // 'return' fx, dx and d2x.
  }
private:
  T a; // to be 'nth_rooted'.
};

template <int N, class T = double>
T nth_root_2deriv(T x)
{ // return nth root of x using 1st and 2nd derivatives and Halley.

  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For halley_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy for type T.
  int get_digits = static_cast<int>(digits * 0.4);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = halley_iterate(nth_root_functor_2deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;

  return result;
} // nth_2deriv Halley

template <int N, class T = double>
T nth_root_2deriv_s(T x)
{ // return nth root of x using 1st and 2nd derivatives and Schroder.

  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For schroder_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type;

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent); // Get exponent of z (ignore mantissa).
  T guess = static_cast<T>(ldexp(static_cast<guess_type>(1.), exponent / N)); // Rough guess is to divide the exponent by n.
  T min = static_cast<T>(ldexp(static_cast<guess_type>(1.) / 2, exponent / N)); // Minimum possible value is half our guess.
  T max = static_cast<T>(ldexp(static_cast<guess_type>(2.), exponent / N)); // Maximum possible value is twice our guess.

  int get_digits = static_cast<int>(std::numeric_limits<T>::digits * 0.4);
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = schroder_iterate(nth_root_functor_2deriv<N, T>(x), guess, min, max, get_digits, it);
  iters = it;

  return result;
} // T nth_root_2deriv_s Schroder

//////////////////////////////////////////////////////// end of algorithms - perhaps in a separate .hpp?

//! Print 4 floating-point types info: max_digits10, digits and required accuracy digits as a Quickbook table.
int table_type_info(double digits_accuracy)
{
  std::string qbk_name = full_roots_name; // Prefix by boost_root file.

  qbk_name += "type_info_table";
  std::stringstream ss;
  ss.precision(3);
  ss << "_" << digits_accuracy * 100;
  qbk_name += ss.str();

#ifdef _MSC_VER
  qbk_name += "_msvc.qbk";
#else // assume GCC
  qbk_name += "_gcc.qbk";
#endif

  // Example: type_info_table_100_msvc.qbk
  fout.open(qbk_name, std::ios_base::out);

  if (fout.is_open())
  {
    std::cout << "Output type table to " << qbk_name << std::endl;
  }
  else
  { // Failed to open.
    std::cout << " Open file " << qbk_name << " for output failed!" << std::endl;
    std::cout << "errno " << errno << std::endl;
    return errno;
  }

  fout <<
    "[/"
    << qbk_name
    << "\n"
    "Copyright 2015 Paul A. Bristow.""\n"
    "Copyright 2015 John Maddock.""\n"
    "Distributed under the Boost Software License, Version 1.0.""\n"
    "(See accompanying file LICENSE_1_0.txt or copy at""\n"
    "http://www.boost.org/LICENSE_1_0.txt).""\n"
    "]""\n"
    << std::endl;

  fout << "[h6 Fraction of maximum possible bits of accuracy required is " << digits_accuracy << ".]\n" << std::endl;

  std::string table_id("type_info");
  table_id += ss.str(); // Fraction digits accuracy.

#ifdef _MSC_VER
  table_id += "_msvc";
#else // assume GCC
  table_id += "_gcc";
#endif

  fout << "[table:" << table_id << " Digits for float, double, long double and cpp_bin_float_50\n"
    << "[[type name] [max_digits10] [binary digits] [required digits]]\n";// header.

  // For all fout types:

  fout  << "[[" << "float" << "]"
    << "[" << std::numeric_limits<float>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<float>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<float>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "float" << "]"
    << "[" << std::numeric_limits<double>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<double>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<double>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "long double" << "]"
    << "[" << std::numeric_limits<long double>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<long double>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<long double>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "[[" << "cpp_bin_float_50" << "]"
    << "[" << std::numeric_limits<cpp_bin_float_50>::max_digits10 << "]"  // max_digits10
    << "[" << std::numeric_limits<cpp_bin_float_50>::digits << "]"// < "Binary digits
    << "[" << static_cast<int>(std::numeric_limits<cpp_bin_float_50>::digits * digits_accuracy) << "]]\n"; // Accuracy digits.

  fout << "] [/table table_id_msvc] \n" << std::endl; // End of table.

  fout.close();
  return 0;
} // type_table

//! Evaluate root N timing for each algorithm, and for one floating-point type T.
template <int N, typename T>
int test_root(cpp_bin_float_100 big_value, cpp_bin_float_100 answer, const char* type_name, std::size_t type_no)
{
  std::size_t max_digits = 2 + std::numeric_limits<T>::digits * 3010 / 10000;
  // For new versions use max_digits10
  // std::cout.precision(std::numeric_limits<T>::max_digits10);
  std::cout.precision(max_digits);
  std::cout << std::showpoint << std::endl; // Show trailing zeros too.

  root_infos.push_back(root_info());

  root_infos[type_no].max_digits10 = max_digits;
  root_infos[type_no].full_typename = typeid(T).name(); // Full typename.
  root_infos[type_no].short_typename = type_name; // Short typename.
  root_infos[type_no].bin_digits = std::numeric_limits<T>::digits;
  root_infos[type_no].get_digits = static_cast<int>(std::numeric_limits<T>::digits * digits_accuracy);

  T to_root = static_cast<T>(big_value);

  T result; // root
  T sum = 0;
  T ans = static_cast<T>(answer);

  using boost::timer::nanosecond_type;
  using boost::timer::cpu_times;
  using boost::timer::cpu_timer;

  int eval_count = boost::is_floating_point<T>::value ? 10000000 : 100000; // To give a sufficiently stable timing for the fast built-in types,
  //int eval_count = 1000000; // To give a sufficiently stable timing for the fast built-in types,
  // This takes an inconveniently long time for multiprecision cpp_bin_float_50 etc  types.

  cpu_times now; // Holds wall, user and system times.

  { // Evaluate times etc for each algorithm.
    //algorithm_names.push_back("TOMS748"); //
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_noderiv<N, T>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    // algorithm_names.push_back("Newton"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_1deriv<N, T>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }

    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    //algorithm_names.push_back("Halley"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_2deriv<N>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    ti.stop();
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  {
    // algorithm_names.push_back("Schroder"); // algorithm
    cpu_timer ti; // Can start, pause, resume and stop, and read elapsed.
    ti.start();
    for (long i = 0; i < eval_count; ++i)
    {
      result = nth_root_2deriv_s<N>(to_root); //
      sum += result;
    }
    now = ti.elapsed();
    int time = static_cast<int>(now.user / eval_count);
    root_infos[type_no].times.push_back(time); // CPU time taken.
    if (time < root_infos[type_no].min_time)
    {
      root_infos[type_no].min_time = time;
    }
    ti.stop();
    long int distance = static_cast<int>(boost::math::float_distance<T>(result, ans));
    root_infos[type_no].distances.push_back(distance);
    root_infos[type_no].iterations.push_back(iters); //
    root_infos[type_no].full_results.push_back(result);
  }
  for (size_t i = 0; i != root_infos[type_no].times.size(); i++) // For each time.
  { // Normalize times.
    root_infos[type_no].normed_times.push_back(static_cast<double>(root_infos[type_no].times[i]) / root_infos[type_no].min_time);
  }

  std::cout << "Accumulated result was: " << sum << std::endl;

  return 4;  // eval_count of how many algorithms used.
} // test_root

/*! Fill array of times, iterations, etc for Nth root for all 4 types,
 and write a table of results in Quickbook format.
 */
template <int N>
void table_root_info(cpp_bin_float_100 full_value)
{
   using std::abs;
  std::cout << nooftypes << " floating-point types tested:" << std::endl;
#if defined(_DEBUG) || !defined(NDEBUG)
  std::cout << "Compiled in debug mode." << std::endl;
#else
  std::cout << "Compiled in optimise mode." << std::endl;
#endif
  std::cout << "FP hardware " << fp_hardware << std::endl;
  // Compute the 'right' answer for root N at 100 decimal digits.
  cpp_bin_float_100 full_answer = nth_root_noderiv<N, cpp_bin_float_100>(full_value);

  root_infos.clear(); // Erase any previous data.
  // Fill the elements of the array for each floating-point type.

  test_root<N, float>(full_value, full_answer, "float", 0);
  test_root<N, double>(full_value, full_answer, "double", 1);
  test_root<N, long double>(full_value, full_answer, "long double", 2);
  test_root<N, cpp_bin_float_50>(full_value, full_answer, "cpp_bin_float_50", 3);

  // Use info from 4 floating point types to

  // Prepare Quickbook table for a single root
  // with columns of times, iterations, distances repeated for various floating-point types,
  // and 4 rows for each algorithm.

  std::stringstream table_info;
  table_info.precision(3);
  table_info << "[table:root_" << N << " " << N << "th root(" << static_cast<float>(full_value) << ") for float, double, long double and cpp_bin_float_50 types";
  if (fp_hardware != "")
  {
    table_info << ", using " << fp_hardware;
  }
  table_info << std::endl;

  fout << table_info.str()
    << "[[][float][][][] [][double][][][] [][long d][][][] [][cpp50][][]]\n"
    << "[[Algo     ]";
  for (size_t tp = 0; tp != nooftypes; tp++)
  { // For all types:
    fout << "[Its]" << "[Times]" << "[Norm]" << "[Dis]" << "[ ]";
  }
  fout << "]" << std::endl;

  // Row for all algorithms.
  for (std::size_t algo = 0; algo != noofalgos; algo++)
  {
    fout << "[[" << std::left << std::setw(9) << algo_names[algo] << "]";
    for (size_t tp = 0; tp != nooftypes; tp++)
    { // For all types:
      fout
        << "[" << std::right << std::showpoint
        << std::setw(3) << std::setprecision(2) << root_infos[tp].iterations[algo] << "]["
        << std::setw(5) << std::setprecision(5) << root_infos[tp].times[algo] << "][";
      fout << std::setw(3) << std::setprecision(3);
        double normed_time = root_infos[tp].normed_times[algo];
        if (abs(normed_time - 1.00) <= 0.05)
        { // At or near the best time, so show as blue.
          fout << "[role blue " << normed_time << "]";
        }
        else if (abs(normed_time) > 4.)
        { // markedly poor so show as red.
          fout << "[role red " << normed_time << "]";
        }
        else
        { // Not the best, so normal black.
          fout << normed_time;
        }
        fout << "]["
        << std::setw(3) << std::setprecision(2) << root_infos[tp].distances[algo] << "][ ]";
    } // tp
    fout << "]" << std::endl;
  } // for algo
  fout << "] [/end of table root]\n";
} // void table_root_info

/*! Output program header, table of type info, and tables for 4 algorithms and 4 floating-point types,
 for Nth root required digits_accuracy.
 */

int roots_tables(cpp_bin_float_100 full_value, double digits_accuracy)
{
  ::digits_accuracy = digits_accuracy;
  // Save globally so that it is available to root-finding algorithms. Ugly :-(

#if defined(_DEBUG) || !defined(NDEBUG)
  std::string debug_or_optimize("Compiled in debug mode.");
#else
     std::string debug_or_optimize("Compiled in optimise mode.");
#endif

  // Create filename for roots_table
  std::string qbk_name = full_roots_name;
  qbk_name += "roots_table";

  std::stringstream ss;
  ss.precision(3);
  // ss << "_" << N // now put all the tables in one .qbk file?
    ss << "_" << digits_accuracy * 100
    << std::flush;
  // Assume only save optimize mode runs, so don't add any  _DEBUG info.
  qbk_name += ss.str();

#ifdef _MSC_VER
  qbk_name += "_msvc";
#else // assume GCC
  qbk_name += "_gcc";
#endif
  if (fp_hardware != "")
  {
    qbk_name += fp_hardware;
  }
  qbk_name += ".qbk";

  fout.open(qbk_name, std::ios_base::out);

  if (fout.is_open())
  {
    std::cout << "Output root table to " << qbk_name << std::endl;
  }
  else
  { // Failed to open.
    std::cout << " Open file " << qbk_name << " for output failed!" << std::endl;
    std::cout << "errno " << errno << std::endl;
    return errno;
  }

  fout <<
    "[/"
    << qbk_name
    << "\n"
    "Copyright 2015 Paul A. Bristow.""\n"
    "Copyright 2015 John Maddock.""\n"
    "Distributed under the Boost Software License, Version 1.0.""\n"
    "(See accompanying file LICENSE_1_0.txt or copy at""\n"
    "http://www.boost.org/LICENSE_1_0.txt).""\n"
    "]""\n"
    << std::endl;

  // Print out the program/compiler/stdlib/platform names as a Quickbook comment:
  fout << "\n[h6 Program " << sourcefilename << ",\n "
    << BOOST_COMPILER << ", "
    << BOOST_STDLIB << ", "
    << BOOST_PLATFORM << "\n"
    << debug_or_optimize
    << ((fp_hardware != "") ? ", " + fp_hardware : "")
    << "]" // [h6 close].
    << std::endl;

  fout << "Fraction of full accuracy " << digits_accuracy << std::endl;

  table_root_info<5>(full_value);
  table_root_info<7>(full_value);
  table_root_info<11>(full_value);

  fout.close();

  //   table_type_info(digits_accuracy);

  return 0;
} // roots_tables

int main()
{
  using namespace boost::multiprecision;
  using namespace boost::math;

  try
  {
    std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << ", ";

// How to: Configure Visual C++ Projects to Target 64-Bit Platforms
// https://msdn.microsoft.com/en-us/library/9yb4317s.aspx

#ifdef _M_X64 // Defined for compilations that target x64 processors.
    std::cout << "X64 " << std::endl;
    fp_hardware += "_X64";
#else
#  ifdef _M_IX86
     std::cout << "X32 " << std::endl;
     fp_hardware += "_X86";
#  endif
#endif

#ifdef _M_AMD64
    std::cout << "AMD64 " << std::endl;
 //   fp_hardware += "_AMD64";
#endif

// https://msdn.microsoft.com/en-us/library/7t5yh4fd.aspx
// /arch (x86) options /arch:[IA32|SSE|SSE2|AVX|AVX2]
// default is to use SSE and SSE2 instructions by default.
// https://msdn.microsoft.com/en-us/library/jj620901.aspx
// /arch (x64) options /arch:AVX and /arch:AVX2

// MSVC doesn't bother to set these SSE macros!
// http://stackoverflow.com/questions/18563978/sse-sse2-is-enabled-control-in-visual-studio
// https://msdn.microsoft.com/en-us/library/b0084kay.aspx  predefined macros.

// But some of these macros are *not* defined by MSVC,
// unlike AVX (but *are* defined by GCC and Clang).
// So the macro code above does define them.
#if (defined(_M_AMD64) || defined (_M_X64))
#ifndef _M_X64
#  define _M_X64
#endif
#ifndef __SSE2__
#  define __SSE2__
#endif
#else
#  ifdef _M_IX86_FP // Expands to an integer literal value indicating which /arch compiler option was used:
    std::cout << "Floating-point _M_IX86_FP = " << _M_IX86_FP << std::endl;
#  if (_M_IX86_FP == 2) // 2 if /arch:SSE2, /arch:AVX or /arch:AVX2
#    define __SSE2__ // x32
#  elif (_M_IX86_FP == 1) // 1 if /arch:SSE was used.
#    define __SSE__ // x32
#  elif (_M_IX86_FP == 0) // 0 if /arch:IA32 was used.
#    define _X32 // No special FP instructions.
#  endif
# endif
#endif
// Set the fp_hardware that is used in the .qbk filename.
#ifdef __AVX2__
    std::cout << "Floating-point AVX2 " << std::endl;
    fp_hardware += "_AVX2";
#  else
#    ifdef __AVX__
    std::cout << "Floating-point AVX " << std::endl;
    fp_hardware += "_AVX";
#    else
#      ifdef __SSE2__
    std::cout << "Floating-point SSE2 " << std::endl;
    fp_hardware += "_SSE2";
#      else
#        ifdef __SSE__
    std::cout << "Floating-point SSE " << std::endl;
    fp_hardware += "_SSE";
#        endif
#      endif
#   endif
# endif

#ifdef _M_IX86
    std::cout << "Floating-point X86 _M_IX86 = " << _M_IX86 << std::endl;
    // https://msdn.microsoft.com/en-us/library/aa273918%28v=vs.60%29.aspx#_predir_table_1..3
    // 600 = Pentium Pro
#endif

#ifdef _MSC_FULL_VER
    std::cout << "Floating-point _MSC_FULL_VER " << _MSC_FULL_VER << std::endl;
#endif

#ifdef __MSVC_RUNTIME_CHECKS
    std::cout << "Runtime __MSVC_RUNTIME_CHECKS " << std::endl;
#endif

    BOOST_MATH_CONTROL_FP;

    cpp_bin_float_100 full_value("28.");
    // Compute full answer to more than precision of tests.
    //T value = 28.; // integer (exactly representable as floating-point)
    // whose cube root is *not* exactly representable.
    // Wolfram Alpha command N[28 ^ (1 / 3), 100] computes cube root to 100 decimal digits.
    // 3.036588971875662519420809578505669635581453977248111123242141654169177268411884961770250390838097895

    std::cout.precision(100);
    std::cout << "value " << full_value << std::endl;
   // std::cout << ",\n""answer = " << full_answer << std::endl;
    std::cout.precision(6);
   // cbrt cpp_bin_float_100 full_answer("3.036588971875662519420809578505669635581453977248111123242141654169177268411884961770250390838097895");

    // Output the table of types, maxdigits10 and digits and required digits for some accuracies.

    // Output tables for some roots at full accuracy.
    roots_tables(full_value, 1.);

    // Output tables for some roots at less accuracy.
    //roots_tables(full_value, 0.75);

    return boost::exit_success;
  }
  catch (std::exception const& ex)
  {
    std::cout << "exception thrown: " << ex.what() << std::endl;
    return boost::exit_failure;
  }
} // int main()

/*

*/


//#########################################################//
//#                                                       #//
//# gaussian_mixture_models  pca.cpp                      #//
//# Roberto Capobianco  <capobianco@dis.uniroma1.it>      #//
//#                                                       #//
//#########################################################//

#include <stdexcept>

#include <Eigen/SVD>
#include <iostream>

#include <particle_filter/pca.h>

namespace gmms {
Eigen::MatrixXd
PCA::pca(const Eigen::MatrixXd& dataset, double& retained_variance) {
    if (num_components_ > dataset.cols()) {
        throw std::runtime_error(
                "Number of components greater than dataset size");
    }

    Eigen::MatrixXd centered = dataset.rowwise() - dataset.colwise().mean();

    Eigen::RowVectorXd maxValues = centered.colwise().maxCoeff();
    Eigen::RowVectorXd minValues = centered.colwise().minCoeff();
    Eigen::MatrixXd normalized_dataset =
            centered.array().rowwise() / (maxValues - minValues).array();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            normalized_dataset, Eigen::ComputeThinV);

    Eigen::VectorXd S = svd.singularValues();
    Eigen::MatrixXd W = svd.matrixV().leftCols(num_components_);

    retained_variance = S.head(num_components_).sum() / S.sum();

    Eigen::MatrixXd projected = normalized_dataset * W;

    return projected;
}
}  // namespace gmms