doubly_censored_expectation <- function(theta,sigma) {
  pM1 <- theta*(pnorm((1.0 - theta)/sigma) - pnorm((0.0 - theta)/sigma))
  pM2 <- sigma*(dnorm((0.0 - theta)/sigma) - dnorm((1.0 - theta)/sigma))
  pR <- 1.0 * (1.0 - pnorm((1.0 - theta)/sigma))
  output <- pM1 + pM2 + pR
  return(output)
}

# Create group_info by repeating group numbers based on ni_vector values
ni_to_group_info <- function(ni_vector) {
  group_info <- unlist(lapply(seq_along(ni_vector), function(i) {
    rep(i, ni_vector[i])
  }))

  return(group_info)
}

# based on A Simple Sampler for the Horseshoe Estimator by Enes Makalic and Daniel F. Schmidt
# Robust version: Prevents computationally singular errors
horseshoe_iteration <- function(y, X, sigma2, lambda2, tau2, nu, xi) {

  n <- nrow(X)
  p <- ncol(X)

  # ---------------------------------------------------------
  # 1. Update beta
  # ---------------------------------------------------------
  # Clamp variances to prevent 1/0 creating Inf in the matrix
  effective_variances <- pmax(tau2 * lambda2, 1e-12)
  Lambda_star_inv <- diag(1 / effective_variances, nrow = p, ncol = p)

  A <- crossprod(X) + Lambda_star_inv

  # Add microscopic jitter to diagonal to guarantee matrix condition
  diag(A) <- diag(A) + 1e-10

  # Use Cholesky decomposition (vastly more stable than solve)
  R <- chol(A)
  A_inv <- chol2inv(R)

  # Calculate mean for beta
  mu_beta <- A_inv %*% crossprod(X, y)

  # Sample beta using the Cholesky factor R directly
  # backsolve(R, z) is mathematically identical to t(chol(A_inv)) %*% z but much faster
  z <- rnorm(p)
  beta <- as.numeric(mu_beta + sqrt(sigma2) * backsolve(R, z))

  # ---------------------------------------------------------
  # 2. Update lambda2_j (local hypervariances)
  # ---------------------------------------------------------
  shape_lambda <- 1
  safe_tau2 <- max(tau2, 1e-12) # Prevent division by zero
  rate_lambda <- 1/nu + (beta^2) / (2 * safe_tau2 * sigma2)

  lambda2 <- 1 / rgamma(p, shape = shape_lambda, rate = rate_lambda)
  lambda2 <- pmax(lambda2, 1e-12) # Clamp output

  # ---------------------------------------------------------
  # 3. Update tau2 (global hypervariance)
  # ---------------------------------------------------------
  shape_tau <- (p + 1) / 2
  safe_lambda2 <- pmax(lambda2, 1e-12) # Prevent division by zero
  rate_tau <- 1/xi + sum(beta^2 / safe_lambda2) / (2 * sigma2)

  tau2 <- 1 / rgamma(1, shape = shape_tau, rate = rate_tau)
  tau2 <- max(tau2, 1e-12) # Clamp output

  # ---------------------------------------------------------
  # 4. Update nu_j (auxiliary variables)
  # ---------------------------------------------------------
  shape_nu <- 1
  rate_nu <- 1 + (1 / safe_lambda2)
  nu <- 1 / rgamma(p, shape = shape_nu, rate = rate_nu)

  # ---------------------------------------------------------
  # 5. Update xi (auxiliary variable)
  # ---------------------------------------------------------
  shape_xi <- 1
  rate_xi <- 1 + (1 / safe_tau2)
  xi <- 1 / rgamma(1, shape = shape_xi, rate = rate_xi)

  # Return updated parameters
  return(list(
    beta = beta,
    lambda2 = lambda2,
    tau2 = tau2,
    nu = nu,
    xi = xi
  ))
}
