///|
/// Constant-jerk model with state `[position, velocity, acceleration, jerk]`
/// per axis for highly dynamic motion.
pub fn constant_jerk_transition(dimensions : Int, dt : Double) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::identity(n * 4)
  let half_dt2 = 0.5 * dt * dt
  let sixth_dt3 = dt * dt * dt / 6.0
  for i in 0.. ignore
    result.set(p, p + 2, half_dt2) |> ignore
    result.set(p, p + 3, sixth_dt3) |> ignore
    result.set(p + 1, p + 2, dt) |> ignore
    result.set(p + 1, p + 3, half_dt2) |> ignore
    result.set(p + 2, p + 3, dt) |> ignore
  }
  result
}

///|
pub fn constant_jerk_process_noise(
  dimensions : Int,
  dt : Double,
  snap_variance : Double,
) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n * 4, n * 4)
  let dt2 = dt * dt
  let dt3 = dt2 * dt
  let dt4 = dt3 * dt
  let dt5 = dt4 * dt
  let dt6 = dt5 * dt
  let dt7 = dt6 * dt
  let dt8 = dt7 * dt
  let variance = if snap_variance < 0.0 { 0.0 } else { snap_variance }
  for i in 0.. ignore
    result.set(p, v, dt7 / 144.0 * variance) |> ignore
    result.set(v, p, dt7 / 144.0 * variance) |> ignore
    result.set(p, a, dt6 / 48.0 * variance) |> ignore
    result.set(a, p, dt6 / 48.0 * variance) |> ignore
    result.set(p, j, dt5 / 24.0 * variance) |> ignore
    result.set(j, p, dt5 / 24.0 * variance) |> ignore
    result.set(v, v, dt6 / 36.0 * variance) |> ignore
    result.set(v, a, dt5 / 12.0 * variance) |> ignore
    result.set(a, v, dt5 / 12.0 * variance) |> ignore
    result.set(v, j, dt4 / 8.0 * variance) |> ignore
    result.set(j, v, dt4 / 8.0 * variance) |> ignore
    result.set(a, a, dt4 / 4.0 * variance) |> ignore
    result.set(a, j, dt3 / 2.0 * variance) |> ignore
    result.set(j, a, dt3 / 2.0 * variance) |> ignore
    result.set(j, j, dt2 * variance) |> ignore
  }
  result
}

///|
pub fn jerk_position_observation(dimensions : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 4)
  for i in 0.. ignore
  }
  result
}

///|
pub fn jerk_velocity_observation(dimensions : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 4)
  for i in 0.. ignore
  }
  result
}

///|
pub fn jerk_acceleration_observation(dimensions : Int) -> Matrix {
  let n = if dimensions < 0 { 0 } else { dimensions }
  let result = Matrix::zeros(n, n * 4)
  for i in 0.. ignore
  }
  result
}

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

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

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

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

///|
/// Predict the Euclidean range of a Cartesian position and the corresponding
/// bearing-free residual used by a range-only sensor.
pub fn planar_range(
  position : Array[Double],
  reference : Array[Double],
) -> Double {
  if position.length() != reference.length() || position.length() == 0 {
    return 0.0
  }
  vector_distance(position, reference)
}

///|
pub fn planar_range_jacobian(
  position : Array[Double],
  reference : Array[Double],
) -> Array[Double] {
  if position.length() != reference.length() || position.length() == 0 {
    return []
  }
  let difference = vector_sub(position, reference)
  let range = vector_l2_norm(difference)
  if range <= 0.000000000001 {
    Array::make(position.length(), 0.0)
  } else {
    vector_scale(difference, 1.0 / range)
  }
}

///|
pub struct RangeMeasurement {
  reference : Array[Double]
  value : Double
  variance : Double
} derive(Debug)

///|
pub fn RangeMeasurement::new(
  reference : Array[Double],
  value : Double,
  variance : Double,
) -> RangeMeasurement {
  {
    reference: reference.copy(),
    value,
    variance: if variance <= 0.0 {
      0.000001
    } else {
      variance
    },
  }
}

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

///|
pub fn RangeMeasurement::value(self : RangeMeasurement) -> Double {
  self.value
}

///|
pub fn RangeMeasurement::variance(self : RangeMeasurement) -> Double {
  self.variance
}

///|
pub fn RangeMeasurement::residual(
  self : RangeMeasurement,
  position : Array[Double],
) -> Double {
  self.value - planar_range(position, self.reference)
}

///|
pub fn RangeMeasurement::jacobian(
  self : RangeMeasurement,
  position : Array[Double],
) -> Array[Double] {
  planar_range_jacobian(position, self.reference)
}

///|
pub struct ModelStep {
  timestamp : Int
  transition : Matrix
  process_noise : Matrix
} derive(Debug)

///|
pub fn ModelStep::new(
  timestamp : Int,
  transition : Matrix,
  process_noise : Matrix,
) -> ModelStep {
  { timestamp, transition, process_noise }
}

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

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

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

///|
pub fn make_model_schedule(
  timestamps : Array[Int],
  dimensions : Int,
  acceleration_variance : Double,
) -> Array[ModelStep] {
  let result : Array[ModelStep] = []
  if timestamps.length() == 0 {
    return result
  }
  let mut previous = timestamps[0]
  for timestamp in timestamps {
    let difference = timestamp - previous
    let dt = if difference <= 0 { 1.0 } else { difference.to_double() }
    result.push(
      ModelStep::new(
        timestamp,
        constant_velocity_transition(dimensions, dt),
        constant_velocity_process_noise(dimensions, dt, acceleration_variance),
      ),
    )
    previous = timestamp
  }
  result
}