///|
/// A serializable-in-memory checkpoint for crash recovery and replay.
pub struct FilterCheckpoint {
  state : Array[Double]
  covariance : Matrix
  timestamp : Int
} derive(Debug)

///|
pub fn FilterCheckpoint::new(
  state : Array[Double],
  covariance : Matrix,
  timestamp : Int,
) -> FilterCheckpoint {
  { state: state.copy(), covariance: covariance.copy(), timestamp }
}

///|
pub fn FilterCheckpoint::state(self : FilterCheckpoint) -> Array[Double] {
  self.state.copy()
}

///|
pub fn FilterCheckpoint::covariance(self : FilterCheckpoint) -> Matrix {
  self.covariance.copy()
}

///|
pub fn FilterCheckpoint::timestamp(self : FilterCheckpoint) -> Int {
  self.timestamp
}

///|
pub fn KalmanND::checkpoint(
  self : KalmanND,
  timestamp : Int,
) -> FilterCheckpoint {
  FilterCheckpoint::new(self.state(), self.covariance(), timestamp)
}

///|
pub fn KalmanND::restore_checkpoint(
  self : KalmanND,
  checkpoint : FilterCheckpoint,
) -> Bool {
  self.restore(checkpoint.state(), checkpoint.covariance())
}

///|
/// Estimate lifecycle used by a tracker manager.
pub(all) enum TrackLifecycle {
  Tentative
  Confirmed
  Lost
  Deleted
} derive(Debug, Eq)

///|
pub struct TrackManager {
  mut lifecycle : TrackLifecycle
  confirmation_hits : Int
  deletion_misses : Int
  mut hits : Int
  mut misses : Int
  mut age : Int
}

///|
pub fn TrackManager::new(
  confirmation_hits : Int,
  deletion_misses : Int,
) -> TrackManager {
  {
    lifecycle: Tentative,
    confirmation_hits: if confirmation_hits < 1 {
      1
    } else {
      confirmation_hits
    },
    deletion_misses: if deletion_misses < 1 {
      1
    } else {
      deletion_misses
    },
    hits: 0,
    misses: 0,
    age: 0,
  }
}

///|
pub fn TrackManager::observe(
  self : TrackManager,
  result : UpdateResult,
) -> TrackLifecycle {
  self.age = self.age + 1
  match result {
    Accepted => {
      self.hits = self.hits + 1
      self.misses = 0
      if self.lifecycle is Tentative && self.hits >= self.confirmation_hits {
        self.lifecycle = Confirmed
      } else if self.lifecycle is Lost {
        self.lifecycle = Confirmed
      }
    }
    RejectedByGate
    | InvalidMeasurement
    | SingularInnovation
    | MissingMeasurement => {
      self.misses = self.misses + 1
      if self.lifecycle is Confirmed && self.misses >= 1 {
        self.lifecycle = Lost
      }
      if self.lifecycle is Lost && self.misses >= self.deletion_misses {
        self.lifecycle = Deleted
      }
    }
  }
  self.lifecycle
}

///|
pub fn TrackManager::lifecycle(self : TrackManager) -> TrackLifecycle {
  self.lifecycle
}

///|
pub fn TrackManager::hits(self : TrackManager) -> Int {
  self.hits
}

///|
pub fn TrackManager::misses(self : TrackManager) -> Int {
  self.misses
}

///|
pub fn TrackManager::age(self : TrackManager) -> Int {
  self.age
}

///|
pub fn TrackManager::reset(self : TrackManager) -> Unit {
  self.lifecycle = Tentative
  self.hits = 0
  self.misses = 0
  self.age = 0
}

///|
/// Per-sensor health state. It can be used to down-weight or disable a sensor
/// before its failures destabilize a global track.
pub struct SensorHealth {
  name : String
  mut score : Double
  decay : Double
  recovery : Double
  minimum_score : Double
  mut accepted : Int
  mut rejected : Int
}

///|
pub fn SensorHealth::new(
  name : String,
  decay : Double,
  recovery : Double,
) -> SensorHealth {
  {
    name,
    score: 1.0,
    decay: if decay < 0.0 {
      0.0
    } else if decay > 1.0 {
      1.0
    } else {
      decay
    },
    recovery: if recovery < 0.0 {
      0.0
    } else if recovery > 1.0 {
      1.0
    } else {
      recovery
    },
    minimum_score: 0.01,
    accepted: 0,
    rejected: 0,
  }
}

