///|
/// Kinematic statistics for one interval of a trajectory.
pub struct TrajectoryInterval {
  start_timestamp : Int
  end_timestamp : Int
  duration : Double
  displacement : Double
  speed : Double
  acceleration : Double
  jerk : Double
  valid : Bool
} derive(Debug)

///|
pub fn TrajectoryInterval::invalid() -> TrajectoryInterval {
  {
    start_timestamp: 0,
    end_timestamp: 0,
    duration: 0.0,
    displacement: 0.0,
    speed: 0.0,
    acceleration: 0.0,
    jerk: 0.0,
    valid: false,
  }
}

///|
pub fn TrajectoryInterval::start_timestamp(self : TrajectoryInterval) -> Int {
  self.start_timestamp
}

///|
pub fn TrajectoryInterval::end_timestamp(self : TrajectoryInterval) -> Int {
  self.end_timestamp
}

///|
pub fn TrajectoryInterval::duration(self : TrajectoryInterval) -> Double {
  self.duration
}

///|
pub fn TrajectoryInterval::displacement(self : TrajectoryInterval) -> Double {
  self.displacement
}

///|
pub fn TrajectoryInterval::speed(self : TrajectoryInterval) -> Double {
  self.speed
}

///|
pub fn TrajectoryInterval::acceleration(self : TrajectoryInterval) -> Double {
  self.acceleration
}

///|
pub fn TrajectoryInterval::jerk(self : TrajectoryInterval) -> Double {
  self.jerk
}

///|
pub fn TrajectoryInterval::valid(self : TrajectoryInterval) -> Bool {
  self.valid
}

///|
pub fn trajectory_interval(
  previous : TrajectoryPoint,
  current : TrajectoryPoint,
  prior_speed : Double,
) -> TrajectoryInterval {
  let delta_time = current.timestamp() - previous.timestamp()
  if delta_time <= 0 || !previous.is_valid() || !current.is_valid() {
    return TrajectoryInterval::invalid()
  }
  let duration = delta_time.to_double()
  let displacement = vector_distance(previous.position(), current.position())
  let speed = displacement / duration
  let acceleration = (speed - prior_speed) / duration
  let jerk = (acceleration - 0.0) / duration
  {
    start_timestamp: previous.timestamp(),
    end_timestamp: current.timestamp(),
    duration,
    displacement,
    speed,
    acceleration,
    jerk,
    valid: true,
  }
}

///|
/// A contiguous segment annotated with kinematic and covariance quality.
pub struct TrajectorySegment {
  start_index : Int
  end_index : Int
  start_timestamp : Int
  end_timestamp : Int
  length : Double
  mean_speed : Double
  max_speed : Double
  max_acceleration : Double
  max_jerk : Double
  mean_uncertainty : Double
  quality : Double
  valid : Bool
} derive(Debug)

///|
pub fn TrajectorySegment::empty() -> TrajectorySegment {
  {
    start_index: 0,
    end_index: 0,
    start_timestamp: 0,
    end_timestamp: 0,
    length: 0.0,
    mean_speed: 0.0,
    max_speed: 0.0,
    max_acceleration: 0.0,
    max_jerk: 0.0,
    mean_uncertainty: 0.0,
    quality: 0.0,
    valid: false,
  }
}

///|
pub fn TrajectorySegment::start_index(self : TrajectorySegment) -> Int {
  self.start_index
}

///|
pub fn TrajectorySegment::end_index(self : TrajectorySegment) -> Int {
  self.end_index
}

///|
pub fn TrajectorySegment::start_timestamp(self : TrajectorySegment) -> Int {
  self.start_timestamp
}

///|
pub fn TrajectorySegment::end_timestamp(self : TrajectorySegment) -> Int {
  self.end_timestamp
}

///|
pub fn TrajectorySegment::length(self : TrajectorySegment) -> Double {
  self.length
}

