// // [[Rcpp::export()]]
void getVgamma_hierIRT(arma::cube &Vgamma,
					arma::mat &gammasigma,
					arma::mat &Ebb,
					const arma::mat &g,
					const arma::mat &i,
                    const arma::mat &j,
                    const arma::mat &z,
                    const int NL,
                    const int NG
                    ) {
				
	signed int m, l;

#pragma omp parallel for private(m,l)
  	for(m=0; m < NG; m++){

		Vgamma.slice(m) = inv_sympd(gammasigma);

		for(l=0; l < NL; l++){
			if( g(i(l,0),0)==m ) Vgamma.slice(m) += Ebb(j(l,0),0) * trans(z.row(i(l,0))) * z.row(i(l,0));
		}

		// Note this yield C_inv, not C_m
		Vgamma.slice(m) = inv_sympd(Vgamma.slice(m));


  	}  // for(k=0; n < NJ; k++){

    return; 
} 

// compute the gradients of f and g at all data points
int dtq::gradFGdata(arma::cube &gfd, arma::cube &ggd)
{
  for (int j=0; j<(ltvec-1); j++)
  {
    for (int l=0; l<numts; l++)
    {
      double xi = (*odata)(j,l);
      gfd.slice(l).col(j) = (*gradf)(xi,curtheta);
      ggd.slice(l).col(j) = (*gradg)(xi,curtheta);
    }
  }
  return 0;
}

//' @title
//' mStep
//' @description
//' Return the parameters' posterior.
//' 
//' @param DP
//' @param DataStorage
//' @param xi
//' @param zeta
//' @export
// [[Rcpp::export]]
arma::cube mStep(NumericVector prior, arma::cube posterior, NumericVector data, IntegerVector xi, IntegerVector zeta){
  //NumericVector data = DataStorage.slot("simulation");
  const int N = data.length();
  
  //arma::cube prior = DP.slot("prior");
  
  arma::mat mu = posterior.slice(0);
  arma::mat Nmat = posterior.slice(1);
  arma::mat v = posterior.slice(2);
  arma::mat vs2 = posterior.slice(3);
  
  std::fill(mu.begin(), mu.end(), prior[0]);
  std::fill(Nmat.begin(), Nmat.end(), prior[1]);
  std::fill(v.begin(), v.end(), prior[2]);
  std::fill(vs2.begin(), vs2.end(), prior[3]);
  
  int temp;
  double update_mu;
  double update_vs2;
  double update_n;
  double update_v;
  
  for(int n=0; n < N; n++){
    // if n is not NA
    if(data(n) == data(n)){
      temp = xi[n];
      const int l = temp - 1;
      temp = zeta[n];
      const int k = temp - 1;
      
      update_mu = (Nmat(l,k)*mu(l,k)+data(n))/(1+Nmat(l,k));
      update_vs2 = vs2(l,k) + (Nmat(l,k)*std::pow((mu(l,k)-data(n)),2)/(1+Nmat(l,k)));
      update_n = Nmat(l,k) + 1.0;
      update_v = v(l,k) + 1.0;
      mu(l,k) = update_mu;
      Nmat(l,k) = update_n;
      v(l,k) = update_v;
      vs2(l,k) = update_vs2;
    }
  }
  arma::cube toRet = posterior;
  toRet.slice(0) = mu;
  toRet.slice(1) = Nmat;
  toRet.slice(2) = v;
  toRet.slice(3) = vs2;
  
  //DP.slot("prior") = posterior;
  return toRet;
}

arma::cube get_Si(int n_states, arma::cube S, arma::mat m) {
    arma::cube Si(n_states, n_states, n_states);
    for (int i=0; i<n_states; i++) {
        Si.slice(i) = S.slice(i) * m;
    }
    return Si;
}

void internalBackward(const arma::mat& transition, const arma::cube& emission,
    const arma::ucube& obs, arma::cube& beta, const arma::mat& scales, unsigned int threads) {

#pragma omp parallel for if(obs.n_slices >= threads) schedule(static) num_threads(threads) \
default(none) shared(beta, scales, obs, emission,transition)
  for (unsigned int k = 0; k < obs.n_slices; k++) {
    beta.slice(k).col(obs.n_cols - 1).fill(scales(obs.n_cols - 1, k));
    for (int t = obs.n_cols - 2; t >= 0; t--) {
        arma::vec tmpbeta = beta.slice(k).col(t + 1);
        for (unsigned int r = 0; r < obs.n_rows; r++) {
          tmpbeta %= emission.slice(r).col(obs(r, t + 1, k));
        }
        beta.slice(k).col(t) =  transition * tmpbeta * scales(t, k);
    }
  }
}

