/* Uniform rejection sampling */
static R_INLINE double urs_a_b(double a, double b) {
  SAMPLER_DEBUG("urs_a_b", a, b);
  const double phi_a = dnorm(a, 0.0, 1.0, FALSE);
  double x = 0.0, u = 0.0;

  /* Upper bound of normal density on [a, b] */
  const double ub = a < 0 && b > 0 ? M_1_SQRT_2PI : phi_a;
  do {
    x = runif(a, b);
  } while (runif(0, 1) * ub > dnorm(x, 0, 1, 0));
  return x;
}

inline T math_rand_normal(RNGType &rng)
{
    mckl::UniformRealDistribution<T> runif(
        static_cast<T>(-1e4), static_cast<T>(1e4));

    T f = runif(rng);
    if (f > 0)
        f += std::numeric_limits<T>::min();
    else
        f -= std::numeric_limits<T>::min();

    return f;
}

void wsrewire_R(double *gi, double *go, double *pn, double *pnv, double *pp)
/*Perform a Watts-Strogatz rewiring process on the adjacency array pointed
to by *gi, storing the results in *go.  It is assumed that gi contains a 
*pn x *pnv *pnv array, whose non-null dyads are rewired (symmetrically) with
uniform probability *pp.  *go should be a copy of *gi.*/
{
  long int n,nv,i,j,k,h,t;
  double p,tempht,tempth;
  char flag;
  
  /*Take care of preliminaries*/
  n=(long int)*pn;
  nv=(long int)*pnv;
  p=*pp;
  GetRNGstate();

  /*Rewire the array*/
  for(i=0;i<n;i++){
    for(j=0;j<nv;j++){
      for(k=j+1;k<nv;k++){
        /*If the original dyad is non-null, rewire it w/prob p*/
        if(((gi[i+j*n+k*n*nv]!=0.0)||(gi[i+j*n+k*n*nv]!=0.0)) &&(runif(0.0,1.0)<p)){
          flag=0;
          while(!flag){
            t=j;  /*Save the head, tail*/
            h=k;
            if(runif(0.0,1.0)<0.5){   /*Switch head or tail w/50% prob*/
              h=(long int)floor(runif(0.0,1.0)*nv);
              if((h!=j)&&(h!=k)&&(go[i+t*n+h*n*nv]==0.0)&& (go[i+h*n+t*n*nv]==0.0)) /*Is h legal?*/
                flag++;
            }else{
              t=(long int)floor(runif(0.0,1.0)*nv);
              if((t!=j)&&(t!=k)&&(go[i+t*n+h*n*nv]==0.0)&& (go[i+h*n+t*n*nv]==0.0)) /*Is t legal?*/
                flag++;
            }
          }
          /*Swap the dyad states*/
          tempth=go[i+t*n+h*n*nv];
          tempht=go[i+h*n+t*n*nv];
          go[i+t*n+h*n*nv]=go[i+j*n+k*n*nv];
          go[i+h*n+t*n*nv]=go[i+k*n+j*n*nv];
          go[i+j*n+k*n*nv]=tempth;
          go[i+k*n+j*n*nv]=tempht;
        }
      }
    }    
  }
  /*Reset the random number generator*/
  PutRNGstate();
}

  //----------------------------------------------------------------------
  // driver function to draw a single element of the correlation
  // matrix conditional on the variances.
  void SepStratSampler::draw_R(int i, int j){
    i_ = i;
    j_ = j;

    double oldr = R_(i,j);
    double slice = logp_slice_R(oldr) - rexp();
    find_limits();
    double rcand = runif(lo_, hi_);
    while(logp_slice_R(rcand) < slice && hi_ > lo_){
      if(rcand > oldr) hi_ = rcand;
      else lo_ = rcand;
      rcand = runif(lo_,hi_);
    }
    set_R(rcand);
  }

