///|
/// A scalar confidence interval with an explicit confidence multiplier.
pub struct ConfidenceBand {
  estimate : Double
  lower : Double
  upper : Double
  standard_deviation : Double
  multiplier : Double
  valid : Bool
} derive(Debug)

///|
/// Construct a confidence band from an estimate and standard deviation.
pub fn ConfidenceBand::new(
  estimate : Double,
  standard_deviation : Double,
  multiplier : Double,
) -> ConfidenceBand {
  let deviation = if standard_deviation < 0.0 || standard_deviation.is_nan() {
    0.0
  } else {
    standard_deviation
  }
  let scale = if multiplier < 0.0 || multiplier.is_nan() {
    0.0
  } else {
    multiplier
  }
  let radius = deviation * scale
  {
    estimate,
    lower: estimate - radius,
    upper: estimate + radius,
    standard_deviation: deviation,
    multiplier: scale,
    valid: !estimate.is_nan() && !estimate.is_inf(),
  }
}

///|
/// Return estimate.
pub fn ConfidenceBand::estimate(self : ConfidenceBand) -> Double {
  self.estimate
}

///|
/// Return lower bound.
pub fn ConfidenceBand::lower(self : ConfidenceBand) -> Double {
  self.lower
}

///|
/// Return upper bound.
pub fn ConfidenceBand::upper(self : ConfidenceBand) -> Double {
  self.upper
}

///|
/// Return standard deviation.
pub fn ConfidenceBand::standard_deviation(self : ConfidenceBand) -> Double {
  self.standard_deviation
}

///|
/// Return confidence multiplier.
pub fn ConfidenceBand::multiplier(self : ConfidenceBand) -> Double {
  self.multiplier
}

///|
/// Return validity.
pub fn ConfidenceBand::valid(self : ConfidenceBand) -> Bool {
  self.valid
}

///|
/// Return interval width.
pub fn ConfidenceBand::width(self : ConfidenceBand) -> Double {
  self.upper - self.lower
}

///|
/// Return whether a value is inside the band.
pub fn ConfidenceBand::contains(self : ConfidenceBand, value : Double) -> Bool {
  value >= self.lower && value <= self.upper
}

///|
/// Construct a band from a variance.
pub fn confidence_band_from_variance(
  estimate : Double,
  variance : Double,
  multiplier : Double,
) -> ConfidenceBand {
  ConfidenceBand::new(
    estimate,
    if variance <= 0.0 {
      0.0
    } else {
      variance.sqrt()
    },
    multiplier,
  )
}

///|
/// A component-wise confidence band for a vector state.
pub struct VectorConfidenceBand {
  estimates : Array[Double]
  lower : Array[Double]
  upper : Array[Double]
  standard_deviations : Array[Double]
  multiplier : Double
  valid : Bool
} derive(Debug)

///|
/// Construct a vector confidence band from a covariance diagonal.
pub fn VectorConfidenceBand::new(
  estimates : Array[Double],
  covariance : Matrix,
  multiplier : Double,
) -> VectorConfidenceBand {
  let lower = Array::make(estimates.length(), 0.0)
  let upper = Array::make(estimates.length(), 0.0)
  let deviations = Array::make(estimates.length(), 0.0)
  let mut valid = covariance.rows() == estimates.length() &&
    covariance.cols() == estimates.length()
  for i in 0.. Array[Double] {
  self.estimates.copy()
}

///|
/// Return lower bounds.
pub fn VectorConfidenceBand::lower(
  self : VectorConfidenceBand,
) -> Array[Double] {
  self.lower.copy()
}

///|
/// Return upper bounds.
pub fn VectorConfidenceBand::upper(
  self : VectorConfidenceBand,
) -> Array[Double] {
  self.upper.copy()
}

///|
/// Return component deviations.
pub fn VectorConfidenceBand::standard_deviations(
  self : VectorConfidenceBand,
) -> Array[Double] {
  self.standard_deviations.copy()
}

///|
/// Return multiplier.
pub fn VectorConfidenceBand::multiplier(self : VectorConfidenceBand) -> Double {
  self.multiplier
}

///|
/// Return validity.
pub fn VectorConfidenceBand::valid(self : VectorConfidenceBand) -> Bool {
  self.valid
}

///|
/// Return state dimension.
pub fn VectorConfidenceBand::dimension(self : VectorConfidenceBand) -> Int {
  self.estimates.length()
}

///|
/// Return whether a vector lies inside component-wise bounds.
pub fn VectorConfidenceBand::contains(
  self : VectorConfidenceBand,
  value : Array[Double],
) -> Bool {
  if value.length() != self.estimates.length() {
    return false
  }
  for i in 0.. self.upper[i] {
      return false
    }
  }
  true
}