// build the big matrix of initial conditions
// and the gradients of those initial conditions!
int dtq::phatinitgrad(arma::mat &phatI, arma::cube &phatG, const arma::cube &gfd, const arma::cube &ggd)
{
  double myh12 = sqrt(myh);
  for (int j=0; j<(ltvec-1); j++)
  {
    // go through each particular initial condition at this time
    // and make a Gaussian
    for (int l=0; l<numts; l++)
    {
      double xi = (*odata)(j,l);
      double mu = xi + ((*f)(xi,curtheta))*myh;
      double gval = (*g)(xi,curtheta);
      double sig = gval*myh12;
      arma::vec thisphat = gausspdf(yvec,mu,sig);
      phatI.col(j) += thisphat;
      for (int i=0; i<curtheta.n_elem; i++)
      {
        arma::vec pgtemp = (yvec - mu)*gfd(i,j,l)/(gval*gval);
        pgtemp -= ggd(i,j,l)/gval;
        pgtemp += arma::pow(yvec - mu,2)*ggd(i,j,l)/(myh*gval*gval*gval);
        phatG.slice(i).col(j) += pgtemp % thisphat;
      }
    }
  }
  phatI = phatI / numts;
  phatG = phatG / numts;
  return 0;
}

// Bellman recursion using row rearrangement
//[[Rcpp::export]]
Rcpp::List Bellman(const arma::mat& grid,
                   Rcpp::NumericVector reward_,
                   const arma::cube& scrap,
                   Rcpp::NumericVector control_,
                   const arma::cube& disturb,
                   const arma::vec& weight) {
  // Passing R objects to C++
  const std::size_t n_grid = grid.n_rows;
  const std::size_t n_dim = grid.n_cols;
  const arma::ivec r_dims = reward_.attr("dim");
  const std::size_t n_pos = r_dims(3);
  const std::size_t n_action = r_dims(2);
  const std::size_t n_dec = r_dims(4) + 1;
  const arma::cube
      reward(reward_.begin(), n_grid, n_dim * n_action * n_pos, n_dec - 1, false);
  const arma::ivec c_dims = control_.attr("dim");
  arma::cube control2;
  arma::imat control;
  bool full_control;
  if (c_dims.n_elem == 3) {
    full_control = false;
    arma::cube temp_control2(control_.begin(), n_pos, n_action, n_pos, false);
    control2 = temp_control2;
  } else {
    full_control = true;
    arma::mat temp_control(control_.begin(), n_pos, n_action, false);
    control = arma::conv_to<arma::imat>::from(temp_control);
  }
  const std::size_t n_disturb = disturb.n_slices;
  // Bellman recursion
  arma::cube value(n_grid, n_dim * n_pos, n_dec);
  arma::cube cont(n_grid, n_dim * n_pos, n_dec - 1, arma::fill::zeros);
  arma::mat d_value(n_grid, n_dim);
  Rcpp::Rcout << "At dec: " << n_dec - 1 << "...";
  for (std::size_t pp = 0; pp < n_pos; pp++) {
    value.slice(n_dec - 1).cols(n_dim * pp, n_dim * (pp + 1) - 1) =
        scrap.slice(pp);
  }
  for (int tt = (n_dec - 2); tt >= 0; tt--) {
    Rcpp::Rcout << tt;
    // Approximating the continuation value
    for (std::size_t pp = 0; pp < n_pos; pp++) {
      cont.slice(tt).cols(n_dim * pp, n_dim * (pp + 1) - 1) =
          Expected(grid,
                   value.slice(tt + 1).cols(pp * n_dim, n_dim * (pp + 1) - 1),
                   disturb, weight);
    }
    Rcpp::Rcout << "..";
    // Optimise value function
    if (full_control) {
      BellmanOptimal(grid, control, value, reward, cont, tt);
    } else {
      BellmanOptimal2(grid, control2, value, reward, cont, tt);
    }
    Rcpp::Rcout << ".";
  }
  return Rcpp::List::create(Rcpp::Named("value") = value,
                            Rcpp::Named("expected") = cont);
}