void predictInterp(double *alpha, double *lambda, double *beta, double *predictPositions, int *NpredictPositions, double *diffPositionj, double *currPositionsj, double *currPositionsjp1, double *thetaj, double *thetajp1, double *predvals) {
  // Runs the prediction code when we are interpolating between two positions
  int Nd = rpois((*lambda)*(*diffPositionj));
  int i;
  double depthEvents[Nd];
  for(i=0;i<Nd;i++) depthEvents[i] = runif(*currPositionsj,*currPositionsjp1);
  R_rsort(depthEvents,Nd);
  double timeEventsUnsc[Nd+1],timeEventsSum=0.0;
  for(i=0;i<Nd+1;i++) timeEventsUnsc[i] = rgamma(*alpha,1/(*beta));
  for(i=0;i<Nd+1;i++) timeEventsSum += timeEventsUnsc[i];
  double timeEvents[Nd+1];
  for(i=0;i<Nd+1;i++) timeEvents[i] = (*thetajp1-*thetaj)*timeEventsUnsc[i]/timeEventsSum;
  double timeEventsCumsum[Nd+1],allTimeEvents[Nd+2];
  timeEventsCumsum[0] = 0.0;
  for(i=1;i<Nd+1;i++) timeEventsCumsum[i] = timeEventsCumsum[i-1] + timeEvents[i];
  for(i=0;i<Nd+1;i++) allTimeEvents[i] = timeEventsCumsum[i]+*thetaj;
  allTimeEvents[Nd+1] = *thetajp1;
  double allDepthEvents[Nd+2];
  allDepthEvents[0] = *currPositionsj;
  for(i=1;i<Nd+1;i++) allDepthEvents[i] = depthEvents[i-1];
  allDepthEvents[Nd+1] = *currPositionsjp1;
  
  int Ndp2 = Nd+2;
  for(i=0;i<*NpredictPositions;i++) {
    linInterp(&Ndp2,&predictPositions[i],allDepthEvents,allTimeEvents,&predvals[i]);
  }
}

void leftTruncNorm(double *mu, double *sigma2, double *x){
 int check1, check2;
 double alphaStar, u, muMinus, z;
 muMinus = -*mu/sqrt(*sigma2);
 if (muMinus <= 0.0){
  check1 = FALSE;
  while(check1 == FALSE){
   GetRNGstate();
   z = rnorm(0.0,1.0);
   PutRNGstate();
   check1 = (z > muMinus);
  }
 } else {
  alphaStar = 0.5 * (muMinus + sqrt(muMinus * muMinus + 4.0));
  check2 = FALSE;
  while(check2 == FALSE){
   GetRNGstate();
   z = muMinus + rexp(1/alphaStar);
   PutRNGstate();
   GetRNGstate();
   u = runif(0.0,1.0);
   PutRNGstate();
   check2 = (u <= exp(-0.5*(z-alphaStar) * (z-alphaStar)));
  }
 }
 *x = *mu + z * sqrt(*sigma2);
}

int CGaussianMDP::multinomial(int ncell, double * nvec)
{
   /* draws just one from a multinomial distribution */
   int i, bindraw;
   double denom,tmp;
   
   /* draw multinomial via binomials */
   denom=0.0;
   
   for(i=0; i<ncell; i++)
      denom+=nvec[i];
   
   for(i=0; i<(ncell-1); i++) 
   {
      tmp = nvec[i]/denom;
      denom -= nvec[i];
      bindraw = runif(0.0,1.0)<=tmp;
      if(bindraw==1)
      {
         bindraw *= (i+1);
         return(bindraw);
      }
   }

   /* if 1,..,k-1 cells don't contain draw, then the last cell contains the draw*/
   bindraw = ncell;
   return(bindraw);
}

/**
 * Simulate beta using the naive Gibbs update
 *
 * @param da an SEXP struct
 *
 */
