///|
/// A scalar or vector safety constraint applied to an estimated state.
pub enum SafetyConstraintKind {
  LowerBound
  UpperBound
  AbsoluteBound
  RateBound
  DistanceFromPoint
  MahalanobisBound
} derive(Debug, Eq)

///|
pub struct SafetyConstraint {
  name : String
  kind : SafetyConstraintKind
  threshold : Double
  tolerance : Double
  severity : Double
  mut enabled : Bool
} derive(Debug)

///|
pub fn SafetyConstraint::new(
  name : String,
  kind : SafetyConstraintKind,
  threshold : Double,
  tolerance : Double,
  severity : Double,
) -> SafetyConstraint {
  {
    name,
    kind,
    threshold: threshold.max(0.0),
    tolerance: tolerance.max(0.0),
    severity: severity.clamp(min=0.0, max=1.0),
    enabled: true,
  }
}

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

///|
pub fn SafetyConstraint::kind(self : SafetyConstraint) -> SafetyConstraintKind {
  self.kind
}

///|
pub fn SafetyConstraint::threshold(self : SafetyConstraint) -> Double {
  self.threshold
}

///|
pub fn SafetyConstraint::tolerance(self : SafetyConstraint) -> Double {
  self.tolerance
}

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

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

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

///|
pub struct SafetyViolation {
  name : String
  kind : SafetyConstraintKind
  value : Double
  threshold : Double
  excess : Double
  severity : Double
  timestamp : Int?
} derive(Debug)

///|
pub fn SafetyViolation::new(
  constraint : SafetyConstraint,
  value : Double,
  excess : Double,
  timestamp : Int?,
) -> SafetyViolation {
  {
    name: constraint.name(),
    kind: constraint.kind(),
    value,
    threshold: constraint.threshold(),
    excess: excess.max(0.0),
    severity: constraint.severity(),
    timestamp,
  }
}

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

///|
pub fn SafetyViolation::kind(self : SafetyViolation) -> SafetyConstraintKind {
  self.kind
}

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

///|
pub fn SafetyViolation::threshold(self : SafetyViolation) -> Double {
  self.threshold
}

///|
pub fn SafetyViolation::excess(self : SafetyViolation) -> Double {
  self.excess
}

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

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

///|
pub enum SafetyCheckStatus {
  Safe
  Violated
  InvalidInput
  Disabled
} derive(Debug, Eq)

///|
pub struct SafetyCheckResult {
  status : SafetyCheckStatus
  value : Double
  margin : Double
  violation : SafetyViolation?
} derive(Debug)

///|
pub fn SafetyCheckResult::safe(
  value : Double,
  margin : Double,
) -> SafetyCheckResult {
  { status: Safe, value, margin, violation: None }
}

///|
pub fn SafetyCheckResult::invalid(value : Double) -> SafetyCheckResult {
  { status: InvalidInput, value, margin: 0.0, violation: None }
}

///|
pub fn SafetyCheckResult::disabled(value : Double) -> SafetyCheckResult {
  { status: Disabled, value, margin: 0.0, violation: None }
}

///|
pub fn SafetyCheckResult::violated(
  value : Double,
  margin : Double,
  violation : SafetyViolation,
) -> SafetyCheckResult {
  { status: Violated, value, margin, violation: Some(violation) }
}

///|
pub fn SafetyCheckResult::status(self : SafetyCheckResult) -> SafetyCheckStatus {
  self.status
}

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

///|
pub fn SafetyCheckResult::margin(self : SafetyCheckResult) -> Double {
  self.margin
}

///|
pub fn SafetyCheckResult::violation(
  self : SafetyCheckResult,
) -> SafetyViolation? {
  self.violation
}