arma::mat cvt_rgb2gray(const arma::cube &image) {
  arma::vec scale = { 0.3, 0.6, 0.1 };
  arma::mat new_image = arma::zeros<arma::mat>(image.n_rows, image.n_cols);
  for (arma::uword i = 0; i < image.n_slices; i++) {
    new_image += scale(i) * image.slice(i); // weighted construction
  }
  return new_image;
}

// // [[Rcpp::export()]]
void getVb2_dynIRT(arma::cube &Vb2,
				 const arma::cube &Ex2x2,
                 const arma::mat &sigma,
                 const int T
                 ) {

	int t;

#pragma omp parallel for
	for(t=0; t<T; t++){

		Vb2.slice(t) = inv_sympd(inv_sympd(sigma) + Ex2x2.slice(t)) ;

	}

    return;

}

/*
 * approx_model:  Gaussian approximation of the original model
 * t:             Time point where the weights are computed
 * alpha:         Simulated particles
 */
arma::vec ung_ssm::log_weights(const ugg_ssm& approx_model,
  const unsigned int t, const arma::cube& alpha) const {
  
  arma::vec weights(alpha.n_slices, arma::fill::zeros);
  
  if (arma::is_finite(y(t))) {
    switch(distribution) {
    case 0  :
      for (unsigned int i = 0; i < alpha.n_slices; i++) {
        double simsignal = alpha(0, t, i);
        weights(i) = -0.5 * (simsignal + std::pow(y(t) / phi, 2.0) * std::exp(-simsignal)) +
          0.5 * std::pow((approx_model.y(t) - simsignal) / approx_model.H(t), 2.0);
      }
      break;
    case 1  :
      for (unsigned int i = 0; i < alpha.n_slices; i++) {
        double simsignal = arma::as_scalar(Z.col(t * Ztv).t() *
          alpha.slice(i).col(t) + xbeta(t));
        weights(i) = y(t) * simsignal  - u(t) * std::exp(simsignal) +
          0.5 * std::pow((approx_model.y(t) - simsignal) / approx_model.H(t), 2.0);
      }
      break;
    case 2  :
      for (unsigned int i = 0; i < alpha.n_slices; i++) {
        double simsignal = arma::as_scalar(Z.col(t * Ztv).t() *
          alpha.slice(i).col(t) + xbeta(t));
        weights(i) = y(t) * simsignal - u(t) * std::log1p(std::exp(simsignal)) +
          0.5 * std::pow((approx_model.y(t) - simsignal) / approx_model.H(t), 2.0);
      }
      break;
    case 3  :
      for (unsigned int i = 0; i < alpha.n_slices; i++) {
        double simsignal = arma::as_scalar(Z.col(t * Ztv).t() *
          alpha.slice(i).col(t) + xbeta(t));
        weights(i) = y(t) * simsignal - (y(t) + phi) *
          std::log(phi + u(t) * std::exp(simsignal)) +
          0.5 * std::pow((approx_model.y(t) - simsignal) / approx_model.H(t), 2.0);
      }
      break;
    }
  }
  return weights;
}

// how to subset outer dimension of arma cube by IntegerVector
// [[Rcpp::export]]
arma::cube subsetCube(arma::cube data, IntegerVector index){
  if(data.n_slices != index.size()){ //informative error message
    Rcout << "subsetCube requires an array and index of the same outer dimension!" << std::endl;
  }
  arma::cube out = arma::zeros(data.n_rows,data.n_cols,data.n_slices);
  for(int i=0; i<data.n_slices; i++){
    out.slice(i) = data.slice(index(i));
  }
  return(out);
}

