///|
/// Small deterministic pseudo-random generator for repeatable examples and
/// benchmarks. It is not intended for cryptographic use.
pub struct DeterministicRng {
  mut state : Int
}

///|
pub fn DeterministicRng::new(seed : Int) -> DeterministicRng {
  { state: if seed <= 0 { 1 } else { seed % 1000003 } }
}

///|
pub fn DeterministicRng::next(self : DeterministicRng) -> Int {
  self.state = (self.state * 997 + 101) % 1000003
  self.state
}

///|
pub fn DeterministicRng::unit(self : DeterministicRng) -> Double {
  self.next().to_double() / 1000003.0
}

///|
pub fn DeterministicRng::symmetric(
  self : DeterministicRng,
  amplitude : Double,
) -> Double {
  (self.unit() * 2.0 - 1.0) * amplitude
}

///|
pub fn DeterministicRng::bounded_int(
  self : DeterministicRng,
  bound : Int,
) -> Int {
  if bound <= 0 {
    0
  } else {
    self.next() % bound
  }
}

///|
pub struct SensorSample {
  timestamp : Int
  truth : Array[Double]
  measurement : Array[Double]
  missing : Bool
  outlier : Bool
} derive(Debug)

///|
pub fn SensorSample::new(
  timestamp : Int,
  truth : Array[Double],
  measurement : Array[Double],
  missing : Bool,
  outlier : Bool,
) -> SensorSample {
  {
    timestamp,
    truth: truth.copy(),
    measurement: measurement.copy(),
    missing,
    outlier,
  }
}

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

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

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

///|
pub fn SensorSample::missing(self : SensorSample) -> Bool {
  self.missing
}

///|
pub fn SensorSample::outlier(self : SensorSample) -> Bool {
  self.outlier
}

///|
pub struct SimulationResult {
  samples : Array[SensorSample]
  truth : Array[Array[Double]]
  estimates : Array[Array[Double]]
  metrics : ErrorMetrics
} derive(Debug)

///|
pub fn SimulationResult::samples(
  self : SimulationResult,
) -> Array[SensorSample] {
  self.samples
}

///|
pub fn SimulationResult::truth(self : SimulationResult) -> Array[Array[Double]] {
  self.truth.map(value => value.copy())
}

///|
pub fn SimulationResult::estimates(
  self : SimulationResult,
) -> Array[Array[Double]] {
  self.estimates.map(value => value.copy())
}

///|
pub fn SimulationResult::metrics(self : SimulationResult) -> ErrorMetrics {
  self.metrics
}

///|
/// Generate a two-dimensional constant-velocity trajectory with repeatable
/// sensor noise, dropped packets, and occasional gross outliers.
pub fn simulate_constant_velocity_2d(
  steps : Int,
  dt : Double,
  initial_position : Array[Double],
  velocity : Array[Double],
  measurement_noise : Double,
  missing_period : Int,
  outlier_period : Int,
  seed : Int,
) -> Array[SensorSample] {
  if steps <= 0 || initial_position.length() != 2 || velocity.length() != 2 {
    return []
  }
  let rng = DeterministicRng::new(seed)
  let samples : Array[SensorSample] = []
  let truth = initial_position.copy()
  let safe_noise = if measurement_noise < 0.0 { 0.0 } else { measurement_noise }
  for index in 0.. 0 && index > 0 && index % missing_period == 0
    let outlier = outlier_period > 0 && index > 0 && index % outlier_period == 0
    let magnitude = if outlier { safe_noise * 18.0 + 10.0 } else { safe_noise }
    let measurement = [
      current_truth[0] + rng.symmetric(magnitude),
      current_truth[1] + rng.symmetric(magnitude),
    ]
    samples.push({
      timestamp: index,
      truth: current_truth,
      measurement,
      missing,
      outlier,
    })
    truth[0] = truth[0] + velocity[0] * dt
    truth[1] = truth[1] + velocity[1] * dt
  }
  samples
}

///|
/// Run the ready-to-use tracker against a simulated two-dimensional stream.
pub fn run_constant_velocity_2d_simulation(
  steps : Int,
  dt : Double,
  measurement_noise : Double,
  missing_period : Int,
  outlier_period : Int,
  seed : Int,
) -> SimulationResult {
  let samples = simulate_constant_velocity_2d(
    steps,
    dt,
    [0.0, 0.0],
    [1.25, -0.45],
    measurement_noise,
    missing_period,
    outlier_period,
    seed,
  )
  let tracker = ConstantVelocityTracker2D::new(
    [0.0, 0.0],
    [0.0, 0.0],
    10.0,
    0.15,
    if measurement_noise <= 0.0 {
      0.01
    } else {
      measurement_noise * measurement_noise
    },
  )
  let truth : Array[Array[Double]] = []
  let estimates : Array[Array[Double]] = []
  for sample in samples {
    truth.push(sample.truth())
    if sample.missing() {
      tracker.predict(dt)
    } else {
      tracker.step_position(
        sample.timestamp(),
        sample.measurement()[0],
        sample.measurement()[1],
      )
      |> ignore
    }
    estimates.push(state_positions(tracker.state()))
  }
  { samples, truth, estimates, metrics: evaluate_errors(estimates, truth) }
}

///|
pub fn simulate_scalar_measurements(
  steps : Int,
  initial_value : Double,
  drift : Double,
  noise : Double,
  missing_period : Int,
  outlier_period : Int,
  seed : Int,
) -> Array[SensorSample] {
  if steps <= 0 {
    return []
  }
  let rng = DeterministicRng::new(seed)
  let samples : Array[SensorSample] = []
  let mut truth_value = initial_value
  for index in 0.. 0 && index > 0 && index % missing_period == 0
    let outlier = outlier_period > 0 && index > 0 && index % outlier_period == 0
    let error = if outlier { noise * 15.0 + 5.0 } else { rng.symmetric(noise) }
    samples.push({
      timestamp: index,
      truth,
      measurement: [truth_value + error],
      missing,
      outlier,
    })
    truth_value = truth_value + drift
  }
  samples
}

///|
pub fn run_scalar_simulation(
  steps : Int,
  measurement_noise : Double,
  missing_period : Int,
  outlier_period : Int,
  seed : Int,
) -> SimulationResult {
  let samples = simulate_scalar_measurements(
    steps, 0.0, 0.25, measurement_noise, missing_period, outlier_period, seed,
  )
  let filter = Kalman1D::new(
    0.0,
    1.0,
    0.05,
    measurement_noise * measurement_noise + 0.0001,
  )
  let truth : Array[Array[Double]] = []
  let estimates : Array[Array[Double]] = []
  for sample in samples {
    truth.push(sample.truth())
    if sample.missing() {
      filter.predict(0.25)
    } else {
      filter.predict(0.25)
      filter.update(sample.measurement()[0])
    }
    estimates.push([filter.state()])
  }
  { samples, truth, estimates, metrics: evaluate_errors(estimates, truth) }
}