///|
/// A complete linear state-space model for a sensor-fusion pipeline.
pub struct LinearModel {
  transition : Matrix
  process_noise : Matrix
  observation : Matrix
  measurement_noise : Matrix
  control : Matrix
} derive(Debug)

///|
pub fn LinearModel::new(
  transition : Matrix,
  process_noise : Matrix,
  observation : Matrix,
  measurement_noise : Matrix,
  control : Matrix,
) -> LinearModel {
  { transition, process_noise, observation, measurement_noise, control }
}

///|
pub fn LinearModel::transition(self : LinearModel) -> Matrix {
  self.transition.copy()
}

///|
pub fn LinearModel::process_noise(self : LinearModel) -> Matrix {
  self.process_noise.copy()
}

///|
pub fn LinearModel::observation(self : LinearModel) -> Matrix {
  self.observation.copy()
}

///|
pub fn LinearModel::measurement_noise(self : LinearModel) -> Matrix {
  self.measurement_noise.copy()
}

///|
pub fn LinearModel::control(self : LinearModel) -> Matrix {
  self.control.copy()
}

///|
pub fn LinearModel::state_dimension(self : LinearModel) -> Int {
  self.transition.rows()
}

///|
pub fn LinearModel::measurement_dimension(self : LinearModel) -> Int {
  self.observation.rows()
}

///|
/// Constant-velocity transition for `dimensions` independent axes.  State
/// order is `[position_0..position_n, velocity_0..velocity_n]`.
pub fn constant_velocity_transition(dimensions : Int, dt : Double) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::identity(n * 2)
  for i in 0.. ignore
  }
  result
}

///|
/// White-acceleration process noise for the constant-velocity model.
pub fn constant_velocity_process_noise(
  dimensions : Int,
  dt : Double,
  acceleration_variance : Double,
) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n * 2, n * 2)
  let dt2 = dt * dt
  let dt3 = dt2 * dt
  let dt4 = dt2 * dt2
  for i in 0.. ignore
    result.set(position, velocity, dt3 * 0.5 * acceleration_variance) |> ignore
    result.set(velocity, position, dt3 * 0.5 * acceleration_variance) |> ignore
    result.set(velocity, velocity, dt2 * acceleration_variance) |> ignore
  }
  result
}

///|
/// Constant-acceleration transition for each independent axis.  State order
/// is `[position, velocity, acceleration]` repeated by axis.
pub fn constant_acceleration_transition(
  dimensions : Int,
  dt : Double,
) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::identity(n * 3)
  let half_dt2 = 0.5 * dt * dt
  for i in 0.. ignore
    result.set(position, acceleration, half_dt2) |> ignore
    result.set(velocity, acceleration, dt) |> ignore
  }
  result
}

///|
pub fn constant_acceleration_process_noise(
  dimensions : Int,
  dt : Double,
  jerk_variance : Double,
) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n * 3, n * 3)
  let dt2 = dt * dt
  let dt3 = dt2 * dt
  let dt4 = dt3 * dt
  let dt5 = dt4 * dt
  let dt6 = dt5 * dt
  for i in 0.. ignore
    result.set(p, v, dt5 / 12.0 * jerk_variance) |> ignore
    result.set(v, p, dt5 / 12.0 * jerk_variance) |> ignore
    result.set(p, a, dt4 / 6.0 * jerk_variance) |> ignore
    result.set(a, p, dt4 / 6.0 * jerk_variance) |> ignore
    result.set(v, v, dt4 / 4.0 * jerk_variance) |> ignore
    result.set(v, a, dt3 / 2.0 * jerk_variance) |> ignore
    result.set(a, v, dt3 / 2.0 * jerk_variance) |> ignore
    result.set(a, a, dt2 * jerk_variance) |> ignore
  }
  result
}

///|
/// Observe only the position components of a constant-velocity state.
pub fn position_observation(dimensions : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 2)
  for i in 0.. ignore
  }
  result
}

///|
/// Observe only the velocity components of a constant-velocity state.
pub fn velocity_observation(dimensions : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 2)
  for i in 0.. ignore
  }
  result
}

///|
/// Observe position, velocity, or both from a constant-acceleration state.
pub fn acceleration_observation(dimensions : Int, component : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 3)
  let offset = if component < 0 {
    0
  } else if component > 2 {
    2
  } else {
    component
  }
  for i in 0.. ignore
  }
  result
}

