#include <RcppArmadillo.h>
// [[Rcpp::depends("RcppArmadillo")]]
// [[Rcpp::depends("BH")]]

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/random.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/math/distributions.hpp>

#include <numeric>
#include <algorithm>
#include <map>
#include <string>
#include <iostream>

using namespace Rcpp;
using namespace arma;
using namespace std;
using namespace boost::multiprecision;
using namespace boost::math;

class LDA {
  public:
    int K; // K: number of topics
  int W; // W: size of Vocabulary
  int D; // D: number of documents
  vector< vector<int> >  w_num;
  vector< vector<int> >  z;
  vector<int> nd_sum;
  vector<int> nw_sum;
  NumericMatrix nd;
  NumericMatrix nw;
  NumericMatrix phi_avg;
  arma::mat PhiProdMat;
  arma::mat n_wd;
  vector< vector < vector<int> > > z_list;
  vector< NumericMatrix > phi_list;
  vector< NumericMatrix > theta_list;
  NumericMatrix theta_avg;
  CharacterVector Vocabulary; // vector storing all (unique) words of vocabulary
  double alpha; // hyper-parameter for Dirichlet prior on theta
  double beta; //  hyper-parameter for Dirichlet prior on phi
  double sigma; // for Langevin sampler
  boost::mt19937 rng; // seed for random sampling
  List a;

  LDA(Reference Obj);

  void collapsedGibbs(int iter, int burnin, int thin);
  void NichollsMH(int iter, int burnin, int thin);
  void LangevinMHSampling(int iter, int burnin, int thin);

  NumericVector DocTopics(int d, int k);
  NumericMatrix Topics(int k);
  CharacterVector TopicTerms(int k, int no);
  CharacterMatrix Terms(int k);
  arma::rowvec rDirichlet(arma::rowvec param, int length);
  arma::rowvec rDirichlet2(arma::rowvec param, int length);
  arma::mat DrawFromProposal(arma::mat phi_current);
  arma::mat InitPhiMat();
  List getPhiList();
  List getZList();

  double PhiDensity2(NumericMatrix phi);
  double PhiDensity(arma::mat phi);

  arma::mat getPhiGradient(arma::mat phi);

  double rgamma_cpp(double alpha);
  double rbeta_cpp(double shape1, double shape2);
  double rnorm_cpp(double mean, double sd);

  double LogPhiProd(arma::mat phi);
  vector<double> LogPhiProd_vec(arma::mat phi);
  arma::mat DrawLangevinProposal(arma::mat phi_current);
  double EvalLangevinProposal(arma::mat PhiFrom, arma::mat PhiTo);
  arma::mat ProjectProposalToSimplex(arma::mat PhiProposal);

  private:
    vector< vector<int> > CreateIntMatrix(List input);

  NumericMatrix get_phis();
  NumericMatrix get_thetas();
  NumericMatrix MatrixToR(NumericMatrix input);
  NumericMatrix avgMatrix(NumericMatrix A, NumericMatrix B, int weight);
  NumericMatrix getTDM(int W, int D, List w_num);

  double ProposalDensity(arma::mat phi);
  double ArrayMax(double array[], int numElements);
  double ArrayMin(double array[], int numElements);

};

LDA::LDA(Reference Obj)
{

  K = as<int>(Obj.field("K"));
  W = as<int>(Obj.field("W"));
  D = as<int>(Obj.field("D"));

  nw = as<NumericMatrix> (Obj.field("nw"));
  nd = as<NumericMatrix> (Obj.field("nd"));

  alpha = Obj.field("alpha");
  beta = Obj.field("beta");
  sigma = 0.00001;

  Vocabulary = Obj.field("Vocabulary");

  List temp_w = Obj.field("w_num");
  List temp_z = Obj.field("z");
  w_num = CreateIntMatrix(temp_w);

  NumericMatrix tdm = getTDM(W, D, temp_w);
  int i = tdm.nrow(), j = tdm.ncol();
  arma::mat n_wd_pointer(tdm.begin(), i, j, false);
  n_wd = n_wd_pointer;

  z = CreateIntMatrix(temp_z);

  nd_sum = as<vector<int> > (Obj.field("nd_sum"));
  nw_sum = as<vector<int> > (Obj.field("nw_sum"));

  a = List::create(Named("z")=z);
};

List LDA::getPhiList()
{
  int iter = phi_list.size();
  List ret(iter);

  for (int i = 0; i<iter; i++)
  {
    ret[i] = phi_list[i];
  }
  return ret;
}

List LDA::getZList()
{
  int length = z_list.size();
  List ret(length);

  for (int i = 0; i<length; i++)
  {
    ret[i] = wrap(z_list[i]);
  }
  return ret;
}

vector< vector<int> > LDA::CreateIntMatrix(List input)
{

  int inputLength = input.size();
  vector< vector<int> > output;

  for(int i=0; i<inputLength; i++) {
    vector<int> test = as<vector<int> > (input[i]);
    output.push_back(test);
  }

  return output;

}

NumericVector LDA::DocTopics(int d, int k)
{
  vector<double> d_theta(K);
  NumericVector d_theta_R = theta_avg(d,_);
  d_theta = as<vector<double> > (d_theta_R);
  NumericVector ret_vector(k);

  for (int i=0;i<k;i++)
  {
    std::vector<double>::iterator result;
    result = std::max_element(d_theta.begin(),d_theta.end());
    int biggest_id = std::distance(d_theta.begin(), result);
    ret_vector[i] = biggest_id;
    d_theta[biggest_id] = 0;
  }

  return ret_vector;
}

NumericMatrix LDA::Topics(int k)
{
  NumericMatrix ret(D,k);
  for (int i = 0; i<D; i++)
  {
    NumericVector temp = DocTopics(i,k);
    ret(i,_) = temp;
  }
  ret = MatrixToR(ret);
  return ret;
}

CharacterVector LDA::TopicTerms(int k, int no)
{
  vector<double> k_phi(W);
  NumericVector k_phi_R = phi_avg(k,_);
  k_phi = as<vector<double> > (k_phi_R);
  NumericVector ret_vector(no);

  for (int i=0;i<no;i++)
  {
    std::vector<double>::iterator result;
    result = std::max_element(k_phi.begin(),k_phi.end());
    int biggest_id = std::distance(k_phi.begin(), result);
    ret_vector[i] = biggest_id;
    k_phi[biggest_id] = 0;
  }

  CharacterVector ret_char_vector(no);

  for (int i=0;i<no;i++)
  {
    ret_char_vector[i] = Vocabulary[ret_vector[i]];
  }

  return ret_char_vector;

}

CharacterMatrix LDA::Terms(int k)
{
  CharacterMatrix ret(K,k);
  for (int i = 0; i < K; i++)
  {
    CharacterVector temp = TopicTerms(i,k);
    ret(i,_) =  temp;
  }
  return ret;
}

NumericMatrix LDA::MatrixToR(NumericMatrix input)
{
  int n = input.nrow(), k = input.ncol();
  NumericMatrix output(n,k);
  for (int i = 0; i<n; i++)
  {
    for (int j = 0; j<k; j++)
    {
      output(i,j) = input(i,j) + 1;
    }
  }
  return output;
}

void LDA::collapsedGibbs(int iter, int burnin, int thin)
{

  double Kd = (double) K;
  double Wd = (double) W;
  double W_Beta  = Wd * beta;
  double K_Alpha = Kd * alpha;

  for (int i = 0; i < iter; ++i)
  {
    for (int d = 0; d < D; ++d)
    {
      for (int w = 0; w < nd_sum[d]; ++w)
      {
        int word = w_num[d][w] - 1;
        int topic = z[d][w] - 1;

        nw(word,topic) -= 1;
        nd(d,topic) -= 1;
        nw_sum[topic] -= 1;
        nd_sum[d] -=  1;

        vector<double>  prob(K);

        for(int j=0; j<K; j++)
        {
          double nw_ij = nw(word,j);
          double nd_dj = nd(d,j);
          prob[j] = (nw_ij + beta) / (nw_sum[j] + W_Beta) *
            (nd_dj + alpha) / (nd_sum[d] + K_Alpha);
        }

        for (int r = 1; r < K; ++r)
        {
          prob[r] = prob[r] + prob[r - 1];
        }

        double u  = prob[K-1] * rand() / double(RAND_MAX);

        int new_topic = 0; // set up new topic

        for (int nt = 0 ; nt < K; ++nt)
        {
          if (prob[nt] > u)
          {
            new_topic = nt;
            break;
          }
        }

        //  assign new z_i to counts
        nw(word,new_topic) +=  1;
        nd(d,new_topic) += 1;
        nw_sum[new_topic] += 1;
        nd_sum[d] += 1;

        z[d][w] = new_topic + 1;

      }

    }

    if (i % thin == 0 && i > burnin)
    {
      z_list.push_back(z);

      NumericMatrix current_phi = get_phis();
      NumericMatrix current_theta = get_thetas();
      phi_list.push_back(current_phi);
      theta_list.push_back(current_theta);

      if(phi_list.size()==1) phi_avg = current_phi;
      else phi_avg =  avgMatrix(phi_avg, current_phi, phi_list.size());

      if(theta_list.size()==1) theta_avg = current_theta;
      else theta_avg =  avgMatrix(theta_avg, current_theta, theta_list.size());

    }

  }
}

NumericMatrix LDA::avgMatrix(NumericMatrix A, NumericMatrix B, int weight)
{
  int nrow = A.nrow();
  int ncol = A.ncol();
  NumericMatrix C(nrow,ncol);

  float wf = (float) weight;
  float propA = (wf-1) / wf;
  float propB = 1 / wf;

  for (int i=0; i<nrow;i++)
  {
    for (int j=0; j<ncol;j++)
    {
      C(i,j) =  propA * A(i,j) + propB * B(i,j);
    }
  }

  return C;
}

NumericMatrix LDA::get_phis()
{

  NumericMatrix phi(K,W);

  for (int k = 0; k < K; k++) {
    for (int w = 0; w < W; w++) {
      phi(k,w) = (nw(w,k) + beta) / (nw_sum[k] + W * beta);
    }
  }

  return phi;
}

NumericMatrix LDA::get_thetas()
{

  NumericMatrix theta(D,K);

  for (int d = 0; d<D; d++) {
    for (int k = 0; k<K; k++) {
      theta(d,k) = (nd(d,k) + alpha) / (nd_sum[d] + K * alpha);
    }
  }
  return theta;
}

NumericMatrix LDA::getTDM(int W, int D, List w_num) {

  NumericMatrix tdm(W,D);
  for (int d=0; d<D; ++d)
    for (int w=0; w<W; ++w)
    {
      int freq = 0;
      vector<int> current_w = as<vector<int> > (w_num[d]);
      int wlen = current_w.size();
      for (int l=0; l<wlen; ++l)
      {
        if(current_w[l] == w + 1) freq += 1;
      }

      tdm(w,d) = freq;
    }
  return tdm;
}

// using R: (unfortunately too slow to call R and convert objects back)
//NumericMatrix LDA::DrawFromProposal()
//  {
  //    Environment MCMCpack("package:MCMCpack");
  //    Function rdirichlet = MCMCpack["rdirichlet"];
  //    return rdirichlet(K,rep(beta,W));
  //  }

double LDA::rbeta_cpp(double shape1, double shape2)
{
  double u  = rand() / double(RAND_MAX);
  beta_distribution<> beta_dist(shape1, shape2);
  return quantile(beta_dist, u);
}

double LDA::rgamma_cpp(double alpha)
{
  boost::gamma_distribution<> dgamma(alpha);
  boost::variate_generator<boost::mt19937&,boost::gamma_distribution<> > ret_gamma( rng, dgamma);
  return ret_gamma();
}

double LDA::rnorm_cpp(double mean, double sd)
{
  boost::normal_distribution<> nd(mean, sd);
  boost::variate_generator<boost::mt19937&,
  boost::normal_distribution<> > ret_norm(rng, nd);
  return ret_norm();
}

arma::rowvec LDA::rDirichlet(arma::rowvec param, int length)
{
  rowvec ret(length);
  for (int l = 0; l<length; l++)
  {
    double beta = param[l];
    ret[l] = rgamma_cpp(beta);
  }
  ret = ret / sum(ret);
  return ret;
}

arma::rowvec LDA::rDirichlet2(arma::rowvec param, int length)
{
  vector<double> ret;
  param *= 10000;
  vector<double> param_vec = conv_to<vector<double> >::from(param);
  Rcout << param_vec[0] << "-";
  int len = length - 1;

  double paramSum = std::accumulate(param_vec.begin()+1,param_vec.end(),(double)0);
  Rcout << paramSum;
  ret.push_back(rbeta_cpp(param_vec[0], paramSum));
  for (int i=1; i<len;i++)
  {
    double paramSum = std::accumulate(param_vec.begin()+i+1,param_vec.end(),(double)0);
    double phi = rbeta_cpp(param_vec[i], paramSum);
    double sumRet = std::accumulate(ret.begin(),ret.end(),(double)0);
    ret.push_back((1-sumRet) * phi);
  }
  double sumRet = std::accumulate(ret.begin(),ret.end(),(double)0);
  ret.push_back(1-sumRet);
  return ret;
}

