///|
/// How independent measurements are combined before the state update.
pub(all) enum FusionStrategy {
  WeightedMean
  Median
  BestConfidence
  TrimmedMean(Int)
} derive(Debug, Eq)

///|
pub struct FusionMeasurement {
  sensor : String
  timestamp : Int
  values : Array[Double]
  covariance : Matrix
  confidence : Double
} derive(Debug)

///|
pub fn FusionMeasurement::new(
  sensor : String,
  timestamp : Int,
  values : Array[Double],
  covariance : Matrix,
  confidence : Double,
) -> FusionMeasurement {
  {
    sensor,
    timestamp,
    values: values.copy(),
    covariance: covariance.copy(),
    confidence: if confidence < 0.0 {
      0.0
    } else {
      confidence
    },
  }
}

///|
pub fn FusionMeasurement::sensor(self : FusionMeasurement) -> String {
  self.sensor
}

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

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

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

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

///|
pub fn FusionMeasurement::is_valid(
  self : FusionMeasurement,
  dimension : Int,
) -> Bool {
  self.values.length() == dimension &&
  dimension > 0 &&
  vector_is_finite(self.values) &&
  covariance_is_psd(self.covariance, 0.001) &&
  self.covariance.rows() == dimension &&
  self.covariance.cols() == dimension
}

///|
pub struct FusionResult {
  strategy : FusionStrategy
  timestamp : Int
  values : Array[Double]
  covariance : Matrix
  used : Int
  rejected : Int
} derive(Debug)

///|
pub fn FusionResult::strategy(self : FusionResult) -> FusionStrategy {
  self.strategy
}

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

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

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

///|
pub fn FusionResult::used(self : FusionResult) -> Int {
  self.used
}

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

///|
fn sort_numbers(values : Array[Double]) -> Array[Double] {
  let result = values.copy()
  for i in 1.. 0 && result[index - 1] > value {
      result[index] = result[index - 1]
      index = index - 1
    }
    result[index] = value
  }
  result
}

///|
fn scalar_fusion(
  measurements : Array[FusionMeasurement],
  strategy : FusionStrategy,
) -> (Array[Double], Double, Int, Int) {
  if measurements.length() == 0 {
    return ([], 0.0, 0, 0)
  }
  let dimension = measurements[0].values().length()
  let valid : Array[FusionMeasurement] = []
  for measurement in measurements {
    if measurement.is_valid(dimension) {
      valid.push(measurement)
    }
  }
  if valid.length() == 0 {
    return ([], 0.0, 0, measurements.length())
  }
  let mut result = Array::make(dimension, 0.0)
  let mut total_weight = 0.0
  match strategy {
    WeightedMean => {
      for measurement in valid {
        let weight = if measurement.confidence() <= 0.0 {
          1.0
        } else {
          measurement.confidence()
        }
        result = vector_axpy(weight, measurement.values(), result)
        total_weight = total_weight + weight
      }
      if total_weight > 0.0 {
        result = vector_scale(result, 1.0 / total_weight)
      }
    }
    BestConfidence => {
      let mut best = valid[0]
      for measurement in valid {
        if measurement.confidence() > best.confidence() {
          best = measurement
        }
      }
      result = best.values()
      total_weight = best.confidence()
    }
    Median | TrimmedMean(_) => {
      for component in 0.. {
          measurement.values()[component]
        })
        let sorted = sort_numbers(component_values)
        let trim = match strategy {
          TrimmedMean(value) => if value < 0 { 0 } else { value }
          Median => sorted.length() / 2
          WeightedMean | BestConfidence => 0
        }
        let start = if trim >= sorted.length() { 0 } else { trim }
        let end = if trim >= sorted.length() - start {
          sorted.length()
        } else {
          sorted.length() - trim
        }
        let selected = if start >= end {
          sorted
        } else {
          sorted[start:end].to_owned()
        }
        result[component] = vector_mean(selected)
      }
      total_weight = valid.length().to_double()
    }
  }
  (result, total_weight, valid.length(), measurements.length() - valid.length())
}