///|
pub fn check_safety_constraint(
  constraint : SafetyConstraint,
  value : Double,
  timestamp : Int?,
) -> SafetyCheckResult {
  if !constraint.enabled() {
    return SafetyCheckResult::disabled(value)
  }
  if value.is_nan() || value.is_inf() {
    return SafetyCheckResult::invalid(value)
  }
  let threshold = constraint.threshold()
  let tolerance = constraint.tolerance()
  let (violated, excess) = match constraint.kind() {
    LowerBound =>
      (value < threshold - tolerance, (threshold - tolerance - value).max(0.0))
    UpperBound =>
      (value > threshold + tolerance, (value - threshold - tolerance).max(0.0))
    AbsoluteBound =>
      (
        value.abs() > threshold + tolerance,
        (value.abs() - threshold - tolerance).max(0.0),
      )
    RateBound =>
      (
        value.abs() > threshold + tolerance,
        (value.abs() - threshold - tolerance).max(0.0),
      )
    DistanceFromPoint =>
      (value > threshold + tolerance, (value - threshold - tolerance).max(0.0))
    MahalanobisBound =>
      (value > threshold + tolerance, (value - threshold - tolerance).max(0.0))
  }
  if violated {
    SafetyCheckResult::violated(
      value,
      -excess,
      SafetyViolation::new(constraint, value, excess, timestamp),
    )
  } else {
    SafetyCheckResult::safe(value, threshold + tolerance - value.abs().max(0.0))
  }
}

///|
pub fn safety_distance_constraint(
  constraint : SafetyConstraint,
  point : Vec3D,
  reference : Vec3D,
  timestamp : Int?,
) -> SafetyCheckResult {
  let value = point.distance(reference)
  check_safety_constraint(constraint, value, timestamp)
}

///|
/// Construct an explicit distance-to-reference constraint.
pub fn safety_distance_bound(
  name : String,
  threshold : Double,
  tolerance : Double,
  severity : Double,
) -> SafetyConstraint {
  SafetyConstraint::new(name, DistanceFromPoint, threshold, tolerance, severity)
}

///|
pub fn safety_mahalanobis_constraint(
  constraint : SafetyConstraint,
  error : Array[Double],
  covariance : Matrix,
  timestamp : Int?,
) -> SafetyCheckResult {
  if error.length() != covariance.rows() ||
    covariance.rows() != covariance.cols() {
    return SafetyCheckResult::invalid(0.0)
  }
  let solved = covariance.solve(error)
  match solved {
    InvalidShape | Singular => SafetyCheckResult::invalid(0.0)
    Solved(solution) =>
      check_safety_constraint(
        constraint,
        vector_dot(error, solution).max(0.0).sqrt(),
        timestamp,
      )
  }
}

///|
pub struct SafetyEnvelope {
  name : String
  position_min : Vec3D
  position_max : Vec3D
  velocity_limit : Double
  acceleration_limit : Double
  uncertainty_limit : Double
  mut checks : Int
  mut violations : Int
  last_timestamp : Int?
} derive(Debug)

///|
pub fn SafetyEnvelope::new(
  name : String,
  position_min : Vec3D,
  position_max : Vec3D,
  velocity_limit : Double,
  acceleration_limit : Double,
  uncertainty_limit : Double,
) -> SafetyEnvelope {
  {
    name,
    position_min: vec3d_min(position_min, position_max),
    position_max: vec3d_max(position_min, position_max),
    velocity_limit: velocity_limit.max(0.0),
    acceleration_limit: acceleration_limit.max(0.0),
    uncertainty_limit: uncertainty_limit.max(0.0),
    checks: 0,
    violations: 0,
    last_timestamp: None,
  }
}

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

///|
pub fn SafetyEnvelope::position_min(self : SafetyEnvelope) -> Vec3D {
  self.position_min
}

///|
pub fn SafetyEnvelope::position_max(self : SafetyEnvelope) -> Vec3D {
  self.position_max
}

///|
pub fn SafetyEnvelope::velocity_limit(self : SafetyEnvelope) -> Double {
  self.velocity_limit
}

///|
pub fn SafetyEnvelope::acceleration_limit(self : SafetyEnvelope) -> Double {
  self.acceleration_limit
}

///|
pub fn SafetyEnvelope::uncertainty_limit(self : SafetyEnvelope) -> Double {
  self.uncertainty_limit
}