mat LDA::DrawFromProposal(arma::mat phi_current)
{
  arma::mat phi_sampled(K,W);
  for (int k=0;k<K;k++)
  {
    arma::rowvec phi_current_row = phi_current.row(k);
    arma::rowvec new_row = rDirichlet2(phi_current_row, W);
    // Rcout << new_row;
    phi_sampled.row(k) = new_row;
  }
  return phi_sampled;
}

mat LDA::InitPhiMat()
{
  arma::mat phi(K,W);

  for (int k=0; k<K; k++)
  {
    for (int w=0; w<W; w++)
    {
      phi(k,w) = beta / (W*beta);
    }
  }
  return phi;
}

void LDA::NichollsMH(int iter, int burnin, int thin)
{

  arma::mat phi_current = InitPhiMat();

  for (int t=1;t<iter;t++)
  {

    // Metropolis-Hastings Algorithm:
      // 1. draw from proposal density:
      arma::mat hyperParams = beta + 0.1 * (phi_current - beta);
    arma::mat phi_new = DrawFromProposal(hyperParams);

    // 2. Calculate acceptance probability
    double pi_new = PhiDensity(phi_new);
    double pi_old = PhiDensity(phi_current);
    double q_new = ProposalDensity(phi_new);
    double q_old = ProposalDensity(phi_current);

    double acceptanceMH = exp(pi_new + q_old - pi_old - q_new);
    double alphaMH = min((double)1,acceptanceMH);
    Rcout << "Acceptance Prob:" << alphaMH;

    // draw U[0,1] random variable
    double u  = rand() / double(RAND_MAX);
    if (u<=alphaMH) phi_current = phi_new;
    else phi_current = phi_current;

    if (t % thin == 0 && t > burnin) {
      NumericMatrix phi_add = wrap(phi_current);
      phi_list.push_back(phi_add);
      if(phi_list.size()==1) phi_avg = phi_add;
      else phi_avg =  avgMatrix(phi_avg, phi_add, phi_list.size());
    };

    // Rcout << pi_new;
    // Rcout << pi_old;
    // Rcout << q_new;
    // Rcout << q_old;

  }

}

double LDA::LogPhiProd(arma::mat phi)
{
  arma::mat logPhi = log(phi);
  double sumLik_vec[K];
  double logPhiProd = 0;

  for (int d=0; d<D; d++)
  {
    double sumLik = 0;
    arma::colvec nd = n_wd.col(d);

    for (int k=0; k<K; k++)
    {
      arma::rowvec logPhi_k = logPhi.row(k);
      sumLik_vec[k] = dot(logPhi_k,nd);
    }
    double b = ArrayMax(sumLik_vec,K);

    for (int k=0; k<K; k++)
    {
      sumLik += exp(sumLik_vec[k]-b);
    }

    logPhiProd += b + log(sumLik);
  }

  return logPhiProd;
}

vector<double> LDA::LogPhiProd_vec(arma::mat phi)
{
  vector<double> ret_vec;
  arma::mat logPhi = log(phi);
  arma::mat PhiProdMat_Pointer(K,D);
  double sumLik_vec[K];

  for (int d=0; d<D; d++)
  {
    double sumLik = 0;
    arma::colvec nd = n_wd.col(d);

    for (int k=0; k<K; k++)
    {
      arma::rowvec logPhi_k = logPhi.row(k);
      sumLik_vec[k] = dot(logPhi_k,nd);
      PhiProdMat_Pointer(k,d) = sumLik_vec[k];
    }
    double b = ArrayMax(sumLik_vec,K);

    for (int k=0; k<K; k++)
    {
      sumLik += exp(sumLik_vec[k]-b);
    }

    double ret_vec_d = b + log(sumLik);
    ret_vec.push_back(ret_vec_d);
  }
  PhiProdMat = PhiProdMat_Pointer;
  return ret_vec;
}

// function adapted from Yunmei Chen and Xiaojing Ye (2011)

// [[Rcpp::export]]
vector<double> ProjectOntoSimplex (vector<double> y)
{
  int m = y.size();
  bool bget = false;

  vector<double> s = y;
  std::sort(s.rbegin(), s.rend());

  double tmpsum = 0;
  double tmax = 0;

  for (int i = 0; i<m-1; i++)
  {
    tmpsum = tmpsum + s[i];
    tmax = (tmpsum - 1)/(i+1);
    if (tmax >= s[i+1])
    {
      bget = true;
      break;
    }
  }

  if (bget==false)
  {
    tmax = (tmpsum + s[m-1] - 1)/m;
  }

  vector<double> x;
  for (int j = 0; j<m;j++)
  {
    double elem1 = y[j] - tmax;
    double ret = max(elem1,0.0);
    x.push_back(ret);
  }

  return x;
}

arma::mat LDA::getPhiGradient(arma::mat phi)
{
  arma::mat phi2 = phi;
  arma::mat logPhi = log(phi);
  vector<double> denom_vec = LogPhiProd_vec(phi2);
  arma::mat gradient(K,W);

  for (int z=0;z<K;z++)
  {
    for(int w=0;w<W;w++)
    {

      double dSum = 0;

      for (int d = 0; d<D;d++)
      {
        double nwd = n_wd(w,d);
        if (nwd==0) dSum += 0;
        else
        {
          // Rcout << "nwd:" << nwd;
          arma::colvec nd = n_wd.col(d);
          arma::rowvec logPhi_k = logPhi.row(z);

          double dotProd = PhiProdMat(z,d);
          // Rcout << "dotProd:" << dotProd;
          // Rcout << "Dot Product: " << dotProd;

          double Numerator = log(nwd) + (nwd - 1)*logPhi(z,w) + dotProd - nd[w]*logPhi_k[w];
          // Rcout << Numerator;
          double Denominator = denom_vec[d];
          // Rcout << Denominator;
          dSum += exp(Numerator - Denominator);
        }
      }
      // Rcout << "dSum:" << dSum;
      gradient(z,w) = dSum + (beta - 1) / phi2(z,w);
      //Rcout << gradient(z,w);
    }
  }

  return gradient;
}

arma::mat LDA::DrawLangevinProposal(arma::mat phi_current)
{
  arma::mat PhiProposal(K,W);
  arma::mat PhiGradient = getPhiGradient(phi_current);
  for (int z=0; z<K; z++)
  {
    vector<double> prop_vec;
    for (int w=0; w<W; w++)
    {
      double error = rnorm_cpp(0,sigma);
      double sigma_squared = pow(sigma,2);
      PhiProposal(z,w) = phi_current(z,w) + 0.5 * sigma_squared * PhiGradient(z,w) + error;
    }
  }
  return PhiProposal;
}

double LDA::EvalLangevinProposal(arma::mat PhiFrom, arma::mat PhiTo)
{
  double sigma_squared = pow(sigma,2);
  double logDensity = 0;
  arma::mat PhiGradient = getPhiGradient(PhiFrom);
  for (int z=0; z<K; z++)
  {
    for (int w=0;w<W;w++)
    {
      double gradient_zw = PhiGradient(z,w);
      double mean = PhiFrom(z,w) + 0.5 * sigma_squared * gradient_zw;
      double PhiMeanDiff = PhiTo(z,w) - mean;
      logDensity -= (1/(2*sigma_squared))*pow(PhiMeanDiff,2);
    }
  }
  return logDensity;
}

arma::mat LDA::ProjectProposalToSimplex(arma::mat PhiProposal)
{
  for (int z=0; z<K; z++)
  {
    vector<double> prop_vec = conv_to<vector<double> >::from(PhiProposal.row(z));
    vector<double> Phi_proj_vec = ProjectOntoSimplex(prop_vec);
    PhiProposal.row(z) = conv_to<rowvec>::from(Phi_proj_vec);
  }
  return PhiProposal;
}

void LDA::LangevinMHSampling(int iter, int burnin, int thin)
{
  arma::mat phi_current = InitPhiMat();
  arma::mat phi_current_projected = ProjectProposalToSimplex(phi_current);

  for (int t=1;t<iter;t++)
  {

    // Metropolis Algorithm:
      // 1. draw from Langevin proposal density:
      arma::mat phi_new = DrawLangevinProposal(phi_current_projected);
    arma::mat phi_new_projected = ProjectProposalToSimplex(phi_new);

    // 2. Calculate acceptance probability
    double pi_new = PhiDensity(phi_new_projected);
    double pi_old = PhiDensity(phi_current_projected);
    double q_num = EvalLangevinProposal(phi_new_projected,phi_current);
    double q_denom = EvalLangevinProposal(phi_current_projected,phi_new);

    // Rcout << "Pi_new:" << pi_new;
    // Rcout << "Pi_old:" << pi_old;
    // Rcout << "Q_numerator:" << q_num;
    // Rcout << "Q_denominator:" << q_denom;

    double acceptanceMH = exp(pi_new + q_num - pi_old - q_denom);
    double alphaMH = min((double)1,acceptanceMH);
    // Rcout << "Acceptance Prob:" << alphaMH;

    // draw U[0,1] random variable
    double u  = rand() / double(RAND_MAX);
    if (u<=alphaMH)
    {
      phi_current = phi_new;
      phi_current_projected = phi_new_projected;
    }
    else
    {
      phi_current = phi_current;
      phi_current_projected = phi_current_projected;
    }

    if (t % thin == 0 && t > burnin) {
      NumericMatrix phi_add = wrap(phi_current_projected);
      phi_list.push_back(phi_add);
      if(phi_list.size()==1) phi_avg = phi_add;
      else phi_avg =  avgMatrix(phi_avg, phi_add, phi_list.size());
    };

  }

}

double LDA::ProposalDensity(arma::mat phi)
{
  double logBetaFun = 0;
  double betaSum = 0;
  for (int k=0; k<K;k++)
  {
    for (int w=0;w<W;w++)
    {
      double phi_scalar = phi(k,w);
      logBetaFun += lgamma(phi_scalar);
      betaSum  += phi_scalar;
    }

  }
  // double logBetaFun = K*(W*lgamma(beta)-lgamma(W*beta));
  logBetaFun -= lgamma(betaSum);

  arma::mat logPhi = log(phi);
  arma::mat temp = logPhi * (beta-1);
  double logPhiSum = accu(temp);

  double logDensity = logPhiSum - logBetaFun;
  return logDensity;
}

double LDA::PhiDensity(arma::mat phi)
{
  arma::mat logPhi = log(phi);
  double sumLik_vec[K];
  double logLikelihood = 0;

  for (int d=0; d<D; d++)
  {
    double sumLik = 0;
    arma::colvec nd = n_wd.col(d);

    for (int k=0; k<K; k++)
    {
      arma::rowvec logPhi_k = logPhi.row(k);
      sumLik_vec[k] = dot(logPhi_k,nd);
    }
    double b = ArrayMax(sumLik_vec,K);

    for (int k=0; k<K; k++)
    {
      sumLik += exp(sumLik_vec[k]-b);
    }

    logLikelihood +=  b + log(sumLik);
  }

  logLikelihood += D * log(alpha);
  logLikelihood -= D * log(K*alpha);
  // Rcout << "logLikelihood: " << logLikelihood;

  double logBetaFun = K*(lgamma(W*beta)-W*lgamma(beta));
  // Rcout << "logBetaFun: " << logBetaFun;

  double logPhiSum = 0;

  arma::mat temp = logPhi * (beta-1);
  logPhiSum = accu(temp);

  // Rcout << "LogPhiSum: " << logPhiSum;

  double logProb = logLikelihood + logBetaFun + logPhiSum;
  return logProb;
}

double LDA::PhiDensity2(NumericMatrix phi)
{
  arma::mat phi2 = as<arma::mat>(phi);
  arma::mat logPhi = log(phi2);
  double logLikelihood_vec[D];
  double logLikelihood = 0;

  for (int d=0; d<D; d++)
  {
    double sumLik = 0;
    arma::colvec nd = n_wd.col(d);

    for (int k=0; k<K; k++)
    {
      arma::rowvec logPhi_k = logPhi.row(k);
      double inProd_k = 0;

      for (int w=0; w<W; w++)
      {
        inProd_k += logPhi_k[w] * nd[w];
      }
      // Rcout << inProd_k;
      double sumLik_k = exp(inProd_k) * alpha;
      sumLik += sumLik_k;
    }
    logLikelihood_vec[d] = log(sumLik);
    logLikelihood += logLikelihood_vec[d];
  }

  logLikelihood -= D * log(K*alpha);
  //Rcout << "logLikelihood: " << logLikelihood;

  double logBetaFun = K*(lgamma(W*beta)-W*lgamma(beta));
  //Rcout << "logBetaFun: " << logBetaFun;

  double logPhiSum = 0;

  arma::mat temp = logPhi * (beta-1);
  logPhiSum = accu(temp);

  //Rcout << "LogPhiSum: " << logPhiSum;

  double logProb = logLikelihood + logBetaFun + logPhiSum;
  double Prob = exp(logProb);
  return Prob;
}

