///|
/// A timestamped state used by offline analysis and path-quality metrics.
pub struct TrajectoryPoint {
  timestamp : Int
  position : Array[Double]
  velocity : Array[Double]
  covariance : Matrix
} derive(Debug)

///|
pub fn TrajectoryPoint::new(
  timestamp : Int,
  position : Array[Double],
  velocity : Array[Double],
  covariance : Matrix,
) -> TrajectoryPoint {
  {
    timestamp,
    position: position.copy(),
    velocity: velocity.copy(),
    covariance: covariance.copy(),
  }
}

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

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

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

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

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

///|
pub fn TrajectoryPoint::is_valid(self : TrajectoryPoint) -> Bool {
  self.position.length() == self.velocity.length() &&
  self.position.length() > 0 &&
  vector_is_finite(self.position) &&
  vector_is_finite(self.velocity) &&
  covariance_is_psd(self.covariance, 0.001)
}

///|
/// Bounded trajectory history with monotonic timestamp protection.
pub struct TrajectoryBuffer {
  points : Array[TrajectoryPoint]
  capacity : Int
  mut rejected : Int
}

///|
pub fn TrajectoryBuffer::new(capacity : Int) -> TrajectoryBuffer {
  { points: [], capacity: if capacity < 0 { 0 } else { capacity }, rejected: 0 }
}

///|
pub fn TrajectoryBuffer::push(
  self : TrajectoryBuffer,
  point : TrajectoryPoint,
) -> Bool {
  if self.capacity == 0 || !point.is_valid() {
    self.rejected = self.rejected + 1
    return false
  }
  match self.points.get(self.points.length() - 1) {
    Some(last) =>
      if point.timestamp() < last.timestamp() {
        self.rejected = self.rejected + 1
        return false
      }
    None => ()
  }
  if self.points.length() >= self.capacity {
    for i in 1.. Int {
  self.points.length()
}

///|
pub fn TrajectoryBuffer::capacity(self : TrajectoryBuffer) -> Int {
  self.capacity
}

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

///|
pub fn TrajectoryBuffer::points(
  self : TrajectoryBuffer,
) -> Array[TrajectoryPoint] {
  self.points.copy()
}

///|
pub fn TrajectoryBuffer::first(self : TrajectoryBuffer) -> TrajectoryPoint? {
  if self.points.length() == 0 {
    None
  } else {
    Some(self.points[0])
  }
}

///|
pub fn TrajectoryBuffer::last(self : TrajectoryBuffer) -> TrajectoryPoint? {
  if self.points.length() == 0 {
    None
  } else {
    Some(self.points[self.points.length() - 1])
  }
}

///|
pub fn TrajectoryBuffer::duration(self : TrajectoryBuffer) -> Int {
  guard self.first() is Some(first) else { return 0 }
  guard self.last() is Some(last) else { return 0 }
  last.timestamp() - first.timestamp()
}

///|
pub fn TrajectoryBuffer::clear(self : TrajectoryBuffer) -> Unit {
  self.points.clear()
  self.rejected = 0
}

///|
/// Arc length of a sequence of positions.
pub fn trajectory_length(points : Array[Array[Double]]) -> Double {
  if points.length() < 2 {
    return 0.0
  }
  let mut total = 0.0
  for i in 1.. Double {
  if points.length() < 2 {
    return 0.0
  }
  let mut distance = 0.0
  for i in 1.. Double {
  let mut maximum = 0.0
  for point in points {
    let speed = vector_l2_norm(point.velocity())
    if speed > maximum {
      maximum = speed
    }
  }
  maximum
}

///|
/// Approximate acceleration from adjacent velocity samples.
pub fn trajectory_accelerations(
  points : Array[TrajectoryPoint],
) -> Array[Array[Double]] {
  let result : Array[Array[Double]] = []
  if points.length() < 2 {
    return result
  }
  for i in 1.. Array[Double] {
  let result : Array[Double] = []
  if points.length() < 2 {
    return result
  }
  for i in 1.. 1.0 {
        1.0
      } else {
        cosine
      }
      result.push(acos_approx(bounded))
    }
  }
  result
}

///|
fn acos_approx(value : Double) -> Double {
  let x = if value < -1.0 { -1.0 } else if value > 1.0 { 1.0 } else { value }
  let sign = if x < 0.0 { -1.0 } else { 1.0 }
  let absolute = x.abs()
  let root = (1.0 - absolute).sqrt()
  let angle = (
      ((-0.0187293 * absolute + 0.0742610) * absolute - 0.2121144) * absolute +
      1.5707288
    ) *
    root
  if sign > 0.0 {
    angle
  } else {
    3.141592653589793 - angle
  }
}

///|
/// Interpolate a position between two timestamped points.
pub fn interpolate_trajectory_point(
  left : TrajectoryPoint,
  right : TrajectoryPoint,
  timestamp : Int,
) -> TrajectoryPoint {
  let span = right.timestamp() - left.timestamp()
  if span <= 0 || left.position().length() != right.position().length() {
    return left
  }
  let amount = (timestamp - left.timestamp()).to_double() / span.to_double()
  let position = vector_lerp(left.position(), right.position(), amount)
  let velocity = vector_lerp(left.velocity(), right.velocity(), amount)
  let covariance = left.covariance().add(right.covariance()).scale(0.5)
  TrajectoryPoint::new(timestamp, position, velocity, covariance)
}

///|
pub fn resample_trajectory(
  points : Array[TrajectoryPoint],
  timestamps : Array[Int],
) -> Array[TrajectoryPoint] {
  let result : Array[TrajectoryPoint] = []
  if points.length() == 0 {
    return result
  }
  for timestamp in timestamps {
    if timestamp <= points[0].timestamp() {
      result.push(points[0])
      continue
    }
    if timestamp >= points[points.length() - 1].timestamp() {
      result.push(points[points.length() - 1])
      continue
    }
    let mut inserted = false
    for i in 1.. TrajectoryPoint {
  let dt = if seconds < 0.0 { 0.0 } else { seconds }
  let position = vector_axpy(dt, point.velocity(), point.position())
  let covariance = point.covariance().add_diagonal(dt * dt)
  TrajectoryPoint::new(
    point.timestamp() + dt.to_int(),
    position,
    point.velocity(),
    covariance,
  )
}

///|
pub struct Pose2D {
  x : Double
  y : Double
  heading : Double
} derive(Debug)

///|
pub fn Pose2D::new(x : Double, y : Double, heading : Double) -> Pose2D {
  { x, y, heading }
}

///|
pub fn Pose2D::x(self : Pose2D) -> Double {
  self.x
}

///|
pub fn Pose2D::y(self : Pose2D) -> Double {
  self.y
}

///|
pub fn Pose2D::heading(self : Pose2D) -> Double {
  self.heading
}

///|
pub fn Pose2D::distance(self : Pose2D, other : Pose2D) -> Double {
  ((self.x - other.x) * (self.x - other.x) +
  (self.y - other.y) * (self.y - other.y)).sqrt()
}

///|
pub fn Pose2D::translate(self : Pose2D, dx : Double, dy : Double) -> Pose2D {
  Pose2D::new(self.x + dx, self.y + dy, self.heading)
}

///|
pub fn Pose2D::rotate(self : Pose2D, amount : Double) -> Pose2D {
  Pose2D::new(self.x, self.y, angle_difference(self.heading + amount, 0.0))
}