///|
pub fn TrajectorySegment::mean_speed(self : TrajectorySegment) -> Double {
  self.mean_speed
}

///|
pub fn TrajectorySegment::max_speed(self : TrajectorySegment) -> Double {
  self.max_speed
}

///|
pub fn TrajectorySegment::max_acceleration(self : TrajectorySegment) -> Double {
  self.max_acceleration
}

///|
pub fn TrajectorySegment::max_jerk(self : TrajectorySegment) -> Double {
  self.max_jerk
}

///|
pub fn TrajectorySegment::mean_uncertainty(self : TrajectorySegment) -> Double {
  self.mean_uncertainty
}

///|
pub fn TrajectorySegment::quality(self : TrajectorySegment) -> Double {
  self.quality
}

///|
pub fn TrajectorySegment::valid(self : TrajectorySegment) -> Bool {
  self.valid
}

///|
/// Build an annotated segment and retain only valid monotonic intervals.
pub fn trajectory_segment(
  points : Array[TrajectoryPoint],
  start_index : Int,
  end_index : Int,
  max_speed_limit : Double,
  max_acceleration_limit : Double,
  max_jerk_limit : Double,
) -> TrajectorySegment {
  if points.length() == 0 ||
    start_index < 0 ||
    end_index <= start_index ||
    end_index >= points.length() {
    return TrajectorySegment::empty()
  }
  let mut length = 0.0
  let mut speed_sum = 0.0
  let mut max_speed = 0.0
  let mut max_acceleration = 0.0
  let mut max_jerk = 0.0
  let mut uncertainty_sum = 0.0
  let mut valid_intervals = 0
  let mut previous_speed = 0.0
  let mut i = start_index + 1
  while i < end_index + 1 {
    let interval = trajectory_interval(points[i - 1], points[i], previous_speed)
    if interval.valid() {
      length = length + interval.displacement()
      speed_sum = speed_sum + interval.speed()
      max_speed = max_speed.max(interval.speed())
      max_acceleration = max_acceleration.max(interval.acceleration().abs())
      max_jerk = max_jerk.max(interval.jerk().abs())
      uncertainty_sum = uncertainty_sum +
        covariance_trace3d(points[i].covariance()).max(0.0).sqrt()
      previous_speed = interval.speed()
      valid_intervals = valid_intervals + 1
    }
    i = i + 1
  }
  if valid_intervals == 0 {
    return TrajectorySegment::empty()
  }
  let speed_limit = max_speed_limit.max(1.0e-12)
  let acceleration_limit = max_acceleration_limit.max(1.0e-12)
  let jerk_limit = max_jerk_limit.max(1.0e-12)
  let speed_score = (1.0 - max_speed / speed_limit).clamp(min=0.0, max=1.0)
  let acceleration_score = (1.0 - max_acceleration / acceleration_limit).clamp(
    min=0.0,
    max=1.0,
  )
  let jerk_score = (1.0 - max_jerk / jerk_limit).clamp(min=0.0, max=1.0)
  {
    start_index,
    end_index,
    start_timestamp: points[start_index].timestamp(),
    end_timestamp: points[end_index].timestamp(),
    length,
    mean_speed: speed_sum / valid_intervals.to_double(),
    max_speed,
    max_acceleration,
    max_jerk,
    mean_uncertainty: uncertainty_sum / valid_intervals.to_double(),
    quality: (speed_score + acceleration_score + jerk_score) / 3.0,
    valid: true,
  }
}

///|
/// Quality flags emitted by trajectory validation.
pub enum TrajectoryQualityFlag {
  ValidTrajectory
  EmptyTrajectory
  InvalidPoint
  NonMonotonicTime
  LargeGap
  SpeedLimitExceeded
  AccelerationLimitExceeded
  JerkLimitExceeded
  UncertaintyTooLarge
} derive(Debug, Eq)