double LDA::ArrayMax(double array[], int numElements)
{
  double max = array[0];       // start with max = first element

  for(int i = 1; i<numElements; i++)
  {
    if(array[i] > max)
      max = array[i];
  }
  return max;                // return highest value in array
}

double LDA::ArrayMin(double array[], int numElements)
{
  double min = array[0];       // start with min = first element

  for(int i = 1; i<numElements; i++)
  {
    if(array[i] < min)
      min = array[i];
  }
  return min;                // return smallest value in array
}

// [[Rcpp::export]]
RCPP_MODULE(LDA_module) {
  class_<LDA>( "LDA" )
  .constructor<Reference>()
  //.field( "w_num", &LDA::w_num)
  .field( "a", &LDA::a)
  .field( "nd_sum", &LDA::nd_sum)
  .field("nd",&LDA::nd)
  .field( "nw_sum", &LDA::nw_sum)
  .field("nw",&LDA::nw)
  .field("K", &LDA::K)
  .field("D",&LDA::D)
  .field("phi_avg",&LDA::phi_avg)
  .field("theta_avg",&LDA::theta_avg)
  .method("collapsedGibbs",&LDA::collapsedGibbs)
  .method("Topics",&LDA::Topics)
  .method("Terms",&LDA::Terms)
  .method("NichollsMH",&LDA::NichollsMH)
  .method("DrawFromProposal",&LDA::DrawFromProposal)
  .method("getPhiList",&LDA::getPhiList)
  .method("getZList",&LDA::getZList)
  .method("getPhiGradient",&LDA::getPhiGradient)
  .method("rgamma_cpp",&LDA::rgamma_cpp)
  .method("rbeta_cpp",&LDA::rbeta_cpp)
  .method("InitPhiMat",&LDA::InitPhiMat)
  .method("rDirichlet2",&LDA::rDirichlet2)
  .method("LogPhiProd_vec",&LDA::LogPhiProd_vec)
  .method("DrawLangevinProposal",&LDA::DrawLangevinProposal)
  .method("LangevinMHSampling",&LDA::LangevinMHSampling)
  .method("PhiDensity",&LDA::PhiDensity)
  .method("ProjectProposalToSimplex",&LDA::ProjectProposalToSimplex)
  ;
}


#include <iostream>
#include <math.h>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

using namespace std;
using namespace boost::python;

double rand_norm(double mean, double stdev){
  boost::mt19937 rng(rand());

  boost::normal_distribution<> nd(mean, stdev);

  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);

  double d = var_nor();

  return d;
}

double rand_norm_range(double mean, double stdev, double min, double max){
  int max_tries = 1000;
  int tries = 0;

  while (1){
    tries++;

    double x = rand_norm(mean, stdev);
    if ((x >= min) & (x <= max)){
      return x;
    }
    else if (tries >= max_tries){
      cout << "ERROR: exceeded max tries (n="
	   << max_tries
	   << ") to find a random variable" << endl;
      exit(1);
    }
  }
}

double calc_diffusion_simple(double frag_gc, double frag_len){
  /*
    Simple calculation of diffusion based on fragment length
  */
  double diff_coef = 44500;
  double stdev = sqrt(diff_coef / frag_len);
  return frag_gc + rand_norm(0, stdev);
}

double calc_diffusion_GC(double frag_GC, double frag_len,
		         double T, double B, double G, int M){
  /*
    Calculating diffusion in standard deviation of %G+C equivalents.
    Adding diffusion G+C (drawn from normal distribution with s.d.=calculated s.d.)
    to input G+C value.
    Args:
    frag_GC = G+C content of DNA fragment
    frag_len = DNA fragment length (bp)
    T = absolute temperature
    B = beta
    G = G coefficient (see Clay et al., 2003)
    M = molecular weight per base pair of dry cesium DNA
   */

  double frag_BD = frag_GC / 100 * 0.098 + 1.66;
  double R = 8.3145e7;
  double GC_sd = sqrt(pow(100 / 0.098, 2) * ((frag_BD*R*T)/(pow(B,2)*G*M*frag_len)));

  return rand_norm(0, GC_sd);
}

double calc_diffusion_BD(double frag_BD, double frag_len,
		         double T, double B, double G, int M){
  /*
    Calculating diffusion in standard deviation of buoyant_density equivalents (rho).
    Args:
    frag_BD = rho (buoyant density)
    frag_len = fragment length (bp)
    T = absolute temperature
    B = beta
    G = G coefficient (see Clay et al., 2003)
    M = molecular weight per base pair of dry cesium DNA
    Return:
    BD error due to diffusion value drawn from a normal distribution with a
    standard deviation determined by calculated diffusion
   */

  double R = 8.3145e7;
  double sd_BD = sqrt((frag_BD*R*T)/(pow(B,2)*G*M*frag_len));

  return rand_norm(0, sd_BD);
}

double GC2BD(double GC){
  /*
    Calaculate buoyant density from G+C.
    Args:
    GC = % GC of DNA fragment
  */
  return GC / 100.0 * 0.098 + 1.66;
}

double addIncorpBD(double frag_BD, double incorp_perc, double isoMaxBD){
  return incorp_perc / 100 * isoMaxBD + frag_BD;
}

BOOST_PYTHON_MODULE(SIPSimCpp)
{
  def("rand_norm", rand_norm);
  def("rand_norm_range", rand_norm_range);
  def("calc_diffusion_GC", calc_diffusion_GC);
  def("calc_diffusion_BD", calc_diffusion_BD);
  def("GC2BD", GC2BD);
  def("addIncorpBD", addIncorpBD);
}


// ConsoleApplication1.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
//映入Cholesky
#include <Eigen/Cholesky>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Sparse>
#include <Eigen/Dense>
using namespace Eigen;     // 改成这样亦可 using Eigen::MatrixXd;
using namespace std;

#define  MATRIX_SIZE 4
/*
int main(int argc, char** argv)
{
    // 解方程
    // 我们求解 A * x = b 这个方程
    // 直接求逆自然是最直接的，但是求逆运算量大

    //Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > A1;
    //A1 = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    SparseMatrix<double> A1(MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > b1;
    b1 = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

    clock_t time_stt = clock(); // 计时
    //Cholesky 解方程

    // 直接求逆
    Eigen::Matrix<double, MATRIX_SIZE, 1> x = A1.inverse() * b1;
    cout << "time use in normal inverse is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;
    // QR分解colPivHouseholderQr()
    //time_stt = clock();
    x = A1.colPivHouseholderQr().solve(b1);
    cout << "time use in Qr decomposition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;
    //QR分解fullPivHouseholderQr()
    //time_stt = clock();
    //x = A1.fullPivHouseholderQr().solve(b1);
    //cout << "time use in Qr decomposition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;
    //llt分解 要求矩阵A正定
    time_stt = clock();
    x = A1.llt().solve(b1);
    cout <<"time use in llt decomposition is " <<1000*(clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
    cout <<x<<endl;
    //ldlt分解  要求矩阵A正或负半定
    time_stt = clock();
    x = A1.ldlt().solve(b1);
    cout <<"time use in ldlt decomposition is " <<1000*(clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
    cout <<x<<endl;
    //lu分解 partialPivLu()
    time_stt = clock();
    x = A1.partialPivLu().solve(b1);
    cout << "time use in lu decomposition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;
    //lu分解（fullPivLu()
    // = clock();
    //x = A1.fullPivLu().solve(b1);
    //cout << "time use in lu decomposition is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;

    //x = A1.bdcSvd(ComputeThinU | ComputeThinV).solve(b1);
    cout << "time use in svd is " << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
    //cout << x << endl;
    return 0;

}
*/
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧:
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件


#include <iostream>
#include <cmath>
#include <vector>
#include <array>
#include <math.h>
#include "AdaptiveHeat.hpp"
#include "StiffnessMatrix.hpp"
#include <fstream>
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;

const double M_PI = 2*acos(0);

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

SpaceMesh smesh;
smesh.GenerateSpaceMesh({0, 0.15,  0.25, 0.5, 1.0});
//smesh.GloballyBisectSpaceMesh();

smesh.PrintSpaceNodes();
std::cout<<smesh.meshsize() <<"\n";
std::cout<<"\n";

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 2), 1.0);

AdaptiveHeatEquation adaptiveheat;
adaptiveheat.SetSpaceTimeMesh( smesh, tmesh, "soultion1.txt");
adaptiveheat.AdaptiveSolver();

//adaptiveheat.Solve();

//adaptiveheat.PrintSolution();

//adaptiveheat.PrintErrorMesh();
adaptiveheat.PrintSolution();

}

Sover class with method to adapt in space and time/driver.cpp


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char, Dynamic> ImgLine;
typedef Matrix<ImgLine, Dynamic, Dynamic> FlattenedImages;

unsigned char max(u_char a, u_char b)
{
    if (a < b)
        return b;
    else
        return a;
}

unsigned char min(u_char a, u_char b)
{
    if (a > b)
        return b;
    else
        return a;
}

double max(double a, double b)
{
    if (a < b)
        return b;
    else
        return a;
}

double min(double a, double b)
{
    if (a > b)
        return b;
    else
        return a;
}

vector<double> projectOnEigenSpace(EigenSolver<TempMatrixImg> &solver, TempMatrixImg &img, int K)
{
    vector<double> res;
    for (int i = 0; i < K; i++)
    {
        double test = ((solver.eigenvectors().col(i).real().transpose()) * img)(0);
        res.push_back(test);
    }
    return res;
}

void writeEigenfaces(EigenSolver<TempMatrixImg> &covMat, MatrixImg &A, int nbrFaces)
{
    // covMat.eigenvalues.
}