// @title Gradient step for regression coefficient
// @param X The ratings matrix. Unobserved entries must be marked NA. Users
// must be along rows, and tracks must be along columns.
// @param P The learned user latent factors.
// @param Q The learned track latent factors.
// @param beta The learned regression coefficients.
// @param lambda The regularization parameter for beta.
// @param gamma The step-size in the gradient descent.
// @return The update regression coefficients.
arma::vec update_beta(arma::mat X, arma::cube Z, arma::mat P, arma::mat Q,
		      arma::vec beta, double lambda, double gamma) {
  arma::uvec obs_ix = arma::conv_to<arma::uvec>::from(arma::find_finite(X));
  arma::mat resid = X - P * Q.t() - cube_multiply(Z, beta);
  arma::vec beta_grad = arma::zeros(beta.size());
  for(int l = 0; l < beta.size(); l++) {
    beta_grad[l] = accu(resid(obs_ix) % Z.slice(l)(obs_ix));
  }
  beta_grad = 2 * (lambda * beta - beta_grad);
  return beta - gamma * beta_grad;
}

// **********************************************************//
//                     Calculate mu matrix                   //
// **********************************************************//
// [[Rcpp::export]]
arma::mat mu_cpp (arma::cube Y, arma::rowvec eta) {
	int D = Y.n_rows;
	int A = Y.n_cols;
	int Q = Y.n_slices;
	arma::mat mu = arma::zeros(D, A);
	for (unsigned int d = 0; d < D; d++) {
		for (unsigned int a = 0; a < A; a++) {
			arma::vec Y_da = Y.subcube(d, a, 0, d, a, Q-1);
			mu(d, a) = sum(eta * Y_da);
		}
	}
	return mu;
}

NumericVector logLikMixHMM(const arma::mat& transition, const arma::cube& emission,
  const arma::vec& init, const arma::ucube& obs, const arma::mat& coef, const arma::mat& X,
  const arma::uvec& numberOfStates, unsigned int threads) {
  
  arma::mat weights = exp(X * coef).t();
  if (!weights.is_finite()) {
    return wrap(-arma::datum::inf);
  }
  weights.each_row() /= sum(weights, 0);
  
  arma::vec ll(obs.n_slices);
  arma::sp_mat transition_t(transition.t());
#pragma omp parallel for if(obs.n_slices >= threads) schedule(static) num_threads(threads) \
  default(none) shared(ll, obs, weights, init, emission, transition_t, numberOfStates)
    for (unsigned int k = 0; k < obs.n_slices; k++) {
      arma::vec alpha = init % reparma(weights.col(k), numberOfStates);
      
      for (unsigned int r = 0; r < obs.n_rows; r++) {
        alpha %= emission.slice(r).col(obs(r, 0, k));
      }
      
      double tmp = sum(alpha);
      ll(k) = log(tmp);
      alpha /= tmp;
      
      for (unsigned int t = 1; t < obs.n_cols; t++) {
        alpha = transition_t * alpha;
        for (unsigned int r = 0; r < obs.n_rows; r++) {
          alpha %= emission.slice(r).col(obs(r, t, k));
        }
        
        tmp = sum(alpha);
        ll(k) += log(tmp);
        alpha /= tmp;
      }
    }
    return wrap(ll);
}

// **********************************************************//
//                     Calculate lambda list                 //
// **********************************************************//
// [[Rcpp::export]]
arma::mat lambda_cpp (arma::cube X_d, arma::rowvec beta) {
	int A = X_d.n_rows;
	int P = X_d.n_slices;
	arma::mat lambda_d = arma::zeros(A, A);
	for (unsigned int a = 0; a < A; a++) {
		for (unsigned int r = 0; r < A; r++) {
			if (r != a) {
				arma::vec X_dar = X_d.subcube(a, r, 0, a, r, P-1);
				lambda_d(a,r) = sum(beta * X_dar);
			}
		}
	}
	return lambda_d;
}

