///|
/// Noise estimates inferred from paired truth/measurement samples.
pub struct NoiseEstimate {
  variance : Double
  standard_deviation : Double
  samples : Int
  confidence : Double
} derive(Debug)

///|
pub fn NoiseEstimate::variance(self : NoiseEstimate) -> Double {
  self.variance
}

///|
pub fn NoiseEstimate::standard_deviation(self : NoiseEstimate) -> Double {
  self.standard_deviation
}

///|
pub fn NoiseEstimate::samples(self : NoiseEstimate) -> Int {
  self.samples
}

///|
pub fn NoiseEstimate::confidence(self : NoiseEstimate) -> Double {
  self.confidence
}

///|
pub fn estimate_noise(
  measurements : Array[Double],
  expected : Array[Double],
) -> NoiseEstimate {
  let count = if measurements.length() < expected.length() {
    measurements.length()
  } else {
    expected.length()
  }
  if count < 2 {
    return {
      variance: 0.0,
      standard_deviation: 0.0,
      samples: count,
      confidence: 0.0,
    }
  }
  let residuals = Array::makei(count, i => measurements[i] - expected[i])
  let variance = vector_variance(residuals)
  let confidence = if count >= 30 { 0.95 } else { count.to_double() / 30.0 }
  { variance, standard_deviation: variance.sqrt(), samples: count, confidence }
}

///|
pub fn estimate_process_noise(
  states : Array[Double],
  spacing : Double,
) -> NoiseEstimate {
  if states.length() < 3 {
    return {
      variance: 0.0,
      standard_deviation: 0.0,
      samples: 0,
      confidence: 0.0,
    }
  }
  let differences = signal_difference(states, spacing)
  let accelerations = signal_difference(differences, spacing)
  let variance = vector_variance(accelerations)
  let samples = if accelerations.length() > 2 {
    accelerations.length() - 2
  } else {
    0
  }
  {
    variance,
    standard_deviation: variance.sqrt(),
    samples,
    confidence: samples.to_double() / (samples + 10).to_double(),
  }
}

///|
pub struct ScalarCalibration {
  measurement : NoiseEstimate
  process : NoiseEstimate
  recommended_process_noise : Double
  recommended_measurement_noise : Double
} derive(Debug)

///|
pub fn ScalarCalibration::measurement(
  self : ScalarCalibration,
) -> NoiseEstimate {
  self.measurement
}

///|
pub fn ScalarCalibration::process(self : ScalarCalibration) -> NoiseEstimate {
  self.process
}

///|
pub fn ScalarCalibration::recommended_process_noise(
  self : ScalarCalibration,
) -> Double {
  self.recommended_process_noise
}

///|
pub fn ScalarCalibration::recommended_measurement_noise(
  self : ScalarCalibration,
) -> Double {
  self.recommended_measurement_noise
}

///|
pub fn calibrate_scalar(
  measurements : Array[Double],
  expected : Array[Double],
  spacing : Double,
  minimum_noise : Double,
) -> ScalarCalibration {
  let measurement = estimate_noise(measurements, expected)
  let process = estimate_process_noise(expected, spacing)
  let floor = if minimum_noise <= 0.0 { 0.000001 } else { minimum_noise }
  {
    measurement,
    process,
    recommended_process_noise: if process.variance() < floor {
      floor
    } else {
      process.variance()
    },
    recommended_measurement_noise: if measurement.variance() < floor {
      floor
    } else {
      measurement.variance()
    },
  }
}

///|
/// Online innovation monitor used as a quality gate and alert source.
pub struct InnovationMonitor {
  dimension : Int
  threshold : Double
  mut samples : Int
  mut accepted : Int
  mut rejected : Int
  nis_stats : RunningStats
  mut last_nis : Double
}

///|
pub fn InnovationMonitor::new(
  dimension : Int,
  threshold : Double,
) -> InnovationMonitor {
  {
    dimension: if dimension < 0 {
      0
    } else {
      dimension
    },
    threshold: if threshold < 0.0 {
      0.0
    } else {
      threshold
    },
    samples: 0,
    accepted: 0,
    rejected: 0,
    nis_stats: RunningStats::new(),
    last_nis: 0.0,
  }
}