void makeEigenSpace(std::string dirIn, std::string dirOut)
{
    int nH, nW;
    vector<OCTET *> set = vector<OCTET *>();

    OCTET *meanImg;
    long long *tempMean;

    long totNumberIm = 0;
    for (auto &file : std::filesystem::directory_iterator(dirIn))
    {
        if (totNumberIm == 0)
        {
            cout << file.path().c_str() << endl;
            lire_nb_lignes_colonnes_image_pgm(file.path().c_str(), &nH, &nW);
            allocation_tableau(meanImg, OCTET, nH * nW);
            tempMean = (long long *)calloc(nH * nW, sizeof(long long));
        }
        // OCTET* im; allocation_tableau(im,OCTET,nH*nW);
        OCTET *im;
        set.push_back(im);
        allocation_tableau(set[totNumberIm], OCTET, nH * nW);
        lire_image_pgm(file.path().c_str(), set[totNumberIm], nH * nW);

        for (int i = 0; i < nH * nW; i++)
        {
            tempMean[i] += set[totNumberIm][i];
        }

        totNumberIm++;
    }

    for (int i = 0; i < nH * nW; i++)
    {
        meanImg[i] = (unsigned char)((double)tempMean[i] / ((double)totNumberIm));
    }
    ecrire_image_pgm(string(dirOut + string("/mean.pgm")).c_str(), meanImg, nH, nW);

    TempMatrixImg bigMatNoMean(nH * nW, totNumberIm);
    for (int i = 0; i < totNumberIm; i++)
    {
        for (int pix = 0; pix < nW * nH; pix++)
        {
            bigMatNoMean(pix, i) = set[i][pix] - meanImg[pix] + 128;
        }
    }

    TempMatrixImg cov = bigMatNoMean.transpose() * bigMatNoMean;
    EigenSolver<TempMatrixImg> eigensolver(cov);

    for (int i = 0; i < eigensolver.eigenvectors().cols(); i++)
    {
        OCTET *im;
        allocation_tableau(im, OCTET, nH * nW);
        TempMatrixImg face = bigMatNoMean * eigensolver.eigenvectors().col(i).real();
        double minD = face(0);
        double maxD = face(0);
        for (int pix = 1; pix < nH * nW; pix++)
        {
            minD = min(minD, face(pix));
            maxD = max(maxD, face(pix));
        }
        for (int pix = 0; pix < nH * nW; pix++)
        {
            // cout<<(face(pix)-minD)*(255.0/(maxD-minD))<<endl;
            im[pix] = (unsigned char)((face(pix) - minD) * (255.0 / (maxD - minD))); // comment fit dans 0 255 ?
        }
        char name[50];
        sprintf(name, "/eigenfaces/im%d.pgm", i);
        ecrire_image_pgm(string(dirOut + string(name)).c_str(), im, nH, nW);
    }
}

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

    std::string nom = string("in");
    string dirIn = string(argv[1]);
    string dirOut = string(argv[2]);
    makeEigenSpace(dirIn,dirOut);

    //      int nH,nW;
    //
    // //     vector<vector<OCTET*>> imageSet = vector<vector<OCTET*>>();
    // //     for (int i=0; i<40;i++){
    // //         imageSet.push_back(vector<OCTET*>());
    // //         for (int j=0;j<10;j++){
    // //             OCTET* im;
    // //             imageSet[i].push_back(im);
    // //         }
    // //     }

    // //     OCTET* meanImg;
    // //     long long* tempMean ;
    // //     int countDirs = 0;
    // //     long totalNumberOfImages=0;
    // int i=0;int j=0;
    //     for (auto & dir : std::filesystem::directory_iterator(nom)){
    //         int countFile=0;
    //         for (auto & file : std::filesystem::directory_iterator(dir.path())){
    //             OCTET* img;
    //             lire_nb_lignes_colonnes_image_pgm(file.path().c_str(),&nH,&nW);
    //             allocation_tableau(img,OCTET,nH*nW);
    //             lire_image_pgm(file.path().c_str(),img,nH*nW);
    //             char name[50];
    //             sprintf(name,"in/%d%d.pgm",i,j);
    //             ecrire_image_pgm(name,img,nH,nW);

    // allocation_tableau(imageSet[countDirs][countFile],OCTET,nH*nW);
    // for (int pix = 0;pix<nH*nW;pix++){
    //     imageSet[countDirs][countFile][pix]=img[pix];
    // }
    // //imageSet[countDirs].push_back(img);
    // if (countDirs==0 && countFile==0){
    //     allocation_tableau(meanImg,OCTET,nH*nW);
    //     tempMean=(long long*)calloc(nH*nW,sizeof(long long));
    // }

    // for (int i=0;i<nH*nW;i++){
    //     tempMean[i]+=img[i];
    // }

    // totalNumberOfImages++;
    // countFile++;

    // //free(img);
    //         j++;
    //     }
    //     i++;

    // }

    //     allocation_tableau(meanImg,OCTET,nH*nW);
    //     for (int i=0;i<nH*nW;i++){
    //         meanImg[i]=(unsigned char)(tempMean[i]/totalNumberOfImages);
    //     }

    //     ecrire_image_pgm("MEAN.pgm",meanImg,nH,nW);

    //     vector<vector<OCTET*>> imagesSansMean = vector<vector<OCTET*>>();
    //     vector<vector<TempMatrixImg>> matrixNoMean = vector<vector<TempMatrixImg>>();

    //     for (int i=0;i<imageSet.size();i++){
    //         imagesSansMean.push_back(vector<OCTET*>());
    //         matrixNoMean.push_back(vector<TempMatrixImg>());
    //         for (int j=0; j<imageSet[i].size();j++){
    //             OCTET* img;
    //             TempMatrixImg mat(nH,nW);
    //             imagesSansMean[i].push_back(img);
    //             allocation_tableau(imagesSansMean[i][j],OCTET,nH*nW);

    //             allocation_tableau(img,OCTET,nH*nW);
    //             for (int pix=0;pix<nH*nW;pix++){
    //                 img[pix]=min(255,max(0,imageSet[i][j][pix]+128-meanImg[pix]));
    //                 imagesSansMean[i][j][pix]=img[pix];

    //             }
    //             for (int x=0;x<nH;x++){
    //                 for (int y=0; y<nW;y++){
    //                     mat(x,y)=img[x*nW+y];
    //                 }
    //             }
    //             matrixNoMean[i].push_back(mat);

    //             //uncomment to write images

    //             //char name[50];
    //             //sprintf(name,"meanless/im%d_%d",i,j);
    //             //ecrire_image_pgm(name,img,nH,nW);
    //         }
    //     }

    //    TempMatrixImg bigMatNoMean(nH*nW,totalNumberOfImages);
    //     for (int i=0;i<imageSet.size();i++){
    //         for (int j=0; j<imageSet[i].size();j++){
    //             for (int pix=0;pix<nH*nW;pix++){
    //                 bigMatNoMean(pix,i*imageSet[0].size()+j)=(double)imagesSansMean[i][j][pix];
    //             }
    //         }
    //     }

    //     TempMatrixImg cov = bigMatNoMean.transpose()*bigMatNoMean;

    //     EigenSolver<TempMatrixImg> eigensolver(cov);
    //     //cout<<eigensolver.eigenvalues()<<endl;

    //     for (int i=0;i<eigensolver.eigenvectors().cols();i++){
    //         OCTET* im; allocation_tableau(im,OCTET,nH*nW);
    //         TempMatrixImg face = bigMatNoMean*eigensolver.eigenvectors().col(i).real();
    //         double minD=face(0);double maxD=face(0);
    //         for (int pix =1; pix<nH*nW;pix++){minD=min(minD,face(pix));maxD=max(maxD,face(pix));}
    //         for (int pix=0;pix<nH*nW;pix++){
    //             //cout<<(face(pix)-minD)*(255.0/(maxD-minD))<<endl;
    //             im[pix]=(unsigned char)((face(pix)-minD)*(255.0/(maxD-minD)));     // comment fit dans 0 255 ?
    //         }
    //         char name[50];
    //         sprintf(name,"eigenfaces/im%d.pgm",i);
    //         ecrire_image_pgm(name,im,nH,nW);

    //     }

    // OCTET* felix; allocation_tableau(felix,OCTET,nH*nW);
    // lire_image_pgm("felixResized.pgm",felix,nH*nW);

    // TempMatrixImg fix(nH,nW) ;
    // for (int i=0;i<nH;i++){
    //     for (int j=0; j<nW;j++){
    //         fix(i,j)=(double)felix[i*nW+j] - meanImg[i*nW+j];
    //     }
    // }

    // vector<double> proj ; proj = projectOnEigenSpace(eigensolver, fix, 40);
    // for (int i=0; i<40; i++){
    //     cout<<proj[i];
    // }
    // cout<<endl;
}


#include <algorithm>
#include <iostream>
#include <random>
#include <vector>
#include <boost/filesystem.hpp>
#include <boost/thread/thread.hpp>
#include "sampling.h"
#include "utilities.h"

const int MAX_BATCH = 13;
const int MAX_PATHS = 40320;

/*
    Single unit of the Travellers Algorithm

    A set of paths are generated. These paths start at the 'source' point, go
    through each point in 'points' and end at the 'destination' point.

    The maximum number of paths that can be generated is 'max_paths'. If the
    number of possible permutations for the paths is larger than 'max_paths',
    the paths generated will be chosen at random.

    Once the set of paths has been generated, they are sorted on a scale from
    0 to 1. The 0 indicates the path with the shortest distance and 1 indicates
    the path with the longest. The path chosen is the path which is nearest on
    the scale to 'calibration'.

    This function returns the indicies of the 'points' of the chosen path
*/
std::vector<int> travel(
    const point& source,
    const point& destination,
    const std::vector<point>& points,
    const float& calibration,
    const int& max_paths=MAX_PATHS
)
{
    std::random_device rd;
    std::mt19937 gen(rd());

    /*
        Create a 2d matrix which stores the euclidean distances between each of
        the points.
    */
    matrix graph = euclideanMatrix(source, points, destination);

    int n = points.size();
    int n_factorial = factorial(n);
    int permutation_count;
    std::vector<int> values;

    if (n_factorial <= max_paths) {
        for (int i = 0; i < n_factorial; ++i) {
            values.emplace_back(i);
        }
        std::shuffle(values.begin(), values.end(), gen);
        permutation_count = n_factorial;
    }
    else {
        permutation_count = max_paths;
        values = sampling::sample_range(permutation_count, n_factorial, gen);
    }

    permutation permutation;
    std::vector<permutation_cost> permutation_costs;
    for (int value: values) {

        float cost = 0;
        int current_index = 0;

        permutation = integer_to_permutation(value, n);

        for (int next_index: permutation) {
            cost += graph[current_index][next_index];
            current_index = next_index;
        }

        cost += graph[current_index][n+1];

        permutation_costs.push_back(permutation_cost(permutation, cost));
    }

    /*
        Order the permutation_costs from lowest to highest.
        Select the permutation according to the calibration.
    */
    sort(permutation_costs.begin(), permutation_costs.end(), sortByCost);

    return permutation_costs[calibration*(permutation_count-1)].first;
}

std::vector<std::pair<int, permutation>> indexed_permutation;
boost::mutex mutex;

void shall(
    int& index,
    const point& source,
    const point& destination,
    const std::vector<point>& points,
    const float& calibration
)
{
    permutation perm = travel(source, destination, points, calibration);

    mutex.lock();
    indexed_permutation.emplace_back(index, perm);
    mutex.unlock();
}

/*
     Batching unit of the Travellers Algorithm

     Points are batched the the single unit Travellers Algorithm is applied to
     each batch, returning the indicies in the order as if the path sequentially
     passes through each batch.
*/
std::vector<int> travels(std::vector<point> points, const float& calibration)
{
    int n = points.size();
    std::vector<int> batch_counts = get_batches(n, MAX_BATCH);

    std::random_device rd;
    std::mt19937 gen(rd());

    std::vector<int> indicies = sampling::sample_range(n, n, gen);
    std::vector<int> results;

    point source = points[indicies[0]];
    point destination;

    std::vector<point> batch_points;
    int i = 1;

    std::vector<boost::thread> threads;

    for (int k = 0; k < batch_counts.size(); ++k) {

        batch_points.clear();
        for (int j = 0; j < batch_counts[k] - 1; ++j) {
            batch_points.emplace_back(points[indicies[i++]]);
        }

        destination = points[indicies[i++ % n]];

        threads.emplace_back(std::move(boost::thread(
            shall,
            i-batch_counts[k]-1,
            source,
            destination,
            batch_points,
            calibration
        )));

        destination = source;
    }

    for (int i = 0; i < threads.size(); ++i) {
        threads[i].join();
    }

    for (std::pair<int, std::vector<int>> pair: indexed_permutation) {
        results.emplace_back(indicies[pair.first]);
        for (int num: pair.second) {
            results.emplace_back(indicies[pair.first+num]);
        }
    }

    return results;
}

int main(int argc, char* argv[])
{
    const int count = atoi(argv[1]);
    const int dimension = atoi(argv[2]);
    const float calibration = atof(argv[3]);

    std::vector<point> points;
    point temp;

    for (int i = 0; i < count; ++i) {
        temp.clear();
        for (int j = 0; j < dimension; ++j) {
            temp.emplace_back(atof(argv[i*dimension + j + 4]));
        }
        points.emplace_back(temp);
    }

    std::vector<int> indicies = travels(points, calibration);

    std::cout << indicies[0];
    for (int i = 1; i < indicies.size(); ++i) {
        std::cout << " " << indicies[i];
    }

    return 0;
}


#include <iostream>
#include <string>
#include <vector>
#include <regex>
#include <optional>

#include <boost/multiprecision/cpp_int.hpp>

using boost::multiprecision::int128_t;

struct Cube {
    Cube(int128_t x1, int128_t x2, int128_t y1, int128_t y2, int128_t z1, int128_t z2) :
        x1(x1), x2(x2), y1(y1), y2(y2), z1(z1), z2(z2) {
        assert(x2 >= x1);
        assert(y2 >= y1);
        assert(z2 >= z1);
    }

    auto operator<=>(Cube const& rhs) const = default;

    bool exceedsLimit(int128_t limit) const {
        if (abs(x1) > limit || abs(x2) > limit || abs(y1) > limit || abs(y2) > limit || abs(z1) > limit || abs(z2) > limit)
            return true;
        return false;
    }

    int128_t Size() const {
        return (1+x2-x1) * (1+y2-y1) * (1+z2-z1);
    }

    std::optional<Cube> Intersects(const Cube& cube) const;

    int128_t x1;
    int128_t x2;
    int128_t y1;
    int128_t y2;
    int128_t z1;
    int128_t z2;
};

std::ostream& operator<<(std::ostream& os, const Cube& cube) {
    os << "(" << cube.x1 << " " << cube.x2 << " " << cube.y1 << " " << cube.y2 << " " << cube.z1 << " " << cube.z2 << ")\n";
    return os;
}

std::optional<Cube> Cube::Intersects(const Cube& cube) const {
    std::optional<Cube> result;
    int128_t xx1 = std::max(x1, cube.x1);
    int128_t xx2 = std::min(x2, cube.x2);
    int128_t yy1 = std::max(y1, cube.y1);
    int128_t yy2 = std::min(y2, cube.y2);
    int128_t zz1 = std::max(z1, cube.z1);
    int128_t zz2 = std::min(z2, cube.z2);
    if (xx2 >= xx1 && yy2 >= yy1 && zz2 >= zz1) {
        result = Cube(xx1, xx2, yy1, yy2, zz1, zz2);
    }
    return result;
}

class Reactor {
public:
    Reactor() {};
    Reactor(const std::optional<int128_t>& limit) : limit_(limit) {};

    void on(const Cube& cube);
    void off(const Cube& cube);

    int128_t count();

private:
    std::optional<int128_t> limit_;
    std::vector<Cube> state_;
};

int128_t Reactor::count() {
    int128_t total = 0;
    for (auto cube : state_) {
        total += cube.Size();
    }
    return total;
}