///|
/// A summary of covariance health and scale.
pub struct CovarianceSummary {
  dimension : Int
  trace : Double
  determinant : Double
  minimum_diagonal : Double
  maximum_diagonal : Double
  condition : Double
  rank : Int
  symmetric : Bool
  positive_diagonal : Bool
  finite : Bool
} derive(Debug)

///|
/// Return covariance dimension.
pub fn CovarianceSummary::dimension(self : CovarianceSummary) -> Int {
  self.dimension
}

///|
/// Return trace.
pub fn CovarianceSummary::trace(self : CovarianceSummary) -> Double {
  self.trace
}

///|
/// Return determinant.
pub fn CovarianceSummary::determinant(self : CovarianceSummary) -> Double {
  self.determinant
}

///|
/// Return minimum diagonal.
pub fn CovarianceSummary::minimum_diagonal(self : CovarianceSummary) -> Double {
  self.minimum_diagonal
}

///|
/// Return maximum diagonal.
pub fn CovarianceSummary::maximum_diagonal(self : CovarianceSummary) -> Double {
  self.maximum_diagonal
}

///|
/// Return condition estimate.
pub fn CovarianceSummary::condition(self : CovarianceSummary) -> Double {
  self.condition
}

///|
/// Return numerical rank.
pub fn CovarianceSummary::rank(self : CovarianceSummary) -> Int {
  self.rank
}

///|
/// Return symmetry status.
pub fn CovarianceSummary::symmetric(self : CovarianceSummary) -> Bool {
  self.symmetric
}

///|
/// Return positive-diagonal status.
pub fn CovarianceSummary::positive_diagonal(self : CovarianceSummary) -> Bool {
  self.positive_diagonal
}

///|
/// Return finite status.
pub fn CovarianceSummary::finite(self : CovarianceSummary) -> Bool {
  self.finite
}

///|
/// Return whether covariance is suitable for confidence reporting.
pub fn CovarianceSummary::usable(
  self : CovarianceSummary,
  maximum_condition : Double,
) -> Bool {
  self.finite &&
  self.symmetric &&
  self.positive_diagonal &&
  self.rank == self.dimension &&
  self.condition <= maximum_condition
}

///|
/// Summarize a covariance matrix with an absolute symmetry tolerance.
pub fn summarize_uncertainty_covariance(
  covariance : Matrix,
  tolerance : Double,
) -> CovarianceSummary {
  let square = covariance.is_square()
  let dimension = if square { covariance.rows() } else { 0 }
  let mut symmetric = square
  let mut positive = square
  if square {
    for i in 0.. tolerance {
          symmetric = false
        }
      }
    }
  }
  {
    dimension,
    trace: covariance.trace(),
    determinant: covariance.determinant(),
    minimum_diagonal: covariance.diagonal_min(),
    maximum_diagonal: covariance.diagonal_max(),
    condition: covariance.condition_estimate(),
    rank: covariance.rank(tolerance),
    symmetric,
    positive_diagonal: positive,
    finite: covariance.is_finite(),
  }
}

///|
/// A named uncertainty contribution for a budget review.
pub struct UncertaintyContribution {
  name : String
  variance : Double
  fraction : Double
  enabled : Bool
} derive(Debug)