///|
pub fn SafetyEnvelope::checks(self : SafetyEnvelope) -> Int {
  self.checks
}

///|
pub fn SafetyEnvelope::violations(self : SafetyEnvelope) -> Int {
  self.violations
}

///|
pub fn SafetyEnvelope::violation_rate(self : SafetyEnvelope) -> Double {
  if self.checks == 0 {
    0.0
  } else {
    self.violations.to_double() / self.checks.to_double()
  }
}

///|
fn SafetyEnvelope::safety_envelope_check_result(
  self : SafetyEnvelope,
  result : SafetyCheckResult,
) -> SafetyCheckResult {
  self.checks = self.checks + 1
  if result.status() is Violated {
    self.violations = self.violations + 1
  }
  result
}

///|
pub fn SafetyEnvelope::check_position(
  self : SafetyEnvelope,
  position : Vec3D,
  timestamp : Int?,
) -> SafetyCheckResult {
  let x_low = SafetyConstraint::new(
    "x-lower",
    LowerBound,
    self.position_min.x(),
    0.0,
    1.0,
  )
  let x_high = SafetyConstraint::new(
    "x-upper",
    UpperBound,
    self.position_max.x(),
    0.0,
    1.0,
  )
  let y_low = SafetyConstraint::new(
    "y-lower",
    LowerBound,
    self.position_min.y(),
    0.0,
    1.0,
  )
  let y_high = SafetyConstraint::new(
    "y-upper",
    UpperBound,
    self.position_max.y(),
    0.0,
    1.0,
  )
  let z_low = SafetyConstraint::new(
    "z-lower",
    LowerBound,
    self.position_min.z(),
    0.0,
    1.0,
  )
  let z_high = SafetyConstraint::new(
    "z-upper",
    UpperBound,
    self.position_max.z(),
    0.0,
    1.0,
  )
  let results = [
    check_safety_constraint(x_low, position.x(), timestamp),
    check_safety_constraint(x_high, position.x(), timestamp),
    check_safety_constraint(y_low, position.y(), timestamp),
    check_safety_constraint(y_high, position.y(), timestamp),
    check_safety_constraint(z_low, position.z(), timestamp),
    check_safety_constraint(z_high, position.z(), timestamp),
  ]
  for result in results {
    if result.status() is Violated {
      return self.safety_envelope_check_result(result)
    }
  }
  self.safety_envelope_check_result(SafetyCheckResult::safe(0.0, 1.0))
}

///|
pub fn SafetyEnvelope::check_velocity(
  self : SafetyEnvelope,
  velocity : Vec3D,
  timestamp : Int?,
) -> SafetyCheckResult {
  let constraint = SafetyConstraint::new(
    "velocity",
    AbsoluteBound,
    self.velocity_limit,
    0.0,
    1.0,
  )
  self.safety_envelope_check_result(
    check_safety_constraint(constraint, velocity.norm(), timestamp),
  )
}

///|
pub fn SafetyEnvelope::check_acceleration(
  self : SafetyEnvelope,
  acceleration : Vec3D,
  timestamp : Int?,
) -> SafetyCheckResult {
  let constraint = SafetyConstraint::new(
    "acceleration",
    RateBound,
    self.acceleration_limit,
    0.0,
    1.0,
  )
  self.safety_envelope_check_result(
    check_safety_constraint(constraint, acceleration.norm(), timestamp),
  )
}

///|
pub fn SafetyEnvelope::check_uncertainty(
  self : SafetyEnvelope,
  covariance : Matrix,
  timestamp : Int?,
) -> SafetyCheckResult {
  let constraint = SafetyConstraint::new(
    "uncertainty",
    MahalanobisBound,
    self.uncertainty_limit,
    0.0,
    0.8,
  )
  self.safety_envelope_check_result(
    check_safety_constraint(
      constraint,
      covariance_trace3d(covariance).max(0.0).sqrt(),
      timestamp,
    ),
  )
}