void Reactor::on(const Cube& cube) {
    if (limit_.has_value() && cube.exceedsLimit(limit_.value()))
        return;
    off(cube);
    state_.emplace_back(cube);
}

void Reactor::off(const Cube& cube) {
    if (limit_.has_value() && cube.exceedsLimit(limit_.value()))
        return;

    std::vector<Cube> new_state;
    for (auto& a : state_) {
        auto intersect = cube.Intersects(a);
        if (intersect.has_value()) {
            {
                int128_t x1 = a.x1;
                int128_t x2 = intersect.value().x1 - 1;
                if (x1 <= x2)
                    new_state.emplace_back(x1, x2, a.y1, a.y2, a.z1, a.z2);
            }
            {
                int128_t x1 = intersect.value().x2 + 1;
                int128_t x2 = a.x2;
                if (x1 <= x2)
                    new_state.emplace_back(x1, x2, a.y1, a.y2, a.z1, a.z2);
            }

            {
                int128_t y1 = a.y1;
                int128_t y2 = intersect.value().y1 - 1;
                if (y1 <= y2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, y1, y2, a.z1, a.z2);
            }
            {
                int128_t y1 = intersect.value().y2 + 1;
                int128_t y2 = a.y2;
                if (y1 <= y2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, y1, y2, a.z1, a.z2);
            }

            {
                int128_t z1 = a.z1;
                int128_t z2 = intersect.value().z1 - 1;
                if (z1 <= z2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, intersect.value().y1, intersect.value().y2, z1, z2);
            }
            {
                int128_t z1 = intersect.value().z2 + 1;
                int128_t z2 = a.z2;
                if (z1 <= z2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, intersect.value().y1, intersect.value().y2, z1, z2);
            }

        } else {
            new_state.emplace_back(a);
        }
    }

    state_ = std::move(new_state);
}

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

    std::vector<std::string> lines;
    while (!std::cin.eof() && !std::cin.fail()) {
        std::string line;
        getline(std::cin, line);
        lines.push_back(line);
    }

    Reactor part_1(50);
    Reactor part_2;

    for (auto& line : lines) {
        const std::regex reg("(on|off) x=(-?\\d*)..(-?\\d*),y=(-?\\d*)..(-?\\d*),z=(-?\\d*)..(-?\\d*)");
        std::smatch match;
        if (regex_search(line, match, reg)) {
            bool on = match.str(1) == "on";
            int128_t x1 = stoi(match.str(2));
            int128_t x2 = stoi(match.str(3));
            int128_t y1 = stoi(match.str(4));
            int128_t y2 = stoi(match.str(5));
            int128_t z1 = stoi(match.str(6));
            int128_t z2 = stoi(match.str(7));
            Cube cube(x1, x2, y1, y2, z1, z2);
            if (on) {
                part_1.on(cube);
                part_2.on(cube);
            } else {
                part_1.off(cube);
                part_2.off(cube);
            }
        }
    }

    std::cout << part_1.count() << std::endl;
    std::cout << part_2.count() << std::endl;

    return 0;
}


// Ben Martin
// January 31, 2006

// This currently works only on undirected graphs...
// Graph must model Adjacency Graph, Incidence Graph, VertexListGraph

#ifndef BOOST_GRAPH_SPARSE_SPECTRUM_HPP
#define BOOST_GRAPH_SPARSE_SPECTRUM_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/vector_property_map.hpp>
#include <utility> // for pair

//#include <iostream.h>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>
#include <ietl/iteration.h>
#include <boost/random/linear_congruential.hpp>
#include <ietl/interface/ublas.h>

typedef long int integer;
typedef double doublereal;

namespace boost {

  template <typename Graph, typename EigenvectorMatrix >
  void sparse_spectrum(Graph& g,
		       int first_eigenvector_index,
		       int num_eigenvectors,
		       EigenvectorMatrix &eigenvectors,
		       double rel_tol = 100,
		       double abs_tol = 1000)
  {
    std::vector<double> evals(num_eigenvectors);
    sparse_spectrum(g, first_eigenvector_index, num_eigenvectors, eigenvectors, evals, rel_tol, abs_tol);
  }

  // Parameters:
  //   first_eigenvector_index:
  //     Since the smallest eigenvector is not useful, often this will
  //       be set to 1, for the "Fiedler vector," though if all
  //       eigenvectors are desired, 0 may be a more logical value.
  //     Negative values are interpreted as allowing a default choice of 1.
  //   num_eigenvectors:
  //     The number of eigencectors to return.

  template <typename Graph, typename EigenvectorMatrix, typename EVector >
  void sparse_spectrum(Graph& g,
		       int first_eigenvector_index,
		       int num_eigenvectors,
		       EigenvectorMatrix &eigenvectors,
		       EVector &eigenalues,
		       double rel_tol = 100,
		       double abs_tol = 1000)
  {

    //    Matrix eigenvectors = *(in_eigenvectors);

    if (first_eigenvector_index < 0)
      first_eigenvector_index = 1;

    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef typename Graph::vertex_iterator VertexIterator;
    typedef typename Graph::edge_iterator EdgeIterator;
    typedef typename boost::graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    IndexMap index_map = get(vertex_index, g);

    VertexIterator v, vs, ve;
    using std::pair;
    std::pair<VertexIterator, VertexIterator> p;
    p = vertices(g);
    vs = p.first;
    ve = p.second;

    EdgeIterator e, es, ee;
    using std::pair;
    std::pair<EdgeIterator, EdgeIterator> ep;
    ep = edges(g);
    es = ep.first;
    ee = ep.second;
    typename Graph::vertex_descriptor src, tgt;

    integer N = num_vertices(g);

    typedef boost::numeric::ublas::compressed_matrix<double> Matrix;
    Matrix A(N, N);

    int i;

    AdjacencyIterator a, as, ae;
    std::pair<AdjacencyIterator, AdjacencyIterator> ap;

    /*
    for (v = vs; v != ve; ++v) {
      A(index_map[*v], index_map[*v]) = (double)out_degree(*v, g);
    }
    */
    for (e = es; e != ee; ++e) {
      src = source(*e, g);
      tgt = target(*e, g);
      if (src != tgt && A(index_map[src], index_map[tgt]) != -1) {
	A(index_map[src], index_map[tgt]) = (double)(-1);
	A(index_map[tgt], index_map[src]) = (double)(-1);
	A(index_map[src], index_map[src]) += (double)1;
	A(index_map[tgt], index_map[tgt]) += (double)1;
      }
    }

    /*
    cout << "index_map = " << endl;
    for (v = vs; v != ve; ++v) {
      cout << index_map[*v] << " ";
    }
    cout << endl;
    cout << "A = " << endl;
    for (i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        cout << A[i*N + j] << " ";
      }
        cout << endl;
    }
    */

    using namespace ietl;

    vectorspace<boost::numeric::ublas::vector<double> > VS(N);
    lanczos<Matrix, vectorspace<boost::numeric::ublas::vector<double> > > LanczosObject(A, VS);

    double _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    double _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    //    _rel_tol = 100.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 100.*std::numeric_limits<double>::epsilon();

    // SO far these are the best I have found:
    //    _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    _rel_tol = rel_tol*std::numeric_limits<double>::epsilon();
    _abs_tol = abs_tol*std::numeric_limits<double>::epsilon();

    lanczos_iteration_nlowest<double> LanczosIterationControl(1000000, first_eigenvector_index + num_eigenvectors, _rel_tol, _abs_tol);
    //    std::cout << std::numeric_limits<double>::epsilon() << std::endl;
    boost::minstd_rand gen(1);
    //boost::rand48 gen(1);
    LanczosObject.calculate_eigenvalues(LanczosIterationControl, gen);
    std::vector<double> evals = LanczosObject.eigenvalues();

    //    for (int i = 0; i < N; i++)
    //      std::cout << evals[i] << " ";
    //    std::cout << std::endl;

    /*
    std::vector<int> multiplicities = LanczosObject.multiplicities();
    int lowest_eval_multiplicity = multiplicities[1];
    int second_eval_multiplicity;
    if (lowest_eval_multiplicity == 1 && multiplicities[2] > 1)
      second_eval_multiplicity = 2;
    else
      second_eval_multiplicity = 1;

    for (int i = 0; i < N; i++)
      std::cout << multiplicities[i] << " ";
    std::cout << std::endl;
    */

    std::vector<boost::numeric::ublas::vector<double> > evecs(num_eigenvectors);
    for (int j = 0; j < num_eigenvectors; j++)
      evecs[j] = *(new boost::numeric::ublas::vector<double>(N));
    std::vector<boost::numeric::ublas::vector<double> >::iterator out_it = evecs.begin();

    Info<double> info;
    std::vector<double>::iterator ebegin, eend;
    ebegin = evals.begin();
    ebegin += first_eigenvector_index;
    eend = evals.begin();
    //    eend += 4 + (1 - lowest_eval_multiplicity) + (1 - second_eval_multiplicity);
    eend += first_eigenvector_index + num_eigenvectors;
    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen, 100000);
    //    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen);

    /*
    // If eigenvalues are repeated, we need to copy eigenvectors
    if (lowest_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[1][i] = evecs[0][i];
      if (lowest_eval_multiplicity > 2) {
	for(int i = 0; i < N; i++)
	  evecs[2][i] = evecs[0][i];
      }
    }
    if (second_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[2][i] = evecs[1][i];
    }
    */

    //    for (int i = 0; i < 3; i++)
    //      for (int j = 0; j < N; j++)
    //	std::cout << evecs[i][j] << " ";
    //    cout << std::endl;

    //    std::cout << A << std::endl;
    //    std::cout << LanczosIterationControl.error_code() << endl;;
    //    std::cout << info.error_info(1) << " " << info.error_info(2) << " " << info.error_info(3) << std::endl;

    //    std::vector<Vector> retval(num_eigenvectors);
    for (int j = 0; j < num_eigenvectors; j++) {
      // retval[j] = *(new Vector(N));
      i = 0;
      for (v = vs; v != ve; ++v) {
	//	  retval[j][i] = evecs[first_eigenvector_index+j-1][i];
	eigenvectors[j][i] = evecs[first_eigenvector_index+j-1][i];

	i++;
      }
    }

    //    return retval;

  } // end spectrum()

} // end namespace boost

#endif // BOOST_GRAPH_SPARSE_SPECTRUM_HPP


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Dense>
#include <queue>
// #include<Eigen/SparseCore>
using namespace Rcpp;
using namespace Eigen;
using namespace std;

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

//
using Eigen::Map;               	// 'maps' rather than copies
using Eigen::Matrix;                  //  matrix generic
using Eigen::MatrixXd;                  // variable size matrix, double precision
using Eigen::VectorXd;                  // variable size vector, double precision
using Eigen::Transpositions;
using Eigen::HouseholderQR;    // Fast scalable QR solver
using Eigen::ColPivHouseholderQR;    // Fast scalable QR solver
using Eigen::FullPivHouseholderQR; // slow full (colsand rows pivoting)
using Eigen::JacobiSVD;
using Eigen::GeneralizedSelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::SelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::LLT;
using Eigen::LDLT;
using Rcpp::List;
using Rcpp::wrap;

// ##########  OK vrsione Sept 04 works

// copied to fspca_sept.cpp

// =========================================================================

// creates a sub-mat of S with indices in e base 0
Eigen::MatrixXd makeSubS(Eigen::MatrixXd S, Eigen::VectorXi e){
  int n = S.cols();
  int r = S.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(r, d );
  for (int i = 0; i < d; ++i){
    M.col(i) = S.col(e(i));
  }
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, d);
}

// retruns the rows in e and keeps first c columns
Eigen::MatrixXd selectRowsC(Eigen::MatrixXd A, Eigen::VectorXi e, int c){
  // ATTENZIONE INDICES BASE 0
  // ATTENZIONE e must be sorted e(0) < e(1)

  int n = A.cols();
  int r = A.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(A.topLeftCorner(r,c));
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, c);
}

void   makeSdAndM(Eigen::MatrixXd S, Eigen::VectorXi e, Eigen::MatrixXd& M, Eigen::MatrixXd& N,
                  int n, int d){
  // M(d, r) N(d, d)
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  //  Eigen::MatrixXd M(d, r);
  for (int i = 0; i < d; ++i){
    M.row(i) = S.row(e(i));
  }
  //  Eigen::MatrixXd N = M.topLeftCorner(d, d);
  for (int i = 0; i < d; ++i){
    N.col(i) = M.col(e(i));
  }
}