///|
/// Construct a contribution.
pub fn UncertaintyContribution::new(
  name : String,
  variance : Double,
  enabled : Bool,
) -> UncertaintyContribution {
  {
    name,
    variance: if variance < 0.0 || variance.is_nan() {
      0.0
    } else {
      variance
    },
    fraction: 0.0,
    enabled,
  }
}

///|
/// Return name.
pub fn UncertaintyContribution::name(self : UncertaintyContribution) -> String {
  self.name
}

///|
/// Return variance.
pub fn UncertaintyContribution::variance(
  self : UncertaintyContribution,
) -> Double {
  self.variance
}

///|
/// Return normalized fraction.
pub fn UncertaintyContribution::fraction(
  self : UncertaintyContribution,
) -> Double {
  self.fraction
}

///|
/// Return enabled flag.
pub fn UncertaintyContribution::enabled(self : UncertaintyContribution) -> Bool {
  self.enabled
}

///|
/// An uncertainty budget accumulated from independent contributions.
pub struct UncertaintyBudget {
  contributions : Array[UncertaintyContribution]
  mut total_variance : Double
  mut total_standard_deviation : Double
} derive(Debug)

///|
/// Construct an empty budget.
pub fn UncertaintyBudget::new() -> UncertaintyBudget {
  { contributions: [], total_variance: 0.0, total_standard_deviation: 0.0 }
}

///|
/// Add a contribution and recalculate fractions.
pub fn UncertaintyBudget::add(
  self : UncertaintyBudget,
  contribution : UncertaintyContribution,
) -> Unit {
  self.contributions.push(contribution)
  self.recalculate()
}

