// The particle filter: a cloud of hypotheses instead of a Gaussian. It is the
// estimator that survives a measurement model the other two cannot express, and
// it is the one that needs a seeded generator to be reproducible.

///|
/// A particle filter over any plant that implements the model contract.
///
/// The particles are drawn around the prior at construction and then carried by
/// the model itself, so the filter never assumes the state distribution stays
/// Gaussian. Resampling is systematic and only runs when the effective sample
/// size falls below half the cloud, which keeps the diversity a plain
/// resample-every-step filter loses.
pub(all) struct ParticleFilter {
  model : &@contract.Model
  process : @linear.Mat
  measurement : @linear.Mat
  mut particles : Array[@linear.Vec]
  mut weights : Array[Double]
  rng : &@rand_contract.Rng
}

///|
/// A particle filter with `samples` particles drawn around `state`.
pub fn ParticleFilter::new(
  model~ : &@contract.Model,
  process~ : @linear.Mat,
  measurement~ : @linear.Mat,
  state~ : @linear.Vec,
  covariance~ : @linear.Mat,
  rng~ : &@rand_contract.Rng,
  samples~ : Int32 = 1000,
) -> ParticleFilter raise @linear.Refused {
  let n = state.len()
  if samples < 2 {
    raise @linear.Refused::Range(got=samples.to_double(), low=2.0, high=1.0e9)
  }
  if covariance.rows != n || covariance.cols != n {
    raise @linear.Refused::Dimension(got=covariance.rows, want=n)
  }
  let jitter = @linear.Mat::identity(n).scale(1.0e-12)
  let factor = @linear.cholesky(covariance.add(jitter))
  let particles = Array::make(samples, state)
  let weights = Array::make(samples, 1.0 / samples.to_double())
  for i in 0.. Array[@linear.Vec] {
  self.particles
}

///|
/// The weight of each particle.
pub fn ParticleFilter::weights(self : ParticleFilter) -> Array[Double] {
  self.weights
}

///|
pub impl @contract.Estimator for ParticleFilter with fn predict(self, u, dt) {
  let factor = matrix_sqrt(self.process)
  let n = factor.rows
  for i in 0.. Double {
  let m = innovation.len()
  let column = @linear.Mat::zeros(m, 1)
  for i in 0.. -0.5 * innovation.dot(innovation)
  }
  if exponent < -700.0 {
    0.0
  } else {
    @math.exp(exponent)
  }
}

///|
/// Systematic resampling: one uniform draw, then even steps through the
/// cumulative weights.
fn ParticleFilter::resample(self : ParticleFilter) -> Unit {
  let count = self.particles.length()
  let spacing = 1.0 / count.to_double()
  let first = @rand.random(self.rng) * spacing
  let resampled = Array::make(count, self.particles[0])
  let mut cursor = 0
  let mut cumulative = self.weights[0]
  for i in 0.. cumulative && cursor < count - 1 {
      cursor = cursor + 1
      cumulative = cumulative + self.weights[cursor]
    }
    resampled[i] = self.particles[cursor].clone()
  }
  for i in 0..