///|
pub fn SensorHealth::observe(
  self : SensorHealth,
  result : UpdateResult,
) -> Unit {
  match result {
    Accepted => {
      self.accepted = self.accepted + 1
      self.score = self.score + self.recovery * (1.0 - self.score)
    }
    RejectedByGate
    | InvalidMeasurement
    | SingularInnovation
    | MissingMeasurement => {
      self.rejected = self.rejected + 1
      self.score = self.score * (1.0 - self.decay)
    }
  }
  if self.score < self.minimum_score {
    self.score = self.minimum_score
  }
}

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

///|
pub fn SensorHealth::score(self : SensorHealth) -> Double {
  self.score
}

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

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

///|
pub fn SensorHealth::is_usable(self : SensorHealth, threshold : Double) -> Bool {
  self.score >= threshold
}

///|
/// Accumulate covariance from a stream without storing every sample.
pub struct CovarianceAccumulator {
  dimension : Int
  mut count : Int
  mut mean : Array[Double]
  mut scatter : Matrix
}

///|
pub fn CovarianceAccumulator::new(dimension : Int) -> CovarianceAccumulator {
  let size = if dimension < 0 { 0 } else { dimension }
  {
    dimension: size,
    count: 0,
    mean: Array::make(size, 0.0),
    scatter: Matrix::zeros(size, size),
  }
}

///|
pub fn CovarianceAccumulator::add(
  self : CovarianceAccumulator,
  sample : Array[Double],
) -> Bool {
  if sample.length() != self.dimension || !vector_is_finite(sample) {
    return false
  }
  self.count = self.count + 1
  let difference = vector_sub(sample, self.mean)
  self.mean = vector_add(
    self.mean,
    vector_scale(difference, 1.0 / self.count.to_double()),
  )
  let updated_difference = vector_sub(sample, self.mean)
  self.scatter = self.scatter.add(Matrix::outer(difference, updated_difference))
  true
}

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

///|
pub fn CovarianceAccumulator::mean(
  self : CovarianceAccumulator,
) -> Array[Double] {
  self.mean.copy()
}

///|
pub fn CovarianceAccumulator::covariance(
  self : CovarianceAccumulator,
) -> Matrix {
  if self.count < 2 {
    Matrix::zeros(self.dimension, self.dimension)
  } else {
    self.scatter.scale(1.0 / (self.count - 1).to_double())
  }
}

///|
pub fn CovarianceAccumulator::reset(self : CovarianceAccumulator) -> Unit {
  self.count = 0
  self.mean = Array::make(self.dimension, 0.0)
  self.scatter = Matrix::zeros(self.dimension, self.dimension)
}

///|
/// Bounded residual reweighting for robust updates. The output can be used to
/// inflate measurement covariance before calling `KalmanND::update`.
pub fn robust_weight(residual : Double, tuning : Double) -> Double {
  let scale = if tuning <= 0.0 { 1.0 } else { tuning }
  let magnitude = residual.abs()
  if magnitude <= scale {
    1.0
  } else {
    scale / magnitude
  }
}

///|
pub fn robust_covariance_factor(
  innovation : Array[Double],
  tuning : Double,
) -> Double {
  if innovation.length() == 0 {
    return 1.0
  }
  let mut factor = 1.0
  for value in innovation {
    let weight = robust_weight(value, tuning)
    if weight < factor {
      factor = weight
    }
  }
  if factor <= 0.000000000001 {
    1000000.0
  } else {
    1.0 / (factor * factor)
  }
}

///|
pub struct MeasurementSchedule {
  period : Int
  mut elapsed : Int
}

///|
pub fn MeasurementSchedule::new(period : Int) -> MeasurementSchedule {
  { period: if period < 1 { 1 } else { period }, elapsed: 0 }
}

///|
pub fn MeasurementSchedule::tick(self : MeasurementSchedule) -> Bool {
  self.elapsed = self.elapsed + 1
  if self.elapsed >= self.period {
    self.elapsed = 0
    true
  } else {
    false
  }
}

///|
pub fn MeasurementSchedule::period(self : MeasurementSchedule) -> Int {
  self.period
}

///|
pub fn MeasurementSchedule::elapsed(self : MeasurementSchedule) -> Int {
  self.elapsed
}