///|
/// Recalculate total variance and fractions.
pub fn UncertaintyBudget::recalculate(self : UncertaintyBudget) -> Unit {
  let mut total = 0.0
  for contribution in self.contributions {
    if contribution.enabled() {
      total = total + contribution.variance()
    }
  }
  self.total_variance = total
  self.total_standard_deviation = if total <= 0.0 { 0.0 } else { total.sqrt() }
  for i in 0.. Array[UncertaintyContribution] {
  self.contributions.copy()
}

///|
/// Return total variance.
pub fn UncertaintyBudget::total_variance(self : UncertaintyBudget) -> Double {
  self.total_variance
}

///|
/// Return total standard deviation.
pub fn UncertaintyBudget::total_standard_deviation(
  self : UncertaintyBudget,
) -> Double {
  self.total_standard_deviation
}

///|
/// Return the largest active contribution.
pub fn UncertaintyBudget::dominant(
  self : UncertaintyBudget,
) -> UncertaintyContribution? {
  let mut index = -1
  let mut largest = -1.0
  for i, contribution in self.contributions {
    if contribution.enabled() && contribution.variance() > largest {
      index = i
      largest = contribution.variance()
    }
  }
  if index < 0 {
    None
  } else {
    Some(self.contributions[index])
  }
}

///|
/// Return a budget contribution by name.
pub fn UncertaintyBudget::find(
  self : UncertaintyBudget,
  name : String,
) -> UncertaintyContribution? {
  for contribution in self.contributions {
    if contribution.name() == name {
      return Some(contribution)
    }
  }
  None
}

///|
/// Remove all contributions.
pub fn UncertaintyBudget::clear(self : UncertaintyBudget) -> Unit {
  self.contributions.clear()
  self.total_variance = 0.0
  self.total_standard_deviation = 0.0
}

///|
/// An axis-aligned uncertainty region for a 3D position.
pub struct UncertaintyBox3D {
  center : Vec3D
  half_width : Vec3D
  confidence : Double
} derive(Debug)

///|
/// Construct a 3D uncertainty box from standard deviations.
pub fn UncertaintyBox3D::new(
  center : Vec3D,
  standard_deviations : Vec3D,
  multiplier : Double,
) -> UncertaintyBox3D {
  UncertaintyBox3D::from_half_width(
    center,
    standard_deviations.scale(multiplier.max(0.0)),
    multiplier.max(0.0),
  )
}

///|
/// Construct a 3D box directly from half widths.
pub fn UncertaintyBox3D::from_half_width(
  center : Vec3D,
  half_width : Vec3D,
  confidence : Double,
) -> UncertaintyBox3D {
  { center, half_width: half_width.clamp(0.0, 1.0e300), confidence }
}

///|
/// Return center.
pub fn UncertaintyBox3D::center(self : UncertaintyBox3D) -> Vec3D {
  self.center
}

///|
/// Return half widths.
pub fn UncertaintyBox3D::half_width(self : UncertaintyBox3D) -> Vec3D {
  self.half_width
}

///|
/// Return confidence multiplier.
pub fn UncertaintyBox3D::confidence(self : UncertaintyBox3D) -> Double {
  self.confidence
}

///|
/// Return lower corner.
pub fn UncertaintyBox3D::lower(self : UncertaintyBox3D) -> Vec3D {
  self.center.sub(self.half_width)
}

///|
/// Return upper corner.
pub fn UncertaintyBox3D::upper(self : UncertaintyBox3D) -> Vec3D {
  self.center.add(self.half_width)
}

///|
/// Return volume.
pub fn UncertaintyBox3D::volume(self : UncertaintyBox3D) -> Double {
  8.0 * self.half_width.x() * self.half_width.y() * self.half_width.z()
}

///|
/// Return whether a point is inside.
pub fn UncertaintyBox3D::contains(
  self : UncertaintyBox3D,
  point : Vec3D,
) -> Bool {
  let lower = self.lower()
  let upper = self.upper()
  point.x() >= lower.x() &&
  point.x() <= upper.x() &&
  point.y() >= lower.y() &&
  point.y() <= upper.y() &&
  point.z() >= lower.z() &&
  point.z() <= upper.z()
}

///|
/// Inflate a box by a non-negative factor.
pub fn UncertaintyBox3D::inflate(
  self : UncertaintyBox3D,
  factor : Double,
) -> UncertaintyBox3D {
  UncertaintyBox3D::from_half_width(
    self.center,
    self.half_width.scale(1.0 + factor.max(0.0)),
    self.confidence,
  )
}

///|
/// Propagate a diagonal covariance through a linear gain.
pub fn propagate_diagonal_uncertainty(
  covariance : Matrix,
  gain : Array[Double],
  process_variance : Double,
) -> Matrix {
  let dimension = if covariance.is_square() { covariance.rows() } else { 0 }
  let result = covariance.copy()
  let process = process_variance.max(0.0)
  for i in 0.. ignore
    }
    result.set(i, i, result.get(i, i) + process) |> ignore
  }
  result.symmetric_part()
}

///|
/// Combine independent covariance estimates using precision weighting.
pub fn combine_independent_variances(variances : Array[Double]) -> Double {
  let mut precision = 0.0
  for variance in variances {
    if variance > 0.0 && !variance.is_nan() {
      precision = precision + 1.0 / variance
    }
  }
  if precision <= 0.0 {
    0.0
  } else {
    1.0 / precision
  }
}

///|
/// Combine scalar estimates using inverse-variance weights.
pub fn combine_independent_estimates(
  estimates : Array[Double],
  variances : Array[Double],
) -> ConfidenceBand {
  let count = if estimates.length() < variances.length() {
    estimates.length()
  } else {
    variances.length()
  }
  let mut numerator = 0.0
  let mut precision = 0.0
  for i in 0.. 0.0 && !estimates[i].is_nan() {
      let weight = 1.0 / variances[i]
      numerator = numerator + weight * estimates[i]
      precision = precision + weight
    }
  }
  if precision <= 0.0 {
    ConfidenceBand::new(0.0, 0.0, 0.0)
  } else {
    ConfidenceBand::new(numerator / precision, (1.0 / precision).sqrt(), 1.0)
  }
}

