# Internal Utility Functions for ImpAdaptType2Censor

#' @keywords internal
.ge_cdf <- function(x, par) {
  alpha <- par[1]
  lambda <- par[2]
  if (alpha <= 0 || lambda <= 0) return(rep(0, length(x)))
  val <- 1 - exp(-lambda * x)
  val <- pmax(pmin(val, 1 - 1e-15), 1e-15)
  return(val^alpha)
}

#' @keywords internal
.ge_pdf <- function(x, par) {
  alpha <- par[1]
  lambda <- par[2]
  if (alpha <= 0 || lambda <= 0) return(rep(0, length(x)))
  val <- 1 - exp(-lambda * x)
  val <- pmax(pmin(val, 1 - 1e-15), 1e-15)
  return(alpha * lambda * exp(-lambda * x) * (val^(alpha - 1)))
}

#' @keywords internal
.ge_qdf <- function(p, par) {
  alpha <- par[1]
  lambda <- par[2]
  p <- pmax(pmin(p, 1 - 1e-15), 1e-15)
  return(- (1 / lambda) * log(1 - p^(1 / alpha)))
}

#' @keywords internal
.ge_surv <- function(x, par) {
  return(pmax(1 - .ge_cdf(x, par), 1e-15))
}

#' @keywords internal
.ge_haz <- function(x, par) {
  pdf_val <- .ge_pdf(x, par)
  surv_val <- .ge_surv(x, par)
  return(pdf_val / surv_val)
}

#' @keywords internal
.solve_quantile <- function(cdf, u, par, lower_bound = 1e-7, upper_bound = 1e5) {
  u <- pmax(pmin(u, 1 - 1e-15), 1e-15)
  target_fn <- function(x) {
    cdf(x, par) - u
  }
  
  l <- lower_bound
  u_b <- upper_bound
  
  val_l <- target_fn(l)
  val_u <- target_fn(u_b)
  
  # Expand upper bound if necessary
  iter <- 0
  while (val_l * val_u > 0 && iter < 10) {
    if (val_l > 0) l <- l / 10
    if (val_u < 0) u_b <- u_b * 10
    val_l <- target_fn(l)
    val_u <- target_fn(u_b)
    iter <- iter + 1
  }
  
  if (val_l * val_u > 0) {
    # Fallback grid search
    grid_x <- seq(l, u_b, length.out = 1000)
    grid_diff <- abs(sapply(grid_x, target_fn))
    return(grid_x[which.min(grid_diff)])
  }
  
  res <- stats::uniroot(target_fn, lower = l, upper = u_b, tol = 1e-9)
  return(res$root)
}

#' @keywords internal
.num_grad <- function(fn, par, eps = 1e-6, ...) {
  n <- length(par)
  grad <- numeric(n)
  f0 <- fn(par, ...)
  for (i in seq_len(n)) {
    par_high <- par
    par_low <- par
    par_high[i] <- par[i] + eps
    par_low[i] <- par[i] - eps
    grad[i] <- (fn(par_high, ...) - fn(par_low, ...)) / (2 * eps)
  }
  return(grad)
}

#' @keywords internal
.num_hessian <- function(fn, par, eps = 1e-5, ...) {
  n <- length(par)
  hess <- matrix(0, nrow = n, ncol = n)
  f0 <- fn(par, ...)
  for (i in seq_len(n)) {
    for (j in seq_len(n)) {
      if (i == j) {
        par_high <- par
        par_low <- par
        par_high[i] <- par[i] + eps
        par_low[i] <- par[i] - eps
        hess[i, i] <- (fn(par_high, ...) - 2 * f0 + fn(par_low, ...)) / (eps^2)
      } else {
        par_pp <- par
        par_pm <- par
        par_mp <- par
        par_mm <- par
        
        par_pp[i] <- par[i] + eps; par_pp[j] <- par[j] + eps
        par_pm[i] <- par[i] + eps; par_pm[j] <- par[j] - eps
        par_mp[i] <- par[i] - eps; par_mp[j] <- par[j] + eps
        par_mm[i] <- par[i] - eps; par_mm[j] <- par[j] - eps
        
        hess[i, j] <- (fn(par_pp, ...) - fn(par_pm, ...) - fn(par_mp, ...) + fn(par_mm, ...)) / (4 * eps^2)
      }
    }
  }
  return((hess + t(hess)) / 2)
}

#' @keywords internal
.compute_hpd <- function(samples, prob = 0.95) {
  samples <- sort(samples)
  n <- length(samples)
  num_to_keep <- ceiling(prob * n)
  if (num_to_keep >= n) {
    return(c(lower = samples[1], upper = samples[n]))
  }
  diffs <- samples[(num_to_keep):n] - samples[1:(n - num_to_keep + 1)]
  min_idx <- which.min(diffs)
  return(c(lower = samples[min_idx], upper = samples[min_idx + num_to_keep - 1]))
}