///|
pub struct TrajectoryEvent {
  timestamp : Int
  flag : TrajectoryQualityFlag
  severity : Double
  index : Int
  message : String
} derive(Debug)

///|
pub fn TrajectoryEvent::new(
  timestamp : Int,
  flag : TrajectoryQualityFlag,
  severity : Double,
  index : Int,
  message : String,
) -> TrajectoryEvent {
  {
    timestamp,
    flag,
    severity: severity.clamp(min=0.0, max=1.0),
    index,
    message,
  }
}

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

///|
pub fn TrajectoryEvent::flag(self : TrajectoryEvent) -> TrajectoryQualityFlag {
  self.flag
}

///|
pub fn TrajectoryEvent::severity(self : TrajectoryEvent) -> Double {
  self.severity
}

///|
pub fn TrajectoryEvent::index(self : TrajectoryEvent) -> Int {
  self.index
}

///|
pub fn TrajectoryEvent::message(self : TrajectoryEvent) -> String {
  self.message
}

///|
pub struct TrajectoryQualityReport {
  point_count : Int
  valid_points : Int
  interval_count : Int
  valid_intervals : Int
  total_length : Double
  duration : Int
  mean_speed : Double
  max_speed : Double
  max_acceleration : Double
  max_jerk : Double
  mean_uncertainty : Double
  score : Double
  events : Array[TrajectoryEvent]
} derive(Debug)

///|
pub fn TrajectoryQualityReport::empty() -> TrajectoryQualityReport {
  {
    point_count: 0,
    valid_points: 0,
    interval_count: 0,
    valid_intervals: 0,
    total_length: 0.0,
    duration: 0,
    mean_speed: 0.0,
    max_speed: 0.0,
    max_acceleration: 0.0,
    max_jerk: 0.0,
    mean_uncertainty: 0.0,
    score: 0.0,
    events: [
      TrajectoryEvent::new(0, EmptyTrajectory, 1.0, 0, "trajectory is empty"),
    ],
  }
}

///|
pub fn TrajectoryQualityReport::point_count(
  self : TrajectoryQualityReport,
) -> Int {
  self.point_count
}

///|
pub fn TrajectoryQualityReport::valid_points(
  self : TrajectoryQualityReport,
) -> Int {
  self.valid_points
}

///|
pub fn TrajectoryQualityReport::interval_count(
  self : TrajectoryQualityReport,
) -> Int {
  self.interval_count
}

///|
pub fn TrajectoryQualityReport::valid_intervals(
  self : TrajectoryQualityReport,
) -> Int {
  self.valid_intervals
}

///|
pub fn TrajectoryQualityReport::total_length(
  self : TrajectoryQualityReport,
) -> Double {
  self.total_length
}

///|
pub fn TrajectoryQualityReport::duration(self : TrajectoryQualityReport) -> Int {
  self.duration
}

///|
pub fn TrajectoryQualityReport::mean_speed(
  self : TrajectoryQualityReport,
) -> Double {
  self.mean_speed
}

///|
pub fn TrajectoryQualityReport::max_speed(
  self : TrajectoryQualityReport,
) -> Double {
  self.max_speed
}

///|
pub fn TrajectoryQualityReport::max_acceleration(
  self : TrajectoryQualityReport,
) -> Double {
  self.max_acceleration
}

///|
pub fn TrajectoryQualityReport::max_jerk(
  self : TrajectoryQualityReport,
) -> Double {
  self.max_jerk
}

///|
pub fn TrajectoryQualityReport::mean_uncertainty(
  self : TrajectoryQualityReport,
) -> Double {
  self.mean_uncertainty
}

///|
pub fn TrajectoryQualityReport::score(self : TrajectoryQualityReport) -> Double {
  self.score
}

///|
pub fn TrajectoryQualityReport::events(
  self : TrajectoryQualityReport,
) -> Array[TrajectoryEvent] {
  self.events.copy()
}

