///|
/// Static configuration for a sensor channel.
pub struct SensorConfiguration {
  name : String
  dimension : Int
  period : Int
  timeout : Int
  covariance : Matrix
  mut enabled : Bool
} derive(Debug)

///|
pub fn SensorConfiguration::new(
  name : String,
  dimension : Int,
  period : Int,
  timeout : Int,
  covariance : Matrix,
) -> SensorConfiguration {
  let safe_dimension = if dimension < 0 { 0 } else { dimension }
  let safe_period = if period < 1 { 1 } else { period }
  let safe_timeout = if timeout < safe_period {
    safe_period * 3
  } else {
    timeout
  }
  let safe_covariance = if covariance.rows() == safe_dimension &&
    covariance.cols() == safe_dimension {
    covariance.copy()
  } else {
    Matrix::identity(safe_dimension)
  }
  {
    name,
    dimension: safe_dimension,
    period: safe_period,
    timeout: safe_timeout,
    covariance: safe_covariance,
    enabled: true,
  }
}

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

///|
pub fn SensorConfiguration::dimension(self : SensorConfiguration) -> Int {
  self.dimension
}

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

///|
pub fn SensorConfiguration::timeout(self : SensorConfiguration) -> Int {
  self.timeout
}

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

///|
pub fn SensorConfiguration::enabled(self : SensorConfiguration) -> Bool {
  self.enabled
}

///|
pub fn SensorConfiguration::set_enabled(
  self : SensorConfiguration,
  enabled : Bool,
) -> Unit {
  self.enabled = enabled
}

///|
pub fn SensorConfiguration::with_covariance(
  self : SensorConfiguration,
  covariance : Matrix,
) -> SensorConfiguration {
  if covariance.rows() != self.dimension || covariance.cols() != self.dimension {
    self
  } else {
    {
      name: self.name,
      dimension: self.dimension,
      period: self.period,
      timeout: self.timeout,
      covariance: covariance.copy(),
      enabled: self.enabled,
    }
  }
}

///|
/// Timestamp discipline and jitter statistics for an input channel.
pub struct SensorClock {
  period : Int
  mut last_timestamp : Int?
  mut samples : Int
  mut late : Int
  mut early : Int
  mut jitter : RunningStats
}

///|
pub fn SensorClock::new(period : Int) -> SensorClock {
  {
    period: if period < 1 {
      1
    } else {
      period
    },
    last_timestamp: None,
    samples: 0,
    late: 0,
    early: 0,
    jitter: RunningStats::new(),
  }
}

///|
pub fn SensorClock::observe(self : SensorClock, timestamp : Int) -> Bool {
  match self.last_timestamp {
    None => {
      self.last_timestamp = Some(timestamp)
      self.samples = 1
      true
    }
    Some(previous) => {
      let delta = timestamp - previous
      if delta <= 0 {
        self.late = self.late + 1
        return false
      }
      let error = delta - self.period
      self.jitter.add(error.to_double())
      if delta > self.period {
        self.late = self.late + 1
      }
      if delta < self.period {
        self.early = self.early + 1
      }
      self.last_timestamp = Some(timestamp)
      self.samples = self.samples + 1
      true
    }
  }
}

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

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

///|
pub fn SensorClock::late(self : SensorClock) -> Int {
  self.late
}

///|
pub fn SensorClock::early(self : SensorClock) -> Int {
  self.early
}

///|
pub fn SensorClock::jitter_mean(self : SensorClock) -> Double {
  self.jitter.mean()
}

///|
pub fn SensorClock::jitter_variance(self : SensorClock) -> Double {
  self.jitter.variance()
}

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

///|
pub fn SensorClock::reset(self : SensorClock) -> Unit {
  self.last_timestamp = None
  self.samples = 0
  self.late = 0
  self.early = 0
  self.jitter = RunningStats::new()
}

///|
/// Affine calibration transform applied component-wise to raw readings.
pub struct CalibrationTransform {
  offset : Array[Double]
  scale : Array[Double]
}

///|
pub fn CalibrationTransform::new(
  offset : Array[Double],
  scale : Array[Double],
) -> CalibrationTransform {
  if offset.length() != scale.length() {
    { offset: [], scale: [] }
  } else {
    let safe_scale = scale.map(value => {
      if value.is_nan() || value.abs() < 0.000000000001 {
        1.0
      } else {
        value
      }
    })
    { offset: offset.copy(), scale: safe_scale }
  }
}

///|
pub fn CalibrationTransform::dimension(self : CalibrationTransform) -> Int {
  self.offset.length()
}

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

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

///|
pub fn CalibrationTransform::apply(
  self : CalibrationTransform,
  values : Array[Double],
) -> Array[Double] {
  if values.length() != self.dimension() {
    return []
  }
  Array::makei(values.length(), index => {
    (values[index] - self.offset[index]) * self.scale[index]
  })
}

///|
pub fn CalibrationTransform::inverse(
  self : CalibrationTransform,
  values : Array[Double],
) -> Array[Double] {
  if values.length() != self.dimension() {
    return []
  }
  Array::makei(values.length(), index => {
    values[index] / self.scale[index] + self.offset[index]
  })
}