///|
/// Build a usable constant-velocity model for a position sensor.
pub fn constant_velocity_model(
  dimensions : Int,
  dt : Double,
  acceleration_variance : Double,
  measurement_variance : Double,
) -> LinearModel {
  let transition = constant_velocity_transition(dimensions, dt)
  let process_noise = constant_velocity_process_noise(
    dimensions, dt, acceleration_variance,
  )
  let observation = position_observation(dimensions)
  let measurement_noise = Matrix::diagonal(
    if dimensions < 0 {
      0
    } else {
      dimensions
    },
    measurement_variance,
  )
  let control = Matrix::zeros(transition.rows(), 0)
  LinearModel::new(
    transition, process_noise, observation, measurement_noise, control,
  )
}

///|
/// A pair of models for position and velocity sensors sharing one state.
pub struct SensorModelPair {
  position : LinearModel
  velocity : LinearModel
} derive(Debug)

///|
pub fn SensorModelPair::constant_velocity(
  dimensions : Int,
  dt : Double,
  acceleration_variance : Double,
  position_variance : Double,
  velocity_variance : Double,
) -> SensorModelPair {
  let transition = constant_velocity_transition(dimensions, dt)
  let process_noise = constant_velocity_process_noise(
    dimensions, dt, acceleration_variance,
  )
  let position = LinearModel::new(
    transition.copy(),
    process_noise.copy(),
    position_observation(dimensions),
    Matrix::diagonal(dimensions, position_variance),
    Matrix::zeros(transition.rows(), 0),
  )
  let velocity = LinearModel::new(
    transition,
    process_noise,
    velocity_observation(dimensions),
    Matrix::diagonal(dimensions, velocity_variance),
    Matrix::zeros(numeric_state_size(dimensions, 2), 0),
  )
  { position, velocity }
}

///|
pub fn SensorModelPair::position(self : SensorModelPair) -> LinearModel {
  self.position
}

///|
pub fn SensorModelPair::velocity(self : SensorModelPair) -> LinearModel {
  self.velocity
}

///|
fn numeric_state_size(dimensions : Int, order : Int) -> Int {
  if dimensions < 0 || order < 0 {
    0
  } else {
    dimensions * order
  }
}

///|
/// Convert a position/velocity pair into a constant-velocity state vector.
pub fn make_constant_velocity_state(
  position : Array[Double],
  velocity : Array[Double],
) -> Array[Double] {
  if position.length() != velocity.length() {
    return []
  }
  let result = Array::make(position.length() * 2, 0.0)
  for i in 0.. Array[Double] {
  let dimension = state.length() / 2
  Array::makei(dimension, i => state[i])
}

///|
pub fn state_velocities(state : Array[Double]) -> Array[Double] {
  let dimension = state.length() / 2
  Array::makei(dimension, i => state[dimension + i])
}

///|
pub fn make_constant_acceleration_state(
  position : Array[Double],
  velocity : Array[Double],
  acceleration : Array[Double],
) -> Array[Double] {
  if position.length() != velocity.length() ||
    position.length() != acceleration.length() {
    return []
  }
  let result = Array::make(position.length() * 3, 0.0)
  for i in 0.. Array[Double] {
  let dimension = state.length() / 2
  if state.length() != dimension * 2 {
    return []
  }
  let transition = constant_velocity_transition(dimension, dt)
  transition.multiply_vector(state)
}

///|
pub fn predict_constant_acceleration(
  state : Array[Double],
  dt : Double,
) -> Array[Double] {
  let dimension = state.length() / 3
  if state.length() != dimension * 3 {
    return []
  }
  constant_acceleration_transition(dimension, dt).multiply_vector(state)
}

///|
/// Update a transition matrix in-place for a new sampling interval while
/// retaining its shape.
pub fn retime_constant_velocity(
  model : Matrix,
  dimensions : Int,
  dt : Double,
) -> Matrix {
  let expected = (if dimensions < 0 { 0 } else { dimensions }) * 2
  if model.rows() != expected || model.cols() != expected {
    constant_velocity_transition(dimensions, dt)
  } else {
    let result = model.copy()
    for i in 0.. ignore
    }
    result
  }
}

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

///|
pub fn ObservationPacket::new(
  timestamp : Int,
  sensor : String,
  values : Array[Double],
  covariance : Matrix,
) -> ObservationPacket {
  { timestamp, sensor, values: values.copy(), covariance: covariance.copy() }
}

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

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

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

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

///|
pub fn ObservationPacket::is_valid(self : ObservationPacket) -> Bool {
  self.values.length() > 0 &&
  self.values.length() == self.covariance.rows() &&
  self.covariance.is_square() &&
  self.covariance.is_finite() &&
  vector_is_finite(self.values)
}

///|
pub fn ObservationPacket::with_inflated_noise(
  self : ObservationPacket,
  factor : Double,
) -> ObservationPacket {
  let safe_factor = if factor < 1.0 { 1.0 } else { factor }
  ObservationPacket::new(
    self.timestamp,
    self.sensor,
    self.values,
    self.covariance.scale(safe_factor),
  )
}