static void sim_beta(SEXP da){
  int *dm = DIMS_SLOT(da), *k = K_SLOT(da);
  int nB = dm[nB_POS];
  double *beta = FIXEF_SLOT(da), *mh_sd = MHSD_SLOT(da), *l = CLLIK_SLOT(da), 
    *pm = PBM_SLOT(da), *pv = PBV_SLOT(da), *acc = ACC_SLOT(da);
  double xo, xn, l1, l2, A;

  /* initialize llik_mu*/
  *l = llik_mu(da);
  for (int j = 0; j < nB; j++){
    *k = j;
    xo = beta[j];
    xn = rnorm(xo, mh_sd[j]);
    l1 = *l;
    l2 = post_betak(xn, da);
    A = exp(l2 - l1 + 0.5 * (xo - pm[j]) * (xo - pm[j]) / pv[j]);
    /* determine whether to accept the sample */
    if (A < 1 && runif(0, 1) >= A){ /* not accepted */
      *l = l1;       /* revert the likelihood (this is updated in post_betak) */
    }
    else {
      beta[j] = xn;
      acc[j]++;    
    }
  }                  /* update the mean using the new beta */                    
  if (dm[nU_POS]) cpglmm_fitted(beta, 1, da);
  else cpglm_fitted(beta, da);  
}

static void nosort_resamp (int nw, double *w, int np, int *p, int offset) 
{
  int i, j;
  double du, u;

  for (j = 1; j < nw; j++) w[j] += w[j-1];

  if (w[nw-1] <= 0.0)
    error("non-positive sum of weights");

  du = w[nw-1] / ((double) np);
  u = runif(-du,0);

  for (i = 0, j = 0; j < np; j++) {   
    u += du;
    while (u > w[i]) i++;
    p[j] = i;
      
    
  }
 
if (offset)      // add offset if needed
    for (j = 0; j < np; j++) p[j] += offset;

}

int random(int a)
{
  GetRNGstate();
  int f = (int) runif(0, a);
  PutRNGstate();
  return f;
};

double moaftme_sample_int_censored(double tl,
        double tu,
        double mean,
        double sigma) {
    //plnorm args: x, mean, sigma, lowertail=TRUE, log=FALSE
    double Fl = plnorm(tl, mean, sigma, 1, 0);
    if (Fl > (1 - 1e-8)) {
        // tl is very large and both f(tl) and f(tu) are very small.
        // qlnorm would return Inf. We sample uniformly from [tl, tu]. 
        return runif(tl, tu);
    }
    double Fu = plnorm(tu, mean, sigma, 1, 0);
    double Fw = runif(Fl, Fu);
    return qlnorm(Fw, mean, sigma, 1, 0);
    //Rprintf("i %f\n", w[i]);
}

		int rdunif(int n){
			int ret = 0;
			GetRNGstate();
			ret = (int) floor(n * runif(0, 1));
			PutRNGstate();
			return(ret);
		}

// [[Rcpp::export]]
IntegerVector bootPerm(const int n) {
    RNGScope scope;
    NumericVector unRound(runif(n, 0, n));
    NumericVector rounded(floor(unRound));
    IntegerVector out = Rcpp::as< IntegerVector >(rounded);
    return out;
}

// Function that computes the relative frequencies of the first digits of a random number satisfying benfords law
double rpbenf(double *r_pbenf, int *combfdigits, double *qbenfvals, int *n)
{
   int i,j;
   double random_x;
   
//   GetRNGstate();
   //set r_pbenf to zeros
   for (j = 0; j< combfdigits[0]; j++)
   {
      r_pbenf[j] = 0;
   }
   for (i = 0; i < n[0]; i++)
   {
      random_x = runif(0,1);
      
      for (j = 0; j< combfdigits[0]; j++)
      {
         if(random_x<=qbenfvals[j])
         {
            r_pbenf[j] = r_pbenf[j]+1;
            break;
         }
      }
   }
   for (j = 0; j< combfdigits[0]; j++)
   {
      r_pbenf[j] = r_pbenf[j]/n[0];
   }
//   PutRNGstate();
   return(*r_pbenf);
}