// Deflates S (pass already deflated and vector current loads)
// returns vexp by ref
void deflSC(Eigen::VectorXd a, Eigen::MatrixXd& K, Eigen::VectorXi ind, double& vexp){
  // # pass only a nonzero loads
  // K = deflated matrix
  // #  K <-- (S - Saa'S/(a'Sa) // deflated S matrix
  // ## ===
  const int n = ind.size();
  const int p = K.cols();

  // t = Sa
  Eigen::VectorXd t = Eigen::VectorXd::Zero(p);
  for (int i = 0; i < p; i++)
    for(int k = 0; k < n; k++)
      t(i) += K(i, ind(k)) * a(k ); // only elements in ind
  // tt = a'Sa = t'a

  double tt = 0.0;
  for(int k = 0; k < n; k++)
    tt += a(k) * t(ind(k));
  if (tt > 0)
    tt = 1/tt;
  else
    Rf_error("defSC: tt is not > 0");

  // O = Sa/(tt)
  const Eigen::VectorXd O = (t.array()*tt).matrix();

  const double cvk = K.trace();
  // K = S - Saa'S/(a'Sa) deflated S
  Eigen::MatrixXd L = t * O.transpose();
  K = K - t * O.transpose(); //deflated S
  vexp =  cvk - K.trace() ;

  return;
}
//

// finds max part corr exclude small ss, pdates indnot returns ind
int findmax(Eigen::VectorXi& indnot, Eigen::VectorXd vt){

  double p = indnot.size();
  double m = 0.0;
  int ind = 0;
  for (int i = 0; i < p; i++){
    if (indnot(i) == -2){
      if(vt(i) > m){
        m = vt(i);
        ind = i;
      }
    }
  }
  indnot(ind) = ind;
  return ind;
}

// indnot could be used for extracting the indices later, so use -2, -1 and {0:(p-1)}
// fixed
void fwd_selectC(Eigen::MatrixXd S, Eigen::VectorXi& ind, int& card,
                 Eigen::VectorXd si, double totvexp, double pvexp,
                 double fullrank = 0.0){
  Eigen::VectorXd sik = si;
  int p = S.cols();
  // int induno;
  double tmp;
  Eigen::VectorXd vexpt(p);
  Eigen::VectorXd cvexpt(p);
  Eigen::VectorXd vt(p);
  Eigen::VectorXi indnot = Eigen::VectorXi::Constant(p, -2);
  Eigen::VectorXd ba(p);

  for (int i=0; i < p; i++)
    vt(i) = sik(i) * sik(i) / S(i,i);

  ind(0) = findmax(indnot, vt);

  vexpt(0) = vt(ind(0));
  cvexpt(0) = vt(ind(0));
  int i = 1;
  bool stopSelect = false;
  // start looping ============================================
  while (stopSelect == false){

    tmp = sik(ind(i - 1))/S(ind(i - 1), ind(i - 1));
    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        sik(j) = sik(j) -  (tmp * S(ind(i-1), j));
      }
      else{
        sik(j) = 0;
      }
    }

    ba = (S.col(ind(i-1)).array()/sqrt(S(ind(i-1), ind(i-1)))).matrix();
    S = S - ba * ba.transpose();

    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        if (S(j,j)> fullrank)
          vt(j) = sik(j) * sik(j)/S(j,j);
        else{
          indnot(j) = -1;
          vt(j) = 0;
        }
      }
      else{
        vt(j) = 0;
      }
    }

    ind(i) = findmax(indnot, vt);
    indnot(ind(i)) = 0;

    vexpt(i) =  vt(ind(i));
    cvexpt(i) = cvexpt(i-1) + vexpt(i);

    if (cvexpt(i) >= pvexp*totvexp){
      card = i + 1;
      stopSelect = true;
    }
    else{
      i = i + 1;
    }
    //    Rcpp::checkUserInterrupt();

  }
}

// power method computes only first eigvec, about 82 times faster tha eigen!
Eigen::VectorXd eigvecPMC(Eigen::MatrixXd& X, double& val, double eps = 10E-5){
  const int p = X.cols();
  double sqp = sqrt(double(p));
  Eigen::VectorXd v0 = VectorXd::Constant(p, 1.0/sqp);
  Eigen::VectorXd v = VectorXd::Constant(p, 0.0);
  double stp = 1.0;
  int k = 0;
  while (stp > eps){
    v = X * v0;
    val = v.norm();
    v = v.array()/val;
    stp = (v0.array() - v.array()).matrix().norm();
    v0 = v;
    k++;
    if (k > 100){
      Rf_warning("Powermethod: not converged in 100 iterations. Error is", k);
      break;//here should use try-catch
    }
  }
  //  Rcout << "k = " << k << "; stp = " << stp << endl;
  return (v.array() * val);
}

// This is the main function for R
// S correl matrix
// pvexpfs is proportion of PC to explain by each block
// pvexp is proportion total variance of matrix to explain to terminate computing comps
// ncomps nistead of pvexp maximum number of comps (priority)
// full rank small eps to discard vars from selection
// simply projects current full rank PC onto set of variables in ind
// it does not compute the LS SPCA components, it seems to work as well as that
// uses power method to compute PCs
// new version reduces K and S in one function (+6% ) < check cost of resizing
// [[Rcpp::export]]
List fspcaCpmNoe(Eigen::MatrixXd S, double pvexpfs = 0.95, double pvexp = 0.95,
               int ncomps = 0, double fullrank = 0,  double eps = 10E-5){
  int p = S.cols();
  if (ncomps == 0)
    ncomps = p;
  Eigen::MatrixXd K(S);

  double totvexp = S.trace();// total variance S
  double maxvexp;// this is vexp by first PC for fow_select

  Eigen::VectorXd si(p);

  si = eigvecPMC(S, maxvexp);
//Rcout << "done si first " << endl;
  // here could compute D as   vec * diag(val^2) * vec.transpose

  Eigen::MatrixXd Sinv(p, p);
  Eigen::MatrixXd M(p, p);

  Eigen::VectorXd a(p);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(p, ncomps);
  //  List load(p);
  List indout(p);

  Eigen::VectorXd vexp = Eigen::VectorXd::Zero(ncomps);
  Eigen::VectorXd cvexp = vexp;
  double cvt;
  Eigen::VectorXi indj(p);//this to pass to fwd_select

  Eigen::MatrixXd Sd(p, p);// maybe better leave dynamic? this takes S[onlyind, onlyind]

  int cardt = 0;
  Eigen::VectorXi card(p);
  int nc = 0;
  bool stopComp = false;

  int j = 0;
  while (stopComp == false){
    fwd_selectC(S, indj, cardt, si, maxvexp, pvexpfs, fullrank);
//Rcout << "comp "<< j + 1 << "done fwd_swlwct " << endl;

    card(j) = cardt;
    std::sort(indj.data(),indj.data() + cardt);

    // if ( j == 2)
    //   Rf_error("done 1");

    // need make one function that does bot Sd and M
    // create submatrices for computing loaidngs

    Sd.resize(cardt, cardt);
    M.resize(cardt, NoChange);
    makeSdAndM(S, indj.head(cardt), M, Sd, p,cardt);
//    Sd.topLeftCorner(cardt, cardt) = makeSubS(S, indj.head(cardt));
//    M.topLeftCorner(cardt, p) = selectRowsC(K, indj.head(cardt), p);
//Rcout << "comp "<< j + 1 << "done selectRows" << endl;

    //  compute loadings
  //  Sinv.topLeftCorner(cardt, cardt)  = Sd.topLeftCorner(cardt, cardt).llt().solve(MatrixXd::Identity(cardt, cardt));
    Sinv.topLeftCorner(cardt, cardt)  = Sd.llt().solve(MatrixXd::Identity(cardt, cardt));
    //Rcout << "comp "<< j + 1 << "done Sinv" << endl;

    // save loadings
    a.head(cardt) = ((Sinv.topLeftCorner(cardt, cardt) * M * si).array() / maxvexp).matrix();
//Rcout << "comp "<< j + 1 << "done loadings" << endl;
    // save loadings in column j
    for (int i = 0; i < cardt; i++){
      A(indj(i), j) = a(i);
    }
    // save loadings in list
    indout[j] = indj.head(cardt).array() + 1;

    nc = nc + 1;

    // this new func deflates S and D using only last vector of loads
    // returns deflated matr by references and vexp (not cum vexp)
    deflSC(a.head(cardt), K, indj.head(cardt), cvt);
//Rcout << "comp "<< j + 1 << "done deflSC" << endl;

    vexp(j) = cvt;
    if (j > 0)
      cvexp(j) = cvt + cvexp(j-1);
    else
      cvexp(j) = cvt;

    // checks if stopComp met
    if ((cvexp(j) > pvexp * totvexp) || ((j + 1) == ncomps)){
      stopComp = true;
      ncomps = nc;
    }
    else{
      // this power method, returns si and passes maxvexp byref
      si = eigvecPMC(K, maxvexp);
//Rcout << "comp "<< j + 1 << "done si" << endl;

      j = j + 1;
    }
  }//end compute comps

  IntegerVector idx = Rcpp::seq(0, nc - 1);

  return  List::create(Named("loadings") = A.topLeftCorner(p,nc), Named("ncomps") = nc,
                       Named("ind") = indout[idx], Named("card") = card.head(nc),
                       Named("vexp") = vexp.head(nc), Named("cvexp") = cvexp.head(nc));
}


#ifndef _OPT_PLACEMENT_HPP_
#define _OPT_PLACEMENT_HPP_

//#include <boost/graph/graph_traits.hpp>
//#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/graphviz.hpp>
//#include <boost/graph/iteration_macros.hpp>
//#include <boost/foreach.hpp>
//#include <Eigen/Core>
//#include <Eigen/LU>
//#include "mod2.hpp"
#include <modularity.hpp>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

namespace opt_placement
{

  inline gsl_matrix* _laplacian(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* l = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  	size_t j = 0;
		    foreach(Node* node2, g->getNodes()){
		    	if (verts.find(node1) == verts.end()|| verts.find(node2) == verts.end()){
		    		gsl_matrix_set(l, i,j,0.0);
		    	} else if (i == j){
		    		gsl_matrix_set(l, i,j,node1->incomingEdges().count() + node1->outgoingEdges().count());
		    	} else if (mod::_linked(node1, node2) || mod::_linked(node2, node1)){
		    		gsl_matrix_set(l, i,j,-1.0);
		    	} else {
		    		gsl_matrix_set(l, i,j,0.0);
		    	}
		    	++j;
		    }
		    ++i;
	  }

	  return l;
  }

   // include in the adjacency iff source _or_ target is in verts
  inline gsl_matrix* _adjacency(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* a = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  	size_t j = 0;
		    foreach(Node* node2, g->getNodes()){
				  if ((verts.find(node1) != verts.end() || verts.find(node2) != verts.end()) && (mod::_linked(node1, node2) || mod::_linked(node2, node1)))
					  gsl_matrix_set(a, i,j,1.0);
				  else
					  gsl_matrix_set(a, i,j,0.0);

		    	++j;
		    }
		    ++i;
	  }

	  return a;
  }

   // include in the degree matrix iff vertex is in verts
  inline gsl_matrix* _degree(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* d = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  if (verts.find(node1) != verts.end())
			  gsl_matrix_set(d, i,i,node1->incomingEdges().count() + node1->outgoingEdges().count());

		  ++i;
	  }
	  return d;
  }

  template<typename V, typename T>
  inline int _find_index(const V& vect, const T& e)
  {
    for (int i = 0; i < vect.size(); ++i)
      if (e == vect[i]){
//    	  std::cout << i << std::endl;
          return i;
      }

    return -1;
  }

  inline gsl_vector* _f(const Cppn* g,
		  const QList<Node*>& inputs,
		  const QVector<double>& coords_inputs,
		  const QList<Node*>& outputs,
		  const QVector<double>& coords_outputs)
  {
	  gsl_vector* f = gsl_vector_calloc(g->getNrOfNodes());

	  size_t i = 0;

	  foreach(Node* node1, g->getNodes()){
		  int in = _find_index(inputs, node1);
		  if (in != -1){
		  	  gsl_vector_set(f, i, coords_inputs[in]);
		  }
		  int out = _find_index(outputs, node1);
		  if (out != -1){
		  	  gsl_vector_set(f, i, coords_outputs[out]);
		  }
		  ++i;
	  }

	  return f;
  }

  inline QVector<double> compute(const Cppn* g,
                                 const QList<Node*>& inputs,
                                 const QVector<double>& coords_inputs,
                                 const QList<Node*>& outputs,
                                 const QVector<double>& coords_outputs)
  {
    typedef Node* v_d_t;
    std::set<v_d_t> all_set, io_set, no_io_set;

    foreach(Node* node1, g->getNodes()) all_set.insert(node1);
    foreach(Node* node, inputs) io_set.insert(node);
    foreach(Node* node, outputs) io_set.insert(node);

    std::set_difference(all_set.begin(), all_set.end(),
                        io_set.begin(), io_set.end(),
                        std::insert_iterator<std::set<v_d_t> >(no_io_set,
                                                               no_io_set.begin()));
    gsl_matrix* l = _laplacian(g, no_io_set);
    gsl_matrix* b = _adjacency(g, io_set);
    gsl_matrix* d = _degree(g, io_set);
    gsl_vector* f = _f(g, inputs, coords_inputs, outputs, coords_outputs);

	// Define all the used matrices
	gsl_matrix_add (l, d);

	//Calculate inverse of l+d
    int s;
	gsl_matrix* temp1 = gsl_matrix_alloc (l->size1, l->size2);
	gsl_matrix* temp2 = gsl_matrix_alloc (l->size1, l->size2);

	gsl_vector* result = gsl_vector_alloc(l->size1);
	QVector<double> qresult(l->size1);

	gsl_permutation * perm = gsl_permutation_alloc (f->size);
	gsl_linalg_LU_decomp (l, perm, &s);
	gsl_linalg_LU_invert (l, perm, temp1);

	//Multiplying the inverse of l+d with b
	gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, temp1, b, 0.0, temp2);

	//Multiplying the inverse of l+d multiplied with b with f
	gsl_blas_dgemv (CblasNoTrans, 1.0, temp2, f, 0.0, result);

	//Fix positions of input nodes
	int i=0;
	foreach(Node* node1, g->getNodes()){
		if (io_set.find(node1) != io_set.end()){
			qresult[i] = gsl_vector_get(f, i);
		} else {
			qresult[i] = gsl_vector_get(result, i);
		}
		i++;
	}

	gsl_matrix_free(temp1);
	gsl_matrix_free(temp2);
	gsl_vector_free(result);

    return qresult;
  }
}
#endif


