void Force_iid_model::update(arma::colvec& Y,const arma::colvec3& force, const arma::colvec3& torque) {

    features(contact) = gaussian_step_function_positive(arma::norm(force),0.9,1,beta);

    features(up)      = gaussian_step_function_positive(force(0),limits[contact].lower,limits[contact].upper,var[contact]);   //+

    features(down)    = gaussian_step_function_negative(force(0),limits[down].lower,limits[down].upper,var[down]);       //-
    features(left)    = gaussian_step_function_negative(force(1),limits[left].lower,limits[left].upper,var[left]);       // -

    features(right)   = gaussian_step_function_positive(force(1),limits[right].lower,limits[right].upper,var[right]);    // +

    features(push)    = gaussian_step_function_negative(force(2),limits[push].lower,limits[push].upper,var[push]);       //-

    //contact,up,down,left,right,push

    features_simple(0) = features(contact);
    features_simple(1) = arma::max(features(arma::span(1,4)));


    if(type == FULL) {
        Y.resize(6);
        Y(contact)  = features(contact);
        Y(up)       = features(up);
        Y(down)     = features(down);
        Y(left)     = features(left);
        Y(right)    = features(right);
        Y(push)     = features(push);
    } else {
        Y.resize(2);
        Y(0)        = features_simple(0);
        Y(1)        = features_simple(1);
    }
}

// [[Rcpp::export]]
double ksFastTestStatistic(arma::colvec y, arma::colvec z){
	uvec trt = find( z == 1 );
	uvec ctrl = find( z == 0 );
	vec yT = y.elem(trt);
	vec	yC = y.elem(ctrl);
	// assuming no missing vals	yT = yT[!is.na(yT)];
	double nX = yT.n_rows; //length(yT);
	double nY = yC.n_rows;
	double n = nX * nY/(nX + nY);
	colvec w = join_cols(yT, yC); // c(yT,yC)
	uvec orderedw = sort_index(w); //order(w)
	int totalN = nX + nY;
	colvec d(totalN);
	d.fill(1/nX);
	d.elem( find( orderedw > nX ) ).fill(-1/nY);
	// d = cumsum(Rcpp::ifelse(orderedw <= nX, 1/nX, -1/nY));
	colvec F = cumsum(d);
	// check for ties
	vec uniqF = unique(w);
	vec out = F;
	if ( uniqF.n_elem < totalN ){
		//uvec nonzerodiffs = find( diff(sort(F)) != 0 );
		vec Fnonzerodiffs = F.elem( find( diff(sort(F)) != 0 ) );
		vec out = Fnonzerodiffs; // I dont know why I cant use join_cols (Fnonzerodiffs,F(totalN-1))
		out.insert_rows(1,F(totalN-1)); // z <- z[c(which(diff(sort(w)) != 0), n.x + n.y)]
		}
	// return(max(abs(out)));
	return(max(abs(out)));
}

double get_xi_sqr(arma::colvec &x, arma::mat &post_sigma, arma::colvec &post_mu){
  arma::mat v = x.t()*post_sigma*x;
  arma::mat xmu = x.t()*post_mu;
  std::cout << "x.t()*post_sigma*x = " << std::endl;
  v.print();
  std::cout << "x.t()*post_mu = " << std::endl;
  xmu.print();
  return as_scalar(v) + as_scalar(xmu)*as_scalar(xmu);
}

bool CamAruco2DObservationModel::hasClearLineOfSight(const ompl::base::State *state, const arma::colvec& landmark )
{

    using namespace arma;

    colvec xVec = state->as<SE2BeliefSpace::StateType>()->getArmaData();

    colvec robot_to_landmark_ray =  landmark.subvec(1,2) - xVec.subvec(0,1);

    double distance = norm(robot_to_landmark_ray,2);

    int steps = std::floor(distance/ompl::magic::ONE_STEP_DISTANCE_FOR_VISIBILITY);

    ompl::base::State *tempState = this->si_->allocState();

    for(int i=1 ; i < steps; i++)
    {
        double newX = xVec(0) + i*robot_to_landmark_ray(0)/steps;

        double newY = xVec(1) + i*robot_to_landmark_ray(1)/steps;

        tempState->as<SE2BeliefSpace::StateType>()->setXYYaw(newX, newY,0);

        if(!this->si_->isValid(tempState))
        {
            return false;
        }

    }

    si_->freeState(tempState);

    return true;
}

