///|
/// Common quality metrics for state-estimation experiments.
pub(all) struct ErrorMetrics {
  count : Int
  rmse : Double
  mae : Double
  max_error : Double
  final_error : Double
} derive(Debug)

///|
pub fn ErrorMetrics::count(self : ErrorMetrics) -> Int {
  self.count
}

///|
pub fn ErrorMetrics::rmse(self : ErrorMetrics) -> Double {
  self.rmse
}

///|
pub fn ErrorMetrics::mae(self : ErrorMetrics) -> Double {
  self.mae
}

///|
pub fn ErrorMetrics::max_error(self : ErrorMetrics) -> Double {
  self.max_error
}

///|
pub fn ErrorMetrics::final_error(self : ErrorMetrics) -> Double {
  self.final_error
}

///|
pub fn evaluate_errors(
  actual : Array[Array[Double]],
  expected : Array[Array[Double]],
) -> ErrorMetrics {
  let count = if actual.length() < expected.length() {
    actual.length()
  } else {
    expected.length()
  }
  if count == 0 {
    return { count: 0, rmse: 0.0, mae: 0.0, max_error: 0.0, final_error: 0.0 }
  }
  let mut squared = 0.0
  let mut absolute = 0.0
  let mut maximum = 0.0
  let mut final_error = 0.0
  let mut samples = 0
  for i in 0.. maximum {
      maximum = distance
    }
    final_error = distance
    samples = samples + 1
  }
  {
    count: samples,
    rmse: (squared / samples.to_double()).sqrt(),
    mae: absolute / samples.to_double(),
    max_error: maximum,
    final_error,
  }
}

///|
pub struct ConsistencyReport {
  count : Int
  average_nees : Double
  average_nis : Double
  accepted_rate : Double
  covariance_failures : Int
} derive(Debug)

///|
pub fn ConsistencyReport::count(self : ConsistencyReport) -> Int {
  self.count
}

///|
pub fn ConsistencyReport::average_nees(self : ConsistencyReport) -> Double {
  self.average_nees
}

///|
pub fn ConsistencyReport::average_nis(self : ConsistencyReport) -> Double {
  self.average_nis
}

///|
pub fn ConsistencyReport::accepted_rate(self : ConsistencyReport) -> Double {
  self.accepted_rate
}

///|
pub fn ConsistencyReport::covariance_failures(self : ConsistencyReport) -> Int {
  self.covariance_failures
}

///|
pub fn evaluate_consistency(
  estimates : Array[Array[Double]],
  truth : Array[Array[Double]],
  covariances : Array[Matrix],
  innovations : Array[Array[Double]],
  innovation_covariances : Array[Matrix],
  accepted : Array[Bool],
) -> ConsistencyReport {
  let count = estimates.length()
  if truth.length() < count || covariances.length() < count {
    return {
      count: 0,
      average_nees: 0.0,
      average_nis: 0.0,
      accepted_rate: 0.0,
      covariance_failures: 0,
    }
  }
  let mut total_nees = 0.0
  let mut total_nis = 0.0
  let mut total_accepted = 0
  let mut failures = 0
  for i in 0.. {
        failures = failures + 1
        0.0
      }
      Some(inverse) => vector_dot(error, inverse.multiply_vector(error))
    }
    total_nees = total_nees + nees
    if i < innovations.length() && i < innovation_covariances.length() {
      let nis_contribution = match innovation_covariances[i].inverse() {
        None => 0.0
        Some(inverse) =>
          vector_dot(innovations[i], inverse.multiply_vector(innovations[i]))
      }
      total_nis = total_nis + nis_contribution
    }
    if i < accepted.length() && accepted[i] {
      total_accepted = total_accepted + 1
    }
    if !covariance_is_psd(covariances[i], 0.000001) {
      failures = failures + 1
    }
  }
  let innovation_count = if innovations.length() < count {
    innovations.length()
  } else {
    count
  }
  {
    count,
    average_nees: total_nees / count.to_double(),
    average_nis: if innovation_count == 0 {
      0.0
    } else {
      total_nis / innovation_count.to_double()
    },
    accepted_rate: total_accepted.to_double() / count.to_double(),
    covariance_failures: failures,
  }
}

///|
/// Numerically stable streaming mean and variance using Welford's update.
pub struct RunningStats {
  mut count : Int
  mut mean : Double
  mut second_moment : Double
  mut minimum : Double
  mut maximum : Double
}

///|
pub fn RunningStats::new() -> RunningStats {
  { count: 0, mean: 0.0, second_moment: 0.0, minimum: 0.0, maximum: 0.0 }
}