///|
pub fn InnovationMonitor::observe(
  self : InnovationMonitor,
  innovation : Array[Double],
  covariance : Matrix,
) -> Bool {
  if self.dimension == 0 ||
    innovation.length() != self.dimension ||
    covariance.rows() != self.dimension ||
    covariance.cols() != self.dimension {
    self.rejected = self.rejected + 1
    return false
  }
  let nis = match covariance.inverse() {
    None => 1000000000.0
    Some(inverse) => vector_dot(innovation, inverse.multiply_vector(innovation))
  }
  self.samples = self.samples + 1
  self.last_nis = nis
  self.nis_stats.add(nis)
  if nis <= self.threshold {
    self.accepted = self.accepted + 1
    true
  } else {
    self.rejected = self.rejected + 1
    false
  }
}

///|
pub fn InnovationMonitor::samples(self : InnovationMonitor) -> Int {
  self.samples
}

///|
pub fn InnovationMonitor::accepted(self : InnovationMonitor) -> Int {
  self.accepted
}

///|
pub fn InnovationMonitor::rejected(self : InnovationMonitor) -> Int {
  self.rejected
}

///|
pub fn InnovationMonitor::last_nis(self : InnovationMonitor) -> Double {
  self.last_nis
}

///|
pub fn InnovationMonitor::average_nis(self : InnovationMonitor) -> Double {
  self.nis_stats.mean()
}

///|
pub fn InnovationMonitor::acceptance_rate(self : InnovationMonitor) -> Double {
  if self.samples == 0 {
    0.0
  } else {
    self.accepted.to_double() / self.samples.to_double()
  }
}

///|
pub struct GateSchedule {
  mut base_threshold : Double
  minimum_threshold : Double
  maximum_threshold : Double
  mut consecutive_rejections : Int
  recovery_steps : Int
}

///|
pub fn GateSchedule::new(
  base_threshold : Double,
  minimum_threshold : Double,
  maximum_threshold : Double,
  recovery_steps : Int,
) -> GateSchedule {
  let low = if minimum_threshold < 0.0 { 0.0 } else { minimum_threshold }
  let high = if maximum_threshold < low { low } else { maximum_threshold }
  {
    base_threshold: if base_threshold < low {
      low
    } else if base_threshold > high {
      high
    } else {
      base_threshold
    },
    minimum_threshold: low,
    maximum_threshold: high,
    consecutive_rejections: 0,
    recovery_steps: if recovery_steps < 1 {
      1
    } else {
      recovery_steps
    },
  }
}

///|
pub fn GateSchedule::observe(
  self : GateSchedule,
  result : UpdateResult,
) -> Double {
  match result {
    Accepted => {
      self.consecutive_rejections = 0
      self.base_threshold = self.base_threshold -
        (self.base_threshold - self.minimum_threshold) /
        self.recovery_steps.to_double()
    }
    RejectedByGate | InvalidMeasurement | SingularInnovation => {
      self.consecutive_rejections = self.consecutive_rejections + 1
      self.base_threshold = self.base_threshold +
        (self.maximum_threshold - self.base_threshold) * 0.25
    }
    MissingMeasurement => ()
  }
  if self.base_threshold < self.minimum_threshold {
    self.base_threshold = self.minimum_threshold
  }
  if self.base_threshold > self.maximum_threshold {
    self.base_threshold = self.maximum_threshold
  }
  self.base_threshold
}

///|
pub fn GateSchedule::threshold(self : GateSchedule) -> Double {
  self.base_threshold
}

///|
pub fn GateSchedule::consecutive_rejections(self : GateSchedule) -> Int {
  self.consecutive_rejections
}

///|
/// A simple score for comparing two filter runs. Lower is better; rejected
/// observations and covariance failures are penalized separately.
pub fn filter_quality_score(
  metrics : ErrorMetrics,
  rejected : Int,
  covariance_failures : Int,
) -> Double {
  metrics.rmse() +
  metrics.mae() * 0.5 +
  rejected.to_double() * 0.01 +
  covariance_failures.to_double() * 1.0
}

///|
pub fn compare_filter_quality(left : Double, right : Double) -> Int {
  if left < right {
    -1
  } else if left > right {
    1
  } else {
    0
  }
}