// hpr-itl cg.cpp: HPR Conjugate Gradient algorithms
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include "common.hpp"

// enable posit arithmetic exceptions
#define POSIT_THROW_ARITHMETIC_EXCEPTION 1
#include <posit>

// MTL
#include <boost/numeric/itl/itl.hpp>
// defines all Krylov solvers
// CG, CGS, BiCG, BiCGStab, BiCGStab2, BiCGStab_ell, FSM, IDRs, GMRES TFQMR, QMR, PC

namespace hpr {
	template<typename Vector, size_t nbits, size_t es, size_t capacity = 10>
	sw::unum::posit<nbits, es> fused_dot(const Vector& x, const Vector& y) {
		sw::unum::quire<nbits, es, capacity> q = 0;
		size_t ix, iy, n = size(x);
		for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + 1, iy = iy + 1) {
			q += sw::unum::quire_mul(x[ix], y[iy]);
		}
		sw::unum::posit<nbits, es> sum;
		convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
		return sum;
	}

	/// Conjugate Gradients without preconditioning
	template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
		typename Iteration >
	int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
			Iteration& iter)
	{
		mtl::vampir_trace<7001> tracer;
		using std::abs; using mtl::conj; using mtl::lazy;
		typedef HilbertSpaceX Vector;
		typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
		typedef typename Iteration::real                            Real;

		constexpr size_t nbits = Scalar::nbits;
		constexpr size_t es = Scalar::es;

		Scalar rho(0), rho_1(0), alpha(0), alpha_1(0);
		Vector p(resource(x)), q(resource(x)), r(resource(x)), z(resource(x));

		r = b - A*x;
		rho = fused_dot<Vector, nbits, es>(r, r);
		while (!iter.finished(Real(sqrt(abs(rho))))) {
			++iter;
			if (iter.first())
				p = r;
			else
				p = r + (rho / rho_1) * p;

			q = A * p; alpha = rho / fused_dot<Vector, nbits, es>(p, q);
//			(lazy(q) = A * p) || (lazy(alpha_1) = lazy_dot(p, q));
//			alpha = rho / alpha_1;

			x += alpha * p;
			rho_1 = rho;
//			(lazy(r) -= alpha * q) || (lazy(rho) = lazy_unary_dot(r));
			r -= alpha * q; rho = fused_dot<Vector, nbits, es>(r, r);

		}

		return iter;
	}
}

template<typename Scalar>
int regular_CG()
{
	using Matrix = mtl::mat::compressed2D< Scalar >;
	using Vector = mtl::vec::dense_vector< Scalar >;

	// Create a 1,600 x 1,600 matrix using a 5-point Laplacian stencil
	const size_t size = 40, N = size * size;
	Matrix A(N, N);
	mtl::mat::laplacian_setup(A, size, size);

	// Set b such that x == 1 is solution; start with x == 0
	mtl::vec::dense_vector<Scalar>       x(N, 1.0), b(N);
	b = A * x; x = 0;

	// Termination criterion: r < 1e-6 * b or N iterations
	//noisy_iteration< Scalar >  iter(b, 500, 1.e-6);
	itl::cyclic_iteration< Scalar >  iter(b, 500, 1.e-6);

	// Solve Ax == b without a preconditioner P
	itl::cg(A, x, b, iter);

	int nrOfIterations = -1;
	if (iter.is_converged()) nrOfIterations = iter.iterations();
	return nrOfIterations;
}

template<typename Scalar>
int fdp_CG()
{
	using Matrix = mtl::mat::compressed2D< Scalar >;
	using Vector = mtl::vec::dense_vector< Scalar >;

	// Create a 1,600 x 1,600 matrix using a 5-point Laplacian stencil
	const size_t size = 40, N = size * size;
	Matrix A(N, N);
	mtl::mat::laplacian_setup(A, size, size);

	// Set b such that x == 1 is solution; start with x == 0
	mtl::vec::dense_vector<Scalar>       x(N, 1.0), b(N);
	b = A * x; x = 0;

	// Termination criterion: r < 1e-6 * b or N iterations
	//itl::noisy_iteration< Scalar >  iter(b, 500, 1.e-6);
	itl::cyclic_iteration< Scalar >  iter(b, 500, 1.e-6);

	// Solve Ax == b without a preconditioner P
	hpr::cg(A, x, b, iter);

	int nrOfIterations = -1;
	if (iter.is_converged()) nrOfIterations = iter.iterations();
	return nrOfIterations;
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::unum;
	using namespace mtl;
	using namespace mtl::mat;
	using namespace itl;

	bool bSuccess = true;

	fdp_CG< posit<32, 2> >();

	return 0;

#if 0
	cout << "CG<double> #iterations: " << regular_CG<float>() << endl;
	cout << "CG<posit<32,3> #iterations: " << fdp_CG< posit<32, 3> >() << endl;
	cout << "CG<posit<32,2> #iterations: " << fdp_CG< posit<32, 2> >() << endl;
	cout << "CG<posit<32,1> #iterations: " << fdp_CG< posit<32, 1> >() << endl;
//	cout << "CG<posit<32,0> #iterations: " << fdp_CG< posit<32, 0> >() << endl;
	cout << "CG<posit<28,3> #iterations: " << fdp_CG< posit<28, 3> >() << endl;
	cout << "CG<posit<28,2> #iterations: " << fdp_CG< posit<28, 2> >() << endl;
	cout << "CG<posit<28,1> #iterations: " << fdp_CG< posit<28, 1> >() << endl;
//	cout << "CG<posit<28,0> #iterations: " << fdp_CG< posit<28, 0> >() << endl;
	cout << "CG<posit<24,3> #iterations: " << fdp_CG< posit<24, 3> >() << endl;
	cout << "CG<posit<24,2> #iterations: " << fdp_CG< posit<24, 2> >() << endl;
	cout << "CG<posit<24,1> #iterations: " << fdp_CG< posit<24, 1> >() << endl;
//	cout << "CG<posit<24,0> #iterations: " << fdp_CG< posit<24, 0> >() << endl;
	cout << "CG<posit<20,3> #iterations: " << fdp_CG< posit<20, 3> >() << endl;
	cout << "CG<posit<20,2> #iterations: " << fdp_CG< posit<20, 2> >() << endl;
	cout << "CG<posit<20,1> #iterations: " << fdp_CG< posit<20, 1> >() << endl;
//	cout << "CG<posit<20,0> #iterations: " << fdp_CG< posit<20, 0> >() << endl;
	cout << "CG<posit<16,3> #iterations: " << fdp_CG< posit<16, 3> >() << endl;
	cout << "CG<posit<16,2> #iterations: " << fdp_CG< posit<16, 2> >() << endl;
	cout << "CG<posit<16,1> #iterations: " << fdp_CG< posit<16, 1> >() << endl;
#endif

	return (bSuccess ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_SUCCESS; //as we manually throwing the not supported yet it should not fall through the cracks     EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::runtime_error& err) {
	std::cerr << "Uncaught runtime exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_NTHROOT_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_NTHROOT_HPP_INCLUDED
#include <nt2/exponential/functions/nthroot.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/is_odd.hpp>
#include <nt2/include/functions/scalar/minusone.hpp>
#include <nt2/include/functions/scalar/pow.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/sign.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/functions/scalar/is_inf.hpp>
#endif
#include <iostream>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( nthroot_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< floating_<A0> >)(scalar_< integer_<A1> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(2)
    {
      bool is_ltza0 = is_ltz(a0);
      bool is_odda1 = is_odd(a1);
      if (is_ltza0 && !is_odda1) return Nan<A0>();
      A0 x = nt2::abs(a0);
      if (x == One<A0>())  return a0;
      if (!a1) return (x < One<A0>()) ? Zero<A0>() : sign(a0)*Inf<A0>();
      if (!a0) return Zero<A0>();
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (is_inf(a0)) return (a1) ? a0 : One<A0>();
      #endif
      A0 aa1 = static_cast<A0>(a1);
      A0 y = nt2::pow(x,rec(aa1));
      // Correct numerical errors (since, e.g., 64^(1/3) is not exactly 4)
      // by one iteration of Newton's method
      if (y) y -= (nt2::pow(y, a1) - x) / (aa1* nt2::pow(y,minusone(a1)));
      return (is_ltza0 && is_odda1)? -y : y;
    }
  };
} }

#endif


//
// $Id$
//
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//
// Original author: Robert Burke <robert.burke@gmail.com>
//
// This code taken from the following site:
// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?Effective_UBLAS/Matrix_Inversion
//

#ifndef HOUSEHOLDERQR_HPP
#define HOUSEHOLDERQR_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace ublas = boost::numeric::ublas;

namespace pwiz {
namespace math {

template<class T>
void TransposeMultiply (const ublas::vector<T>& vector,
			ublas::matrix<T>& result,
			size_t size)
{
  result.resize (size,size);
  result.clear ();
  for(unsigned int row=0; row< vector.size(); ++row)
    {
      for(unsigned int col=0; col < vector.size(); ++col)
	result(row,col) = vector(col) * vector(row);

    }
}

template<class T>
void HouseholderCornerSubstraction (ublas::matrix<T>& LeftLarge,
				    const ublas::matrix<T>& RightSmall)
{
  using namespace boost::numeric::ublas;
  using namespace std;
  if(
     !(
       (LeftLarge.size1() >= RightSmall.size1())
       && (LeftLarge.size2() >= RightSmall.size2())
	)
      )
    {
      cerr << "invalid matrix dimensions" << endl;
      return;
    }

  size_t row_offset = LeftLarge.size2() - RightSmall.size2();
  size_t col_offset = LeftLarge.size1() - RightSmall.size1();

  for(unsigned int row = 0; row < RightSmall.size2(); ++row )
    for(unsigned int col = 0; col < RightSmall.size1(); ++col )
      LeftLarge(col_offset+col,row_offset+row) -= RightSmall(col,row);
}

template<class T>
void HouseholderQR (const ublas::matrix<T>& M,
		    ublas::matrix<T>& Q,
		    ublas::matrix<T>& R)
{
  using namespace boost::numeric::ublas;
  using namespace std;

  if(
     !(
       (M.size1() == M.size2())
	)
      )
    {
      cerr << "invalid matrix dimensions" << endl;
      return;
    }
  size_t size = M.size1();

  // init Matrices
  matrix<T> H, HTemp;
  HTemp = identity_matrix<T>(size);
  Q = identity_matrix<T>(size);
  R = M;

  // find Householder reflection matrices
  for(unsigned int col = 0; col < size-1; ++col)
    {
      // create X vector
      ublas::vector<T> RRowView = column(R,col);
      vector_range< ublas::vector<T> > X2 (RRowView, range (col, size));
      ublas::vector<T> X = X2;

      // X -> U~
      if(X(0) >= 0)
	X(0) += norm_2(X);
      else
	X(0) += -1*norm_2(X);

      HTemp.resize(X.size(),X.size(),true);

      TransposeMultiply(X, HTemp, X.size());

      // HTemp = the 2UUt part of H
      HTemp *= ( 2 / inner_prod(X,X) );

      // H = I - 2UUt
      H = identity_matrix<T>(size);
      HouseholderCornerSubstraction(H,HTemp);

      // add H to Q and R
      Q = prod(Q,H);
      R = prod(H,R);
    }
}

}
}

#endif // HOUSEHOLDERQR_HPP


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

#pragma once

#include <Eigen/Dense>
#include <time.h>
#include <vector>

#include <boost/random/gamma_distribution.hpp> // for gamma_distribution.
#include <boost/math/special_functions/gamma.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/cat.hpp>
#include <dpMM/mult.hpp>

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

#ifdef BOOST_OLD
using boost::gamma_distribution;
#else
using boost::random::gamma_distribution;
#endif