static void sim_u(SEXP da){
  int *dm = DIMS_SLOT(da), *k = K_SLOT(da);
  int nB = dm[nB_POS], nU = dm[nU_POS];
  double *u = U_SLOT(da), *l = CLLIK_SLOT(da), 
    *mh_sd = MHSD_SLOT(da) + nB + 2, /* shift the proposal variance pointer */
    *acc = ACC_SLOT(da) + nB + 2;    /* shift the acc pointer */
  double xo, xn, l1, l2, A;

  /* initialize llik_mu*/
  *l = llik_mu(da);
  for (int j = 0; j < nU; j++){
    *k = j ;
    xo = u[j];
    xn = rnorm(xo, mh_sd[j]);
    l1 = *l;
    l2 = post_uk(xn, da);
    A = exp(l2 - (l1 + prior_uk(xo, da)));  
    /* determine whether to accept the sample */
    if (A < 1 && runif(0, 1) >= A){ 
      *l = l1;  /* revert llik_mu (this is updated in post_uk) */
    }
    else{
      u[j] = xn;
      acc[j]++;    
    }
  }
  cpglmm_fitted(u, 0, da) ;  /* update the mean using the new u */
}

cs *cs_rR(const cs *A, double nu, double nuR, const css *As, const cs *Roldinv, double Roldldet, const cs *pG){
    
	cs *Rnew, *Rnewinv, *Ainv;
	double Rnewldet, MH;
        int dimG = A->n;
	int cnt = 0;
	int i, j;
	
	Rnewinv = cs_spalloc (dimG, dimG, dimG*dimG, 1, 0);
	
	for (i = 0 ; i < dimG; i++){
	  Rnewinv->p[i] = i*dimG;
	  for (j = 0 ; j < dimG; j++){
		Rnewinv->i[cnt] = j;
		Rnewinv->x[cnt] = 0.0;
                A->x[i*dimG+j] -= pG->x[i*dimG+j];
 		cnt++;
	  }
	}
	Rnewinv->p[dimG] = dimG*dimG;
		
	cs_cov2cor(A);
	Ainv = cs_inv(A);
	
	Rnew = cs_rinvwishart(Ainv, nu, As);	
	cs_cov2cor(Rnew);
		
	Rnewldet = log(cs_invR(Rnew, Rnewinv));

/*****************************************************/
/*       From Eq A.4 in Liu and Daniels (2006)       */
/*       using \pi_{1} = Eq 6 in Barnard (2000)      */
/*  using \pi_{2} = Eq 3.4 in Liu and Daniels (2006) */
/*****************************************************/

        MH = Roldldet-Rnewldet;
 
	for (i = 0 ; i < dimG; i++){
          MH += log(Roldinv->x[i*dimG+i]);
          MH -= log(Rnewinv->x[i*dimG+i]);
	}

	MH *= 0.5*nuR;

	if(MH<log(runif(0.0,1.0)) || Rnewldet<log(Dtol)){
	  Rnewldet = cs_invR(Roldinv, Rnew);	// save old R	
        }

        for (i = 0 ; i < dimG; i++){
          for (j = 0 ; j < dimG; j++){
 	    Rnew->x[i*dimG+j] *= sqrt((pG->x[i*dimG+i])*(pG->x[j*dimG+j]));
          }
        }

        cs_spfree(Rnewinv);
        cs_spfree(Ainv);

    return (cs_done (Rnew, NULL, NULL, 1)) ;	/* success; free workspace, return C */

}

  // This function is supposed to draw a random correlation matrix
  // from the uniform distribution on the space of all correlation
  // matrices.  It is broken
    CM random_cor(uint n){
      CM R(n);
      for(int k = 0; k < 1; ++k){
        for(int i = 0; i < n-1; ++i){
          for(int j = i+1; j < n; ++j){
            Rdet f(R, i, j);
            double f1 = f(1);
            double fn = f(-1);
            double f0 = f(0);
            double a = .5 * (f1 + fn - 2*f0);
            double b = .5 * (f1 - fn);
            double c = f0;

            double d2 = b*b - 4 * a * c;
            if(d2 < 0){
              R(i,j) = 0;
              R(j,i) = 0;
              continue;
            }
            double d = std::sqrt(d2);
            double lo = (-b - d)/(2*a);
            double hi = (-b + d)/(2*a);
            if(a < 0) std::swap(lo, hi);
            double r = runif(lo, hi);
            R(i,j) = r;
            R(j,i) = r;
          }
        }
      }
      return R;
    }