///|
pub fn CalibrationTransform::identity(dimension : Int) -> CalibrationTransform {
  let size = if dimension < 0 { 0 } else { dimension }
  CalibrationTransform::new(Array::make(size, 0.0), Array::make(size, 1.0))
}

///|
/// Estimate an affine offset from reference-aligned samples.
pub struct SensorCalibrator {
  dimension : Int
  mut samples : Int
  mut sum : Array[Double]
  mut sum_squared : Matrix
}

///|
pub fn SensorCalibrator::new(dimension : Int) -> SensorCalibrator {
  let size = if dimension < 0 { 0 } else { dimension }
  {
    dimension: size,
    samples: 0,
    sum: Array::make(size, 0.0),
    sum_squared: Matrix::zeros(size, size),
  }
}

///|
pub fn SensorCalibrator::add(
  self : SensorCalibrator,
  raw : Array[Double],
  reference : Array[Double],
) -> Bool {
  if raw.length() != self.dimension ||
    reference.length() != self.dimension ||
    !vector_is_finite(raw) ||
    !vector_is_finite(reference) {
    return false
  }
  let error = vector_sub(raw, reference)
  self.sum = vector_add(self.sum, error)
  self.sum_squared = self.sum_squared.add(Matrix::outer(error, error))
  self.samples = self.samples + 1
  true
}

///|
pub fn SensorCalibrator::dimension(self : SensorCalibrator) -> Int {
  self.dimension
}

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

///|
pub fn SensorCalibrator::offset(self : SensorCalibrator) -> Array[Double] {
  if self.samples == 0 {
    Array::make(self.dimension, 0.0)
  } else {
    vector_scale(self.sum, 1.0 / self.samples.to_double())
  }
}

///|
pub fn SensorCalibrator::error_covariance(self : SensorCalibrator) -> Matrix {
  if self.samples == 0 {
    return Matrix::zeros(self.dimension, self.dimension)
  }
  let mean = self.offset()
  let correction = Matrix::outer(mean, mean).scale(self.samples.to_double())
  self.sum_squared
  .sub(correction)
  .scale(1.0 / self.samples.to_double())
  .symmetric_part()
}

///|
pub fn SensorCalibrator::transform(
  self : SensorCalibrator,
) -> CalibrationTransform {
  CalibrationTransform::new(self.offset(), Array::make(self.dimension, 1.0))
}

///|
pub fn SensorCalibrator::reset(self : SensorCalibrator) -> Unit {
  self.samples = 0
  self.sum = Array::make(self.dimension, 0.0)
  self.sum_squared = Matrix::zeros(self.dimension, self.dimension)
}

///|
/// Builder for validated packets with a reusable calibration transform.
pub struct SensorPacketBuilder {
  configuration : SensorConfiguration
  mut calibration : CalibrationTransform
  mut built : Int
  mut rejected : Int
}

///|
pub fn SensorPacketBuilder::new(
  configuration : SensorConfiguration,
) -> SensorPacketBuilder {
  {
    configuration,
    calibration: CalibrationTransform::identity(configuration.dimension()),
    built: 0,
    rejected: 0,
  }
}

///|
pub fn SensorPacketBuilder::set_calibration(
  self : SensorPacketBuilder,
  calibration : CalibrationTransform,
) -> Bool {
  if calibration.dimension() != self.configuration.dimension() {
    return false
  }
  self.calibration = calibration
  true
}

///|
pub fn SensorPacketBuilder::build(
  self : SensorPacketBuilder,
  timestamp : Int,
  values : Array[Double],
) -> ObservationPacket? {
  if !self.configuration.enabled() ||
    values.length() != self.configuration.dimension() ||
    !vector_is_finite(values) {
    self.rejected = self.rejected + 1
    return None
  }
  let calibrated = self.calibration.apply(values)
  let packet = ObservationPacket::new(
    timestamp,
    self.configuration.name(),
    calibrated,
    self.configuration.covariance(),
  )
  if !packet.is_valid() {
    self.rejected = self.rejected + 1
    None
  } else {
    self.built = self.built + 1
    Some(packet)
  }
}

///|
pub fn SensorPacketBuilder::configuration(
  self : SensorPacketBuilder,
) -> SensorConfiguration {
  self.configuration
}

///|
pub fn SensorPacketBuilder::built(self : SensorPacketBuilder) -> Int {
  self.built
}

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

///|
pub fn SensorPacketBuilder::reset(self : SensorPacketBuilder) -> Unit {
  self.built = 0
  self.rejected = 0
}

///|
/// Decide whether a packet timestamp is still usable relative to a clock.
pub fn packet_is_fresh(
  packet : ObservationPacket,
  now : Int,
  timeout : Int,
) -> Bool {
  let safe_timeout = if timeout < 0 { 0 } else { timeout }
  let age = now - packet.timestamp()
  age >= 0 && age <= safe_timeout
}

///|
pub fn packet_age(packet : ObservationPacket, now : Int) -> Int {
  let age = now - packet.timestamp()
  if age < 0 {
    0
  } else {
    age
  }
}