#ifdef WIN32
	using boost::math::lgamma;
#endif

template<class Disc, typename T>
class Dir : public Distribution<T>
{
public:
  uint32_t K_;
  Matrix<T,Dynamic,1> alpha_;

  Dir(const Matrix<T,Dynamic,1>& alpha, boost::mt19937 *pRndGen);
  Dir(const Matrix<T,Dynamic,1>& alpha, const Matrix<T,Dynamic,1>& counts, boost::mt19937 *pRndGen);
  Dir(const Dir& other);
  ~Dir();

  Dir<Disc,T>* copy();

  Disc sample();
  Dir<Disc,T> posterior() const;
  Dir<Disc,T> posterior(const VectorXu& z);
  Dir<Disc,T> posterior(const Matrix<T,Dynamic,Dynamic>& x,
      const VectorXu& z, uint32_t k);
  Dir<Disc,T> posteriorFromCounts(const Matrix<T,Dynamic,1>& counts);
  Dir<Disc,T> posteriorFromCounts(const VectorXu& counts);
  Dir<Disc,T> posteriorFromCounts(const vector<Matrix<T,Dynamic,1> > &
      counts, const VectorXu& z, uint32_t k);

  T logPdf(const Disc& cat);

  uint32_t K(){return K_;}

  T logPdf(const Disc& cat) const;
  T logPdfMarginalized() const; // log pdf of SS under NIW prior
  T logPdfUnderPriorMarginalizedMerged(const Dir<Disc,T>& other) const;

  T logLikelihoodMarginalized(const Matrix<T,Dynamic,1>& counts) const;
  void print() const;

  virtual Dir<Disc,T>* merge(const Dir<Disc,T>& other);
  void fromMerge(const Dir<Disc,T>& niwA, const Dir<Disc,T>& niwB);

//  const Matrix<T,Dynamic,Dynamic>& scatter() const {return scatter_;};
//  Matrix<T,Dynamic,Dynamic>& scatter() {return scatter_;};
//  const Matrix<T,Dynamic,1>& mean() const {return mean_;};
//  Matrix<T,Dynamic,1>& mean() {return mean_;};
//  T count() const {return count_;};
//  T& count() {return count_;};
//
  const Matrix<T,Dynamic,1>& counts() const {return counts_;};
  Matrix<T,Dynamic,1>& counts() {return counts_;};
  void setCounts(const Matrix<T,Dynamic,1>& counts) {counts_ = counts;};
  T count() const {return counts_.sum();};

  void computeMergedSS( const Dir<Disc,T>& dirA,
      const Dir<Disc,T>& dirB, Matrix<T,Dynamic,1>& NsM) const;

private:

  Matrix<T,Dynamic,1> counts_; // counts for the different classes -> SS
  vector<gamma_distribution<> > gammas_;

  Matrix<T,Dynamic,1> samplePdf();
};

typedef Dir<Cat<double>, double> DirCatd;
typedef Dir<Cat<float>, float> DirCatf;
typedef Dir<Mult<double>, double> DirMultd;
typedef Dir<Mult<float>, float> DirMultf;


// full credits to https://github.com/ethz-asl/geodetic_utils
// todo add as external lib
#ifndef GEODETIC_CONVERTER_H_
#define GEODETIC_CONVERTER_H_

#include "math.h"
#include <Eigen/Dense>

namespace geodetic_converter
{
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
static double kSemiminorAxis = 6356752.3142;
static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
static double kFlattening = 1 / 298.257223563;

class GeodeticConverter
{
public:
    GeodeticConverter()
    {
        haveReference_ = false;
    }

    ~GeodeticConverter()
    {
    }

    // Default copy constructor and assignment operator are OK.

    bool isInitialised()
    {
        return haveReference_;
    }

    void getReference(double* latitude, double* longitude, double* altitude)
    {
        *latitude = initial_latitude_;
        *longitude = initial_longitude_;
        *altitude = initial_altitude_;
    }

    void initialiseReference(const double latitude, const double longitude, const double altitude)
    {
        // Save NED origin
        initial_latitude_ = deg2Rad(latitude);
        initial_longitude_ = deg2Rad(longitude);
        initial_altitude_ = altitude;

        // Compute ECEF of NED origin
        geodetic2Ecef(latitude, longitude, altitude, &initial_ecef_x_, &initial_ecef_y_, &initial_ecef_z_);

        // Compute ECEF to NED and NED to ECEF matrices
        double phiP = atan2(initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2)));

        ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_);
        ned_to_ecef_matrix_ = nRe(initial_latitude_, initial_longitude_).transpose();

        haveReference_ = true;
    }

    void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x,
                       double* y, double* z)
    {
        // Convert geodetic coordinates to ECEF.
        // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
        double lat_rad = deg2Rad(latitude);
        double lon_rad = deg2Rad(longitude);
        double xi = sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
        *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
        *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
        *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) * sin(lat_rad);
    }

    void ecef2Geodetic(const double x, const double y, const double z, double* latitude,
                       double* longitude, double* altitude)
    {
        // Convert ECEF coordinates to geodetic coordinates.
        // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
        // to geodetic coordinates," IEEE Transactions on Aerospace and
        // Electronic Systems, vol. 30, pp. 957-961, 1994.

        double r = sqrt(x * x + y * y);
        double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
        double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
        double G = r * r + (1 - kFirstEccentricitySquared) * z * z - kFirstEccentricitySquared * Esq;
        double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
        double S = cbrt(1 + C + sqrt(C * C + 2 * C));
        double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
        double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
        double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + sqrt(
                                                                          0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) - P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
        double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
        double V = sqrt(
            pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * z * z);
        double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
        *altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
        *latitude = rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
        *longitude = rad2Deg(atan2(y, x));
    }

    void ecef2Ned(const double x, const double y, const double z, double* north, double* east,
                  double* down)
    {
        // Converts ECEF coordinate position into local-tangent-plane NED.
        // Coordinates relative to given ECEF coordinate frame.

        Eigen::Vector3d vect, ret;
        vect(0) = x - initial_ecef_x_;
        vect(1) = y - initial_ecef_y_;
        vect(2) = z - initial_ecef_z_;
        ret = ecef_to_ned_matrix_ * vect;
        *north = ret(0);
        *east = ret(1);
        *down = -ret(2);
    }

    void ned2Ecef(const double north, const double east, const double down, double* x, double* y,
                  double* z)
    {
        // NED (north/east/down) to ECEF coordinates
        Eigen::Vector3d ned, ret;
        ned(0) = north;
        ned(1) = east;
        ned(2) = -down;
        ret = ned_to_ecef_matrix_ * ned;
        *x = ret(0) + initial_ecef_x_;
        *y = ret(1) + initial_ecef_y_;
        *z = ret(2) + initial_ecef_z_;
    }

    void geodetic2Ned(const double latitude, const double longitude, const double altitude,
                      double* north, double* east, double* down)
    {
        // Geodetic position to local NED frame
        double x, y, z;
        geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
        ecef2Ned(x, y, z, north, east, down);
    }

    void ned2Geodetic(const double north, const double east, const double down, double* latitude,
                      double* longitude, double* altitude)
    {
        // Local NED position to geodetic coordinates
        double x, y, z;
        ned2Ecef(north, east, down, &x, &y, &z);
        ecef2Geodetic(x, y, z, latitude, longitude, altitude);
    }

    void geodetic2Enu(const double latitude, const double longitude, const double altitude,
                      double* east, double* north, double* up)
    {
        // Geodetic position to local ENU frame
        double x, y, z;
        geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);

        double aux_north, aux_east, aux_down;
        ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);

        *east = aux_east;
        *north = aux_north;
        *up = -aux_down;
    }

    void enu2Geodetic(const double east, const double north, const double up, double* latitude,
                      double* longitude, double* altitude)
    {
        // Local ENU position to geodetic coordinates

        const double aux_north = north;
        const double aux_east = east;
        const double aux_down = -up;
        double x, y, z;
        ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
        ecef2Geodetic(x, y, z, latitude, longitude, altitude);
    }

private:
    inline Eigen::Matrix3d nRe(const double lat_radians, const double lon_radians)
    {
        const double sLat = sin(lat_radians);
        const double sLon = sin(lon_radians);
        const double cLat = cos(lat_radians);
        const double cLon = cos(lon_radians);

        Eigen::Matrix3d ret;
        ret(0, 0) = -sLat * cLon;
        ret(0, 1) = -sLat * sLon;
        ret(0, 2) = cLat;
        ret(1, 0) = -sLon;
        ret(1, 1) = cLon;
        ret(1, 2) = 0.0;
        ret(2, 0) = cLat * cLon;
        ret(2, 1) = cLat * sLon;
        ret(2, 2) = sLat;

        return ret;
    }

    inline double rad2Deg(const double radians)
    {
        return (radians / M_PI) * 180.0;
    }

    inline double deg2Rad(const double degrees)
    {
        return (degrees / 180.0) * M_PI;
    }

    double initial_latitude_;
    double initial_longitude_;
    double initial_altitude_;

    double initial_ecef_x_;
    double initial_ecef_y_;
    double initial_ecef_z_;

    Eigen::Matrix3d ecef_to_ned_matrix_;
    Eigen::Matrix3d ned_to_ecef_matrix_;

    bool haveReference_;

}; // class GeodeticConverter
}; // namespace geodetic_conv

#endif // GEODETIC_CONVERTER_H_


#include <iostream>
#include <armadillo>
#include <boost/tuple/tuple.hpp>
#include "gnuplot-iostream.h"
#include "ODE.h"
#include "sysFunc.h"

/* I should make it compatible with armadillo, currently it is working with std::vector containers
to be able to make it work with armadillo I need to make it generic.
*/

using namespace STASC;
int main()
{

  std::vector<State> systemTrajectory;
  constexpr size_t timeStepsCount = 1000;
  constexpr double stepSize = 0.1;
  std::vector<double> timeStepsVec(timeStepsCount);
  std::vector<double> position;

  // ODE
  {
    for (size_t i = 0; i < timeStepsCount; i++)
    {
      timeStepsVec.at(i) = i * stepSize;
    }

    ODE ode(SysFunc::sysFuncOne, SysFunc::initialStateOne, timeStepsVec);

    // Integrate and collect the state at each time point
    for (size_t i = 0; i < timeStepsCount - 1; i++)
    {
      ode.int_u_dt(STASC::IntegrationMode::ERK1);
      systemTrajectory.push_back(ode.getStateVector());
    }
    assert(systemTrajectory.size() == timeStepsCount - 1);
     for ( auto element : systemTrajectory)
        position.push_back(element.at(0).real());
    //   std::cout << element.at(0).real() << "\n"; // show the position.
  }

  // GNU Plot
  // std::cout << "position size: " << position.size() << std::endl;
  // std::cout << "time vector size: " << timeStepsVec.size() << std::endl;

  if(true){
    /* output */
    // to make sizes match.
    position.push_back(*position.cend()); // Duplicating the last element.
    Gnuplot gp;
    gp << "plot '-' using 1:2 with linespoint" << std::endl;
    gp.send1d(std::make_tuple(timeStepsVec, position));
  }

  return 0;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef NL_PROBLEM_SO3_HPP_
#define NL_PROBLEM_SO3_HPP_

#include <Eigen/Dense>
#include <cbr_math/lie/Tn.hpp>
#include <cbr_math/lie/group_product.hpp>
#include <sophus/so3.hpp>

/**
 * @brief Defines an optimal control problem on (X, V) \in SO(3) \timex R3 with three inputs
 *   d^r X_t = V
 *   d^r V_t = u
 */
struct SO3Problem
{
  using state_t = cbr::lie::GroupProduct<double, 0, Sophus::SO3, cbr::lie::T3>;
  using deriv_t = typename state_t::Tangent;
  using input_t = Eigen::Vector3d;

  static constexpr std::size_t nx = state_t::DoF;
  static constexpr std::size_t nu = input_t::SizeAtCompileTime;

  // Dynamics (must be differentiable so we make a generic template)
  template<typename T, typename Derived>
  auto get_f(const T & x, const Eigen::MatrixBase<Derived> & u) const
  {
    using Scalar = typename decltype(x.log() * u.transpose())::EvalReturnType::Scalar;

    Eigen::Matrix<Scalar, 6, 1> ret;
    ret.template segment<3>(0) = std::get<1>(x).translation();
    ret.template segment<3>(3) = u.eval();
    return ret;
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb.setConstant(-0.3);
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub.setConstant(0.3);
  }

  Eigen::Matrix<double, nx, nx> get_Q(double) const
  {
    return 0.1 * Eigen::Matrix<double, nx, nx>::Identity();
  }
  Eigen::Matrix<double, nx, nx> get_QT() const
  {
    return Eigen::Matrix<double, nx, nx>::Identity();
  }
  Eigen::Matrix<double, nu, nu> get_R(double) const
  {
    return 0.01 * Eigen::Matrix<double, nu, nu>::Identity();
  }
};

#endif  // NL_PROBLEM_SO3_HPP_