///|
pub fn RunningStats::add(self : RunningStats, value : Double) -> Unit {
  if value.is_nan() || value.is_inf() {
    return
  }
  self.count = self.count + 1
  if self.count == 1 {
    self.mean = value
    self.minimum = value
    self.maximum = value
  } else {
    let difference = value - self.mean
    self.mean = self.mean + difference / self.count.to_double()
    self.second_moment = self.second_moment + difference * (value - self.mean)
    if value < self.minimum {
      self.minimum = value
    }
    if value > self.maximum {
      self.maximum = value
    }
  }
}

///|
pub fn RunningStats::count(self : RunningStats) -> Int {
  self.count
}

///|
pub fn RunningStats::mean(self : RunningStats) -> Double {
  self.mean
}

///|
pub fn RunningStats::variance(self : RunningStats) -> Double {
  if self.count < 2 {
    0.0
  } else {
    self.second_moment / (self.count - 1).to_double()
  }
}

///|
pub fn RunningStats::standard_deviation(self : RunningStats) -> Double {
  self.variance().sqrt()
}

///|
pub fn RunningStats::minimum(self : RunningStats) -> Double {
  self.minimum
}

///|
pub fn RunningStats::maximum(self : RunningStats) -> Double {
  self.maximum
}

///|
pub fn RunningStats::z_score(self : RunningStats, value : Double) -> Double {
  let deviation = self.standard_deviation()
  if deviation <= 0.000000000001 {
    0.0
  } else {
    (value - self.mean) / deviation
  }
}

///|
pub fn RunningStats::reset(self : RunningStats) -> Unit {
  self.count = 0
  self.mean = 0.0
  self.second_moment = 0.0
  self.minimum = 0.0
  self.maximum = 0.0
}

///|
pub struct AdaptiveNoiseController {
  mut process_scale : Double
  mut measurement_scale : Double
  minimum_scale : Double
  maximum_scale : Double
  target_nis : Double
  learning_rate : Double
}

///|
pub fn AdaptiveNoiseController::new(
  minimum_scale : Double,
  maximum_scale : Double,
  target_nis : Double,
  learning_rate : Double,
) -> AdaptiveNoiseController {
  let low = if minimum_scale <= 0.0 { 0.01 } else { minimum_scale }
  let high = if maximum_scale < low { low } else { maximum_scale }
  {
    process_scale: 1.0,
    measurement_scale: 1.0,
    minimum_scale: low,
    maximum_scale: high,
    target_nis: if target_nis <= 0.0 {
      1.0
    } else {
      target_nis
    },
    learning_rate: if learning_rate < 0.0 {
      0.0
    } else if learning_rate > 1.0 {
      1.0
    } else {
      learning_rate
    },
  }
}

///|
pub fn AdaptiveNoiseController::observe(
  self : AdaptiveNoiseController,
  nis : Double,
) -> Unit {
  let error = (nis - self.target_nis) / self.target_nis
  let adjustment = 1.0 + self.learning_rate * error
  self.measurement_scale = self.measurement_scale * adjustment
  if self.measurement_scale < self.minimum_scale {
    self.measurement_scale = self.minimum_scale
  }
  if self.measurement_scale > self.maximum_scale {
    self.measurement_scale = self.maximum_scale
  }
  self.process_scale = 1.0 / self.measurement_scale
  if self.process_scale < self.minimum_scale {
    self.process_scale = self.minimum_scale
  }
  if self.process_scale > self.maximum_scale {
    self.process_scale = self.maximum_scale
  }
}

///|
pub fn AdaptiveNoiseController::process_scale(
  self : AdaptiveNoiseController,
) -> Double {
  self.process_scale
}

///|
pub fn AdaptiveNoiseController::measurement_scale(
  self : AdaptiveNoiseController,
) -> Double {
  self.measurement_scale
}

///|
pub struct OutlierDetector {
  threshold : Double
  stats : RunningStats
  mut outlier_count : Int
}

///|
pub fn OutlierDetector::new(threshold : Double) -> OutlierDetector {
  {
    threshold: if threshold <= 0.0 {
      3.0
    } else {
      threshold
    },
    stats: RunningStats::new(),
    outlier_count: 0,
  }
}

///|
pub fn OutlierDetector::observe(self : OutlierDetector, value : Double) -> Bool {
  let is_outlier = self.stats.count() >= 2 &&
    self.stats.z_score(value).abs() > self.threshold
  self.stats.add(value)
  if is_outlier {
    self.outlier_count = self.outlier_count + 1
  }
  is_outlier
}

///|
pub fn OutlierDetector::outlier_count(self : OutlierDetector) -> Int {
  self.outlier_count
}

///|
pub fn OutlierDetector::stats(self : OutlierDetector) -> RunningStats {
  self.stats
}