///|
pub fn SafetyEnvelope::check_pose(
  self : SafetyEnvelope,
  pose : Pose3D,
  velocity : Vec3D,
  covariance : Matrix,
  timestamp : Int?,
) -> Array[SafetyCheckResult] {
  [
    self.check_position(pose.translation(), timestamp),
    self.check_velocity(velocity, timestamp),
    self.check_uncertainty(covariance, timestamp),
  ]
}

///|
/// A conservative projection of a point onto an axis-aligned envelope.
pub fn safety_project_position(
  envelope : SafetyEnvelope,
  point : Vec3D,
) -> Vec3D {
  Vec3D::new(
    point
    .x()
    .clamp(min=envelope.position_min().x(), max=envelope.position_max().x()),
    point
    .y()
    .clamp(min=envelope.position_min().y(), max=envelope.position_max().y()),
    point
    .z()
    .clamp(min=envelope.position_min().z(), max=envelope.position_max().z()),
  )
}

///|
pub fn safety_distance_to_envelope(
  envelope : SafetyEnvelope,
  point : Vec3D,
) -> Double {
  let projected = safety_project_position(envelope, point)
  point.distance(projected)
}

///|
pub fn safety_envelope_health(envelope : SafetyEnvelope) -> Double {
  (1.0 - envelope.violation_rate()).clamp(min=0.0, max=1.0)
}

///|
pub fn safety_check_all(
  constraints : Array[SafetyConstraint],
  value : Double,
  timestamp : Int?,
) -> Array[SafetyCheckResult] {
  constraints.map(constraint => {
    check_safety_constraint(constraint, value, timestamp)
  })
}

///|
pub fn safety_violation_count(results : Array[SafetyCheckResult]) -> Int {
  let mut count = 0
  for result in results {
    if result.status() is Violated {
      count = count + 1
    }
  }
  count
}

///|
pub fn safety_results_score(results : Array[SafetyCheckResult]) -> Double {
  if results.length() == 0 {
    1.0
  } else {
    let mut penalty = 0.0
    for result in results {
      match result.violation() {
        None => ()
        Some(violation) =>
          penalty = penalty + violation.severity() * (1.0 + violation.excess())
      }
    }
    (1.0 - penalty / results.length().to_double()).clamp(min=0.0, max=1.0)
  }
}

///|
pub fn safety_constraint_summary(result : SafetyCheckResult) -> String {
  let status = match result.status() {
    Safe => "safe"
    InvalidInput => "invalid"
    Disabled => "disabled"
    Violated => "violated"
  }
  match result.violation() {
    None => "status=" + status
    Some(violation) =>
      "status=violated,name=" +
      violation.name() +
      ",excess=" +
      violation.excess().to_string()
  }
}

///|
pub fn safety_covariance_radius(
  covariance : Matrix,
  multiplier : Double,
) -> Double {
  covariance_trace3d(covariance).max(0.0).sqrt() * multiplier.max(0.0)
}

///|
pub fn safety_inflate_position_bounds(
  minimum : Vec3D,
  maximum : Vec3D,
  covariance : Matrix,
  multiplier : Double,
) -> (Vec3D, Vec3D) {
  let radius = safety_covariance_radius(covariance, multiplier)
  (minimum.sub(vec3d_splat(radius)), maximum.add(vec3d_splat(radius)))
}

///|
pub fn safety_constraint_is_tight(
  result : SafetyCheckResult,
  relative_margin : Double,
) -> Bool {
  result.status() is Safe && result.margin().abs() <= relative_margin.max(0.0)
}

///|
pub fn safety_worst_result(
  results : Array[SafetyCheckResult],
) -> SafetyCheckResult? {
  let mut worst : SafetyCheckResult? = None
  for result in results {
    match worst {
      None => worst = Some(result)
      Some(previous) =>
        if result.margin() < previous.margin() {
          worst = Some(result)
        }
    }
  }
  worst
}

///|
pub fn safety_timestamp_consistent(previous : Int?, current : Int) -> Bool {
  match previous {
    None => true
    Some(value) => current >= value
  }
}