// //' 	Self-concordant Taylor approximation logstar function
// //'
// //' 	Minus log and its first \code{der} derivatives, on \code{eps < x < M}, with
// //' 	fourth order Taylor series approximation to the left of \code{eps} and to the right of \code{M}
// //'
// //' 	@param x column vector of observations
// //' 	@param eps lower tolerance
// //' 	@param M maximum value
// //' 	@param der derivative, either 0, 1 or 2.
// //'
// //' 	@return a matrix with \eqn{der+1} columns containing values
arma::mat mllog(arma::colvec x, double eps, double M, int der=0){
  if(!(der==0 || der==1 || der==2)){
    Rcpp::stop("Only first two derivatives and log functions implemented");
  }
  if(eps > M){
    Rcpp::stop("Thresholds out of order");
  }
  //Coefficients for 4th order Taylor approx below eps
  arma::vec coefs(5);
  arma::vec Coefs(5);
  coefs(0) = -log(eps);
  Coefs(0) = -log(M);
  for(int i=1; i<5; i++){
    coefs(i) = R_pow(-eps,-i)/(double)i;
    Coefs(i) = R_pow(-M,-i)/(double)i;
  }

  arma::uvec lo = find(x < eps);
  arma::uvec hi = find(x > M);

  //Function value
  arma::vec f(x.size());
  f = -log(x);
  f(lo) = h(x(lo)-eps, coefs);
  f(hi) = h(x(hi)-M,   Coefs);

  if(der < 1){
    return f;
  }
  //First derivative

  arma::vec fp(x.size());
  fp = -1.0/x;
  fp(lo) = hp(x(lo)-eps, eps);
  fp(hi) = hp(x(hi)-M,   M);

  if(der < 2){
    return join_rows(f, fp);
  }
  //Second derivative
  arma::vec fpp(x.size());
  fpp = pow(x,-2);
  fpp(lo) = hpp(x(lo)-eps, eps);
  fpp(hi) = hpp(x(hi)-M,   M);

  return join_rows(join_rows(f,fp),fpp);
}

arma::colvec computeBetaJ_cpp(arma::colvec y, arma::mat X, int j, int m, int adjFinSample) {
  int bigT = X.n_rows;
  int numThrow = ceil((double)m / 2);
  if (adjFinSample == 0) numThrow = m ;
  mat XSparse; colvec ySparse;
  XSparse.zeros(bigT - numThrow, X.n_cols); ySparse.zeros(bigT - numThrow);
  if (j < 2) {
    XSparse.rows(0, bigT - numThrow - 1) = X.rows(numThrow, bigT - 1);
    ySparse.rows(0, bigT - numThrow - 1) = y.rows(numThrow, bigT - 1);
  } else {
    XSparse.rows(0, j - 2) = X.rows(0, j - 2); ySparse.rows(0, j - 2) = y.rows(0, j - 2);
    XSparse.rows(j - 1, bigT - numThrow - 1) = X.rows(j + numThrow - 1, bigT - 1);
    ySparse.rows(j - 1, bigT - numThrow - 1) = y.rows(j + numThrow - 1, bigT - 1);
  }
  colvec beta = runOLSFast_cpp(ySparse, XSparse);
  return(beta);
}

void GMM::expection(arma::colvec& x) const{
    x.zeros();
    for(std::size_t k = 0; k < K;k++){
        x += Means[k] * pi(k);
    }


}

void CheckActivationCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the activation function using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::fn(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

void CheckDerivativeCorrect(const arma::colvec input, const arma::colvec target)
{
  // Test the calculation of the derivatives using a single value as input.
  for (size_t i = 0; i < target.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::deriv(input.at(i)),
        target.at(i), 1e-3);
  }

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;
  ActivationFunction::deriv(input, derivatives);
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}