arma::mat exact_trans2(arma::cube joint_means_trans, Rcpp::List eigen_decomp, double time_int, arma::ivec absorb_states, int start_state, int end_state, int exact_time_index){
	arma::mat rate_matrix=Rcpp::as<arma::mat>(eigen_decomp["rate"]);
	arma::mat out=arma::zeros<arma::mat>(rate_matrix.n_rows,rate_matrix.n_rows);
	
	arma::mat temp=arma::zeros<arma::mat>(rate_matrix.n_rows,rate_matrix.n_rows);
	
	int i=start_state-1;
	int j=end_state-1;
	int k=0;  
	
	bool i_in_A=0;
	bool j_in_A=0;
	
	//std::cout<<absorb_states;
	
	while(i_in_A==0 && k<absorb_states.size()){
		int test=absorb_states[k]-1;
		if(test==i){
			i_in_A=1;
		}
		k++;
	}
	
	k=0;
	while(j_in_A==0 && k<absorb_states.size()){
		int test=absorb_states[k]-1;
		if(test==j){
			j_in_A=1;
		}
		k++;
	}
	
	int in_either=i_in_A+j_in_A;

	
	if(in_either==0){
		for(int l=0;l<absorb_states.size();l++){
			int absorb_state=absorb_states[l]-1;
			temp.col(absorb_state)=rate_matrix.col(absorb_state);
		}
		out=joint_means_trans.slice(exact_time_index)*temp;
	}
	if(i_in_A==0 && j_in_A==1){
		arma::mat prob_mat=mat_exp_eigen_cpp(eigen_decomp,time_int);
		out.col(j)=prob_mat.col(i)*rate_matrix(i,j);
	}
	
	
	return(out);
}	

void CNN<
LayerTypes, OutputLayerType, InitializationRuleType, PerformanceFunction
>::Predict(arma::cube& predictors, arma::mat& responses)
{
  deterministic = true;

  arma::mat responsesTemp;
  ResetParameter(network);
  Forward(predictors.slices(0, sampleSize - 1), network);
  OutputPrediction(responsesTemp, network);

  responses = arma::mat(responsesTemp.n_elem, predictors.n_slices);
  responses.col(0) = responsesTemp.col(0);

  for (size_t i = 1; i < (predictors.n_slices / sampleSize); i++)
  {
    Forward(predictors.slices(i, (i + 1) * sampleSize - 1), network);

    responsesTemp = arma::mat(responses.colptr(i), responses.n_rows, 1, false,
        true);
    OutputPrediction(responsesTemp, network);
    responses.col(i) = responsesTemp.col(0);
  }
}

//[[Rcpp::export]]
arma::cube PathDisturb(const arma::vec& start,
                       Rcpp::NumericVector disturb_) {
  // R objects to C++
  const arma::ivec d_dims = disturb_.attr("dim");
  const std::size_t n_dim = d_dims(0);
  const std::size_t n_dec = d_dims(3) + 1;
  const std::size_t n_path = d_dims(2);
  const arma::cube disturb(disturb_.begin(), n_dim, n_dim * n_path, n_dec - 1,
			  false);
  // Simulating the sample paths
  arma::cube path(n_path, n_dim, n_dec);
  // Assign starting values
  for (std::size_t ii = 0; ii < n_dim; ii++) {
    path.slice(0).col(ii).fill(start(ii));
  }
  // Disturb the paths
  for (std::size_t pp = 0; pp < n_path; pp++) {
    for (std::size_t tt = 1; tt < n_dec; tt++) {
      path.slice(tt).row(pp) = path.slice(tt - 1).row(pp) *
          arma::trans(disturb.slice(tt - 1).cols(n_dim * pp, n_dim * (pp + 1) - 1));
    }
  }
  return path;
}

// Expected value using row rearrangement
//[[Rcpp::export]]
arma::mat Expected(const arma::mat& grid,
                   const arma::mat& value,
                   const arma::cube& disturb,
                   const arma::vec& weight) {
  // Passing R objects to C++
  const std::size_t n_grid = grid.n_rows;
  const std::size_t n_dim = grid.n_cols;
  const std::size_t n_disturb = disturb.n_slices;
  // Computing the continuation value function
  arma::mat continuation(n_grid, n_dim, arma::fill::zeros);
  arma::mat d_value(n_grid, n_dim);
  for (std::size_t dd = 0; dd < n_disturb; dd++) {
    d_value = value * disturb.slice(dd);
    continuation += weight(dd) * Optimal(grid, d_value);
  }
  return continuation;
}

//--------------------------------------------------------------------------------------------------
double Krigidx( const arma::colvec& KF,
                 const arma::colvec& comb,
                 const arma::mat& X,
                 const arma::cube& Gamma ) {
  int k;
  int n = X.n_rows;
  int c = comb.size();
  double S;
  
  arma::mat W = arma::ones( n, n );
    
  for ( k = 0; k < c; k++ ) {
    W = W % Gamma.slice( comb( k ) - 1 );
  }
  S = as_scalar( KF.t() * W * KF );

  return S;
}