///|
pub fn fuse_measurements(
  measurements : Array[FusionMeasurement],
  strategy : FusionStrategy,
  covariance_floor : Double,
) -> FusionResult {
  if measurements.length() == 0 {
    return {
      strategy,
      timestamp: 0,
      values: [],
      covariance: Matrix::zeros(0, 0),
      used: 0,
      rejected: 0,
    }
  }
  let (values, weight, used, rejected) = scalar_fusion(measurements, strategy)
  let dimension = values.length()
  let mut timestamp = measurements[0].timestamp()
  for measurement in measurements {
    if measurement.timestamp() > timestamp {
      timestamp = measurement.timestamp()
    }
  }
  let covariance = if dimension == 0 {
    Matrix::zeros(0, 0)
  } else {
    Matrix::identity(dimension)
    .scale(1.0 / (if weight <= 0.0 { 1.0 } else { weight }))
    .add_diagonal(if covariance_floor < 0.0 { 0.0 } else { covariance_floor })
  }
  { strategy, timestamp, values, covariance, used, rejected }
}

///|
pub struct Synchronizer {
  tolerance : Int
  mut last_timestamp : Int?
  mut accepted : Int
  mut rejected : Int
}

///|
pub fn Synchronizer::new(tolerance : Int) -> Synchronizer {
  {
    tolerance: if tolerance < 0 {
      0
    } else {
      tolerance
    },
    last_timestamp: None,
    accepted: 0,
    rejected: 0,
  }
}

///|
pub fn Synchronizer::accept(self : Synchronizer, timestamp : Int) -> Bool {
  match self.last_timestamp {
    None => {
      self.last_timestamp = Some(timestamp)
      self.accepted = self.accepted + 1
      true
    }
    Some(previous) =>
      if (timestamp - previous).abs() <= self.tolerance {
        self.last_timestamp = Some(timestamp)
        self.accepted = self.accepted + 1
        true
      } else {
        self.rejected = self.rejected + 1
        false
      }
  }
}

///|
pub fn Synchronizer::tolerance(self : Synchronizer) -> Int {
  self.tolerance
}

///|
pub fn Synchronizer::last_timestamp(self : Synchronizer) -> Int? {
  self.last_timestamp
}

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

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

///|
pub fn Synchronizer::reset(self : Synchronizer) -> Unit {
  self.last_timestamp = None
  self.accepted = 0
  self.rejected = 0
}

///|
/// Bounded multi-sensor measurement collection.
pub struct MeasurementWindow {
  measurements : Array[FusionMeasurement]
  capacity : Int
  dimension : Int
}

///|
pub fn MeasurementWindow::new(
  capacity : Int,
  dimension : Int,
) -> MeasurementWindow {
  {
    measurements: [],
    capacity: if capacity < 0 {
      0
    } else {
      capacity
    },
    dimension: if dimension < 0 {
      0
    } else {
      dimension
    },
  }
}

///|
pub fn MeasurementWindow::push(
  self : MeasurementWindow,
  measurement : FusionMeasurement,
) -> Bool {
  if self.capacity == 0 || !measurement.is_valid(self.dimension) {
    return false
  }
  if self.measurements.length() >= self.capacity {
    for i in 1.. Array[FusionMeasurement] {
  self.measurements.copy()
}

///|
pub fn MeasurementWindow::length(self : MeasurementWindow) -> Int {
  self.measurements.length()
}

///|
pub fn MeasurementWindow::fuse(
  self : MeasurementWindow,
  strategy : FusionStrategy,
) -> FusionResult {
  fuse_measurements(self.measurements, strategy, 0.000000000001)
}

///|
pub fn MeasurementWindow::clear(self : MeasurementWindow) -> Unit {
  self.measurements.clear()
}

///|
pub fn robust_fusion(
  measurements : Array[FusionMeasurement],
  tuning : Double,
) -> FusionResult {
  let policy = ResidualPolicy::new(
    if tuning <= 0.0 {
      1.0
    } else {
      tuning
    },
    if tuning <= 0.0 {
      5.0
    } else {
      tuning * 4.0
    },
    0.1,
  )
  let filtered : Array[FusionMeasurement] = []
  if measurements.length() == 0 {
    return fuse_measurements([], WeightedMean, 0.0)
  }
  let reference = measurements[0].values()
  for measurement in measurements {
    if measurement.values().length() == reference.length() {
      let action = policy.classify(
        vector_l2_norm(vector_sub(measurement.values(), reference)),
      )
      match action {
        Reject => ()
        Accept => filtered.push(measurement)
        Downweight(_) => filtered.push(measurement)
      }
    }
  }
  fuse_measurements(filtered, WeightedMean, 0.000000000001)
}