///|
pub fn TrajectoryQualityReport::is_usable(
  self : TrajectoryQualityReport,
) -> Bool {
  self.valid_points() > 1 && self.valid_intervals() > 0 && self.score() >= 0.5
}

///|
fn trajectory_quality_event_severity(value : Double, limit : Double) -> Double {
  if limit <= 0.0 {
    1.0
  } else {
    (value / limit - 1.0).clamp(min=0.0, max=1.0)
  }
}

///|
pub fn assess_trajectory_quality(
  points : Array[TrajectoryPoint],
  max_gap : Int,
  max_speed_limit : Double,
  max_acceleration_limit : Double,
  max_jerk_limit : Double,
  uncertainty_limit : Double,
) -> TrajectoryQualityReport {
  if points.length() == 0 {
    return TrajectoryQualityReport::empty()
  }
  let mut valid_points = 0
  let mut interval_count = 0
  let mut valid_intervals = 0
  let mut total_length = 0.0
  let mut speed_sum = 0.0
  let mut max_speed = 0.0
  let mut max_acceleration = 0.0
  let mut max_jerk = 0.0
  let mut uncertainty_sum = 0.0
  let events : Array[TrajectoryEvent] = []
  let mut previous_speed = 0.0
  for i, point in points {
    if point.is_valid() {
      valid_points = valid_points + 1
      uncertainty_sum = uncertainty_sum +
        covariance_trace3d(point.covariance()).max(0.0).sqrt()
      if uncertainty_limit > 0.0 && uncertainty_sum > uncertainty_limit {
        events.push(
          TrajectoryEvent::new(
            point.timestamp(),
            UncertaintyTooLarge,
            trajectory_quality_event_severity(
              uncertainty_sum, uncertainty_limit,
            ),
            i,
            "uncertainty budget exceeded",
          ),
        )
      }
    } else {
      events.push(
        TrajectoryEvent::new(
          point.timestamp(),
          InvalidPoint,
          1.0,
          i,
          "invalid state or covariance",
        ),
      )
    }
    if i > 0 {
      interval_count = interval_count + 1
      let previous = points[i - 1]
      let gap = point.timestamp() - previous.timestamp()
      if gap <= 0 {
        events.push(
          TrajectoryEvent::new(
            point.timestamp(),
            NonMonotonicTime,
            1.0,
            i,
            "timestamps are not strictly increasing",
          ),
        )
      } else if gap > max_gap.max(1) {
        events.push(
          TrajectoryEvent::new(
            point.timestamp(),
            LargeGap,
            (gap - max_gap).to_double() / gap.to_double(),
            i,
            "trajectory gap exceeds limit",
          ),
        )
      }
      let interval = trajectory_interval(previous, point, previous_speed)
      if interval.valid() {
        valid_intervals = valid_intervals + 1
        total_length = total_length + interval.displacement()
        speed_sum = speed_sum + interval.speed()
        max_speed = max_speed.max(interval.speed())
        max_acceleration = max_acceleration.max(interval.acceleration().abs())
        max_jerk = max_jerk.max(interval.jerk().abs())
        previous_speed = interval.speed()
        if interval.speed() > max_speed_limit {
          events.push(
            TrajectoryEvent::new(
              point.timestamp(),
              SpeedLimitExceeded,
              trajectory_quality_event_severity(
                interval.speed(),
                max_speed_limit,
              ),
              i,
              "speed limit exceeded",
            ),
          )
        }
        if interval.acceleration().abs() > max_acceleration_limit {
          events.push(
            TrajectoryEvent::new(
              point.timestamp(),
              AccelerationLimitExceeded,
              trajectory_quality_event_severity(
                interval.acceleration().abs(),
                max_acceleration_limit,
              ),
              i,
              "acceleration limit exceeded",
            ),
          )
        }
        if interval.jerk().abs() > max_jerk_limit {
          events.push(
            TrajectoryEvent::new(
              point.timestamp(),
              JerkLimitExceeded,
              trajectory_quality_event_severity(
                interval.jerk().abs(),
                max_jerk_limit,
              ),
              i,
              "jerk limit exceeded",
            ),
          )
        }
      }
    }
  }
  if valid_points == points.length() && events.length() == 0 {
    events.push(
      TrajectoryEvent::new(
        points[points.length() - 1].timestamp(),
        ValidTrajectory,
        0.0,
        points.length() - 1,
        "trajectory passed quality checks",
      ),
    )
  }
  let interval_ratio = if interval_count == 0 {
    0.0
  } else {
    valid_intervals.to_double() / interval_count.to_double()
  }
  let point_ratio = valid_points.to_double() / points.length().to_double()
  let event_penalty = (events.length().to_double() / points.length().to_double()).min(
    1.0,
  )
  {
    point_count: points.length(),
    valid_points,
    interval_count,
    valid_intervals,
    total_length,
    duration: points[points.length() - 1].timestamp() - points[0].timestamp(),
    mean_speed: if valid_intervals == 0 {
      0.0
    } else {
      speed_sum / valid_intervals.to_double()
    },
    max_speed,
    max_acceleration,
    max_jerk,
    mean_uncertainty: if valid_points == 0 {
      0.0
    } else {
      uncertainty_sum / valid_points.to_double()
    },
    score: (point_ratio * interval_ratio * (1.0 - event_penalty)).clamp(
      min=0.0,
      max=1.0,
    ),
    events,
  }
}