///|
/// Return a normalized uncertainty score where zero is best.
pub fn covariance_uncertainty_score(
  covariance : Matrix,
  scale : Double,
) -> Double {
  let summary = summarize_uncertainty_covariance(covariance, 0.000001)
  let divisor = if scale <= 0.0 { 1.0 } else { scale }
  (summary.trace().abs() / divisor).sqrt()
}

///|
/// Return whether a covariance update is monotonic in trace.
pub fn covariance_trace_decreased(
  before : Matrix,
  after : Matrix,
  tolerance : Double,
) -> Bool {
  after.trace() <= before.trace() + tolerance.abs()
}

///|
/// Compute per-component standard deviations from covariance.
pub fn covariance_standard_deviations(covariance : Matrix) -> Array[Double] {
  if !covariance.is_square() {
    return []
  }
  Array::makei(covariance.rows(), i => {
    if covariance.get(i, i) <= 0.0 {
      0.0
    } else {
      covariance.get(i, i).sqrt()
    }
  })
}

///|
/// Build component confidence bands from a state and covariance.
pub fn state_confidence_bands(
  state : Array[Double],
  covariance : Matrix,
  multiplier : Double,
) -> Array[ConfidenceBand] {
  let deviations = covariance_standard_deviations(covariance)
  Array::makei(state.length(), i => {
    ConfidenceBand::new(
      state[i],
      if i < deviations.length() {
        deviations[i]
      } else {
        0.0
      },
      multiplier,
    )
  })
}

///|
/// Return the fraction of a reference state covered by bands.
pub fn confidence_coverage(
  bands : Array[ConfidenceBand],
  values : Array[Double],
) -> Double {
  let count = if bands.length() < values.length() {
    bands.length()
  } else {
    values.length()
  }
  if count == 0 {
    return 0.0
  }
  let mut covered = 0
  for i in 0.. Double {
  if coverages.length() == 0 {
    return 0.0
  }
  let mut error = 0.0
  for coverage in coverages {
    error = error + (coverage - target).abs()
  }
  (1.0 - error / coverages.length().to_double()).clamp(min=0.0, max=1.0)
}

///|
/// Estimate whether an uncertainty report is overconfident.
pub fn uncertainty_overconfidence(
  coverage : Double,
  target : Double,
  tolerance : Double,
) -> Bool {
  coverage + tolerance.abs() < target
}

///|
/// Estimate whether an uncertainty report is too conservative.
pub fn uncertainty_underconfidence(
  coverage : Double,
  target : Double,
  tolerance : Double,
) -> Bool {
  coverage - tolerance.abs() > target
}

///|
/// Compute the Mahalanobis radius using a covariance matrix.
pub fn uncertainty_mahalanobis_radius(
  residual : Array[Double],
  covariance : Matrix,
) -> Double {
  guard covariance.inverse() is Some(inverse) else { return 0.0 }
  let projected = inverse.multiply_vector(residual)
  let mut value = 0.0
  let count = if residual.length() < projected.length() {
    residual.length()
  } else {
    projected.length()
  }
  for i in 0.. Matrix {
  let result = covariance.symmetric_part()
  let minimum = floor.max(0.0)
  for i in 0.. ignore
    }
  }
  result
}

///|
/// Compute the state uncertainty volume proxy from a covariance determinant.
pub fn uncertainty_volume_proxy(covariance : Matrix) -> Double {
  let determinant = covariance.determinant()
  if determinant <= 0.0 || determinant.is_nan() {
    0.0
  } else {
    determinant.sqrt()
  }
}

///|
/// Compare two covariance matrices by trace and condition.
pub fn compare_uncertainty_covariances(left : Matrix, right : Matrix) -> Int {
  let left_score = left.trace() + left.condition_estimate()
  let right_score = right.trace() + right.condition_estimate()
  if left_score < right_score {
    -1
  } else if left_score > right_score {
    1
  } else {
    0
  }
}