static void test_set_deta_rand(double min, double max)
{
	size_t n = catdist1_ncat(&CATDIST1);
	size_t i = rand() % n;
	double deta = runif(min, max);
	test_set_deta(i, deta);
	
}

void BAFT_LNsurv_update_sigSq(gsl_vector *yL,
                              gsl_vector *yU,
                              gsl_vector *yU_posinf,
                              gsl_vector *c0,
                              gsl_vector *c0_neginf,
                              gsl_matrix *X,
                              gsl_vector *y,
                              gsl_vector *beta,
                              double beta0,
                              double *sigSq,
                              double a_sigSq,
                              double b_sigSq,
                              double sigSq_prop_var,
                              int *accept_sigSq)
{
    int i, u;
    double eta, loglh, loglh_prop, logR, gamma_prop, sigSq_prop;
    double logprior, logprior_prop;
    
    int n = X -> size1;
    gsl_vector *xbeta = gsl_vector_calloc(n);
    
    loglh = 0;
    loglh_prop = 0;
    gamma_prop = rnorm(log(*sigSq), sqrt(sigSq_prop_var));
    sigSq_prop = exp(gamma_prop);
    gsl_blas_dgemv(CblasNoTrans, 1, X, beta, 0, xbeta);
    
    for(i=0;i<n;i++)
    {
        eta = beta0 + gsl_vector_get(xbeta, i);
        if(gsl_vector_get(c0_neginf, i) == 0)
        {
            loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(*sigSq), 1) - pnorm(gsl_vector_get(c0, i), eta, sqrt(*sigSq), 0, 1);
            loglh_prop += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq_prop), 1) - pnorm(gsl_vector_get(c0, i), eta, sqrt(sigSq_prop), 0, 1);
        }else
        {
            loglh += dnorm(gsl_vector_get(y, i), eta, sqrt(*sigSq), 1);
            loglh_prop += dnorm(gsl_vector_get(y, i), eta, sqrt(sigSq_prop), 1);
        }        
    }
    
    logprior = (-a_sigSq-1)*log(*sigSq)-b_sigSq /(*sigSq);
    logprior_prop = (-a_sigSq-1)*log(sigSq_prop)-b_sigSq/sigSq_prop;
    
    logR = loglh_prop - loglh + logprior_prop - logprior + gamma_prop - log(*sigSq);
    
    u = log(runif(0, 1)) < logR;
    
    if(u == 1)
    {
        *sigSq = sigSq_prop;
        *accept_sigSq += 1;
    }
    
    gsl_vector_free(xbeta);
    return;
}

void STGM::CBoolSphereSystem::simSpheres(F f, const char *label) {
  int nTry = 0;
  while(num==0 && nTry<MAX_ITER) {
        num = rpois(m_box.volume()*m_lam);
        ++nTry;
  }
  m_spheres.reserve(num);

  double m[3] = {m_box.m_size[0]+m_box.m_low[0],
                 m_box.m_size[1]+m_box.m_low[1],
                 m_box.m_size[2]+m_box.m_low[2]};

  /* loop over all */
  for (size_t niter=0;niter<num; niter++) {
      STGM::CVector3d center(runif(0.0,1.0)*m[0],runif(0.0,1.0)*m[1],runif(0.0,1.0)*m[2]);
      m_spheres.push_back( STGM::CSphere(center, f(), m_spheres.size()+1,label));
  }
}