///|
pub fn trajectory_detect_segments(
  points : Array[TrajectoryPoint],
  max_gap : Int,
) -> Array[(Int, Int)] {
  let segments : Array[(Int, Int)] = []
  if points.length() == 0 {
    return segments
  }
  let mut start = 0
  for i in 1.. max_gap.max(1) ||
      !points[i].is_valid() {
      if i - start >= 2 {
        segments.push((start, i - 1))
      }
      start = i
    }
  }
  if points.length() - start >= 2 {
    segments.push((start, points.length() - 1))
  }
  segments
}

///|
pub fn trajectory_total_distance(points : Array[TrajectoryPoint]) -> Double {
  let mut total = 0.0
  for i in 1.. Array[Double] {
  if points.length() == 0 {
    return []
  }
  let dimension = points[0].velocity().length()
  let result = Array::make(dimension, 0.0)
  let mut count = 0
  for point in points {
    if point.is_valid() {
      let velocity = point.velocity()
      for i in 0.. 0 {
    for i in 0.. Double {
  let mut maximum = 0.0
  for point in points {
    if point.is_valid() {
      maximum = maximum.max(vector_l2_norm(point.velocity()))
    }
  }
  maximum
}

///|
pub fn trajectory_position_bounds(
  points : Array[TrajectoryPoint],
) -> (Array[Double], Array[Double])? {
  if points.length() == 0 {
    return None
  }
  let dimension = points[0].position().length()
  if dimension == 0 {
    return None
  }
  let lower = Array::make(dimension, 1.0e300)
  let upper = Array::make(dimension, -1.0e300)
  let mut count = 0
  for point in points {
    if point.is_valid() {
      let position = point.position()
      for i in 0.. Double {
  if points.length() < 2 {
    return 0.0
  }
  let noise = process_noise.max(1.0e-12)
  let mut score = 0.0
  let mut count = 0
  for i in 1.. Array[(TrajectoryQualityFlag, Int)] {
  let result : Array[(TrajectoryQualityFlag, Int)] = []
  for event in events {
    let mut found = false
    for i in 0.. String {
  "points=" +
  report.point_count().to_string() +
  ",valid=" +
  report.valid_points().to_string() +
  ",length=" +
  report.total_length().to_string() +
  ",score=" +
  report.score().to_string() +
  ",events=" +
  report.events().length().to_string()
}