//[[Rcpp::export]]
double computeSStat_cpp(arma::colvec beta, arma::mat invS, arma::colvec y, arma::mat X,
                        int j, int m) {
  mat newX = X.rows(j - 1, j + m - 2); colvec newY = y.rows(j - 1, j + m - 2);
  mat A = trans(newX) * invS * (newY - newX * beta);
  mat V =  trans(newX) * invS * newX;
  mat S = trans(A) * inv(V) * A;
  double s = S(0, 0);
  return(s);
}

double nLL2(arma::colvec par, arma::colvec Data, int Nf)  {		
double failcomp=0.0,suscomp=0.0;		
arma::colvec F=Data.rows(0,Nf-1);		
for(int i=0; i<Nf; i++)  {		
failcomp=failcomp+R::dlnorm(F(i),par(0),par(1),1);		
}		
failcomp=-failcomp;		
int Nd=Data.n_rows;		
arma::colvec S;		
if(Nd>Nf)  {		
	 S=Data.rows(Nf,Nd-1);	
	for(int i=0; i<(Nd-Nf); i++)  {	
	suscomp=suscomp+R::plnorm(S(i),par(0),par(1),0,1);	
	}	
}		
double negLL=failcomp-suscomp;		
return negLL;		
}		

//--------------------------------------------------------------------------------------------------
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;
}

int classify(double xi, arma::mat &sigma, arma::mat &sigma_inv, arma::colvec &x, arma::colvec &mu){
  arma::mat sigma_t_inv = get_post_sigma_inv(xi, x, sigma_inv);
  arma::mat sigma_t = sigma_t_inv.i();
  arma::colvec mu_t; 

  int s = 0;
  mu_t = get_post_mu(x, sigma_inv, sigma_t, mu, s);
  
  double log_0 = log(sigmoid(xi)) - xi/2 - lambda(xi)*xi*xi - 0.5*as_scalar(mu.t()*sigma_inv*mu) + 0.5*as_scalar(mu_t.t()*sigma_t.t()*mu_t) + 0.5*log(det(sigma_t)/det(sigma));

  s = 1;
  mu_t = get_post_mu(x, sigma_inv, sigma_t, mu, s);
  
  double log_1 = log(sigmoid(xi)) - xi/2 - lambda(xi)*xi*xi - 0.5*as_scalar(mu.t()*sigma_inv*mu) + 0.5*as_scalar(mu_t.t()*sigma_t.t()*mu_t) + 0.5*log(det(sigma_t)/det(sigma));

  if(log_1 > log_0) return 1;
  else return 0;

}

void CheckInverseCorrect(const arma::colvec input)
{
    // Test the calculation of the inverse using a single value as input.
  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(ActivationFunction::inv(ActivationFunction::fn(
        input.at(i))), input.at(i), 1e-3);
  }

  // Test the calculation of the inverse using the entire vector as input.
  arma::colvec activations;
  ActivationFunction::fn(input, activations);
  ActivationFunction::inv(activations, activations);

  for (size_t i = 0; i < input.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), input.at(i), 1e-3);
  }
}

arma::mat computeCovMatForAndrews_cpp(arma::colvec u, int breakDate) {
 int bigT = u.n_rows;
 int m = bigT - breakDate;
 mat totSum =zeros(m, m);
 for (int j = 0; j < (breakDate + 1); j++) {
   colvec uUsed = u.rows(j, j + m - 1);
   totSum = totSum + uUsed * trans(uUsed);
 }
 mat Sigma = totSum / (breakDate + 1);
 return(Sigma);
}

/*
 * Implementation of the PReLU activation function test. The function
 * is implemented as PReLU layer in the file perametric_relu.hpp
 *
 * @param input Input data used for evaluating the PReLU activation
 *   function.
 * @param target Target data used to evaluate the PReLU activation.
 */
void CheckPReLUActivationCorrect(const arma::colvec input,
                                          const arma::colvec target)
{
  PReLU<> prelu;

  // Test the activation function using the entire vector as input.
  arma::colvec activations;
  prelu.Forward(std::move(input), std::move(activations));
  for (size_t i = 0; i < activations.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
  }
}

arma::colvec computeTestStatDistribution_cpp(arma::colvec y, arma::mat X, int breakDate,
                                             int m, int adjFinSample, arma::mat invS) {
  int bigT = X.n_rows;
  colvec sJ = zeros(breakDate - m + 1);
  for (int j = 0; j < (breakDate - m + 1); j++) {
    colvec betaJ = computeBetaJ_cpp(y.rows(0, bigT - 1 - m), X.rows(0, bigT - 1 - m),
                                    j + 1, m, adjFinSample);
    sJ(j) = computeSStat_cpp(betaJ, invS, y, X, j + 1, m);
//     if (j == 44) {
//       Rcout << "betaJ = "<< betaJ << "sJ(j) = " << sJ(j) << endl;
//     }
  }
  return(sJ);
}

int CamAruco2DObservationModel::findCorrespondingLandmark(const ompl::base::State *state, const arma::colvec &observedLandmark, arma::colvec &candidateObservation)
{
    using namespace arma;

    int landmarkID = observedLandmark[0];

    double maxLikelihood = -1.0;

    double candidatelandmarkRange =0, candidatelandmarkBearing = 0;

    int candidateIndx = -1;

    for(unsigned int i = 0; i < landmarks_.size() ; i++)
    {
        if(landmarks_[i](0) == landmarkID)
        {
            double landmarkRange =0, landmarkBearing = 0;

            // get range and bearing to landmark
            this->calculateRangeBearingToLandmark(state, landmarks_[i], landmarkRange,landmarkBearing);

            arma::colvec prediction;

            prediction<<landmarkRange<<landmarkBearing<<endr;

            // calculate the likelihood
            double lkhd = getDataAssociationLikelihood(observedLandmark.subvec(1,2), prediction);

            if(lkhd > maxLikelihood)
            {
                candidateIndx = i;
                maxLikelihood = lkhd;
                candidatelandmarkRange = landmarkRange;
                candidatelandmarkBearing = landmarkBearing;
            }
        }

    }

    assert(candidateIndx >= 0 && "Candidate index cannot be negative");

    // The observation is id, range, bearing, orientation of the landmark
    candidateObservation<<landmarkID<<candidatelandmarkRange<<candidatelandmarkBearing<<landmarks_[candidateIndx][3]<<endr;

    assert(candidateIndx>=0);

    return candidateIndx;

}

double weight_pow_dist( const arma::colvec& x, 
                        const arma::colvec& y, 
                        const arma::colvec& w, 
                        const arma::colvec& p ) {
  
  double d = 0.0;
  
  if ( x.size() > 0 && x.size() == y.size() && y.size() == w.size() && w.size() == p.size() ) {
    int i, n;
    n = x.size();
    #pragma omp parallel for shared( w, x, y, p ) private( i ) reduction(+:d)
    for( i = 0; i < n; i++ ) {
      d += w(i) * std::pow( std::abs( x(i) - y(i) ), p(i) );
    }
  }
  return d;
}

/*
 * Implementation of the PReLU activation function derivative test.
 * The function is implemented as PReLU layer in the file
 * perametric_relu.hpp
 *
 * @param input Input data used for evaluating the PReLU activation
 *   function.
 * @param target Target data used to evaluate the PReLU activation.
 */
void CheckPReLUDerivativeCorrect(const arma::colvec input,
                                          const arma::colvec target)
{
  PReLU<> prelu;

  // Test the calculation of the derivatives using the entire vector as input.
  arma::colvec derivatives;

  // This error vector will be set to 1 to get the derivatives.
  arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
  prelu.Backward(std::move(input), std::move(error), std::move(derivatives));
  for (size_t i = 0; i < derivatives.n_elem; i++)
  {
    BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
  }
}