///|
/// Limits for production motion sampling. The profile is independent of a
/// solver so the same policy can be used for preview, export, and validation.
pub struct MotionLimitProfile2 {
  maximum_velocity : Double
  maximum_acceleration : Double
  maximum_jerk : Double
  tolerance : Double
  iterations : Int
} derive(Debug)

///|
pub fn motion_limit_profile2(
  maximum_velocity? : Double = 1000000.0,
  maximum_acceleration? : Double = 1000000.0,
  maximum_jerk? : Double = 1000000.0,
  tolerance? : Double = 0.000001,
  iterations? : Int = 8,
) -> MotionLimitProfile2 {
  {
    maximum_velocity: maximum_velocity.max(0.0),
    maximum_acceleration: maximum_acceleration.max(0.0),
    maximum_jerk: maximum_jerk.max(0.0),
    tolerance: tolerance.max(0.000000000001),
    iterations: iterations.max(1),
  }
}

///|
pub fn MotionLimitProfile2::maximum_velocity(
  self : MotionLimitProfile2,
) -> Double {
  self.maximum_velocity
}

///|
pub fn MotionLimitProfile2::maximum_acceleration(
  self : MotionLimitProfile2,
) -> Double {
  self.maximum_acceleration
}

///|
pub fn MotionLimitProfile2::maximum_jerk(self : MotionLimitProfile2) -> Double {
  self.maximum_jerk
}

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

///|
pub fn MotionLimitProfile2::iterations(self : MotionLimitProfile2) -> Int {
  self.iterations
}

///|
pub enum ConstraintStatus2 {
  ConstraintSatisfied
  VelocityLimited
  AccelerationLimited
  JerkLimited
  PositionClamped
} derive(Debug)

///|
pub fn constraint_statuses2() -> Array[ConstraintStatus2] {
  [
    ConstraintSatisfied,
    VelocityLimited,
    AccelerationLimited,
    JerkLimited,
    PositionClamped,
  ]
}

///|
pub struct ConstraintResult2 {
  value : Double
  velocity : Double
  acceleration : Double
  status : ConstraintStatus2
  iterations : Int
  error : Double
} derive(Debug)

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

///|
pub fn ConstraintResult2::velocity(self : ConstraintResult2) -> Double {
  self.velocity
}

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

///|
pub fn ConstraintResult2::status(self : ConstraintResult2) -> ConstraintStatus2 {
  self.status
}

///|
pub fn ConstraintResult2::iterations(self : ConstraintResult2) -> Int {
  self.iterations
}

///|
pub fn ConstraintResult2::error(self : ConstraintResult2) -> Double {
  self.error
}

///|
fn limit_scalar_step(
  target : Double,
  current : Double,
  velocity : Double,
  acceleration : Double,
  dt : Double,
  limit : MotionLimitProfile2,
) -> ConstraintResult2 {
  let safe_dt = dt.max(0.000000000001)
  let desired_velocity = (target - current) / safe_dt
  let velocity_delta = desired_velocity - velocity
  let maximum_velocity = limit.maximum_velocity
  let bounded_velocity = clamp(
    desired_velocity,
    -maximum_velocity,
    maximum_velocity,
  )
  let acceleration_delta = bounded_velocity - velocity
  let desired_acceleration = acceleration_delta / safe_dt
  let bounded_acceleration = clamp(
    desired_acceleration,
    -limit.maximum_acceleration,
    limit.maximum_acceleration,
  )
  let jerk_delta = bounded_acceleration - acceleration
  let maximum_acceleration_delta = limit.maximum_jerk * safe_dt
  let bounded_jerk = clamp(
    jerk_delta,
    -maximum_acceleration_delta,
    maximum_acceleration_delta,
  )
  let result_acceleration = acceleration + bounded_jerk
  let result_velocity = velocity + result_acceleration * safe_dt
  let value = current + result_velocity * safe_dt
  let status = if bounded_velocity != desired_velocity {
    VelocityLimited
  } else if bounded_acceleration != desired_acceleration {
    AccelerationLimited
  } else if bounded_jerk != jerk_delta {
    JerkLimited
  } else {
    ConstraintSatisfied
  }
  {
    value,
    velocity: result_velocity,
    acceleration: result_acceleration,
    status,
    iterations: 1,
    error: (target - value).abs().max(velocity_delta.abs() * safe_dt),
  }
}

///|
pub fn constrain_scalar_step(
  target : Double,
  current : Double,
  velocity : Double,
  acceleration : Double,
  dt : Double,
  limit : MotionLimitProfile2,
) -> ConstraintResult2 {
  limit_scalar_step(target, current, velocity, acceleration, dt, limit)
}

///|
pub struct Point2ConstraintResult {
  value : Point2
  velocity : Point2
  acceleration : Point2
  status : ConstraintStatus2
  error : Double
} derive(Debug)

///|
pub fn Point2ConstraintResult::value(self : Point2ConstraintResult) -> Point2 {
  self.value
}

///|
pub fn Point2ConstraintResult::velocity(
  self : Point2ConstraintResult,
) -> Point2 {
  self.velocity
}

///|
pub fn Point2ConstraintResult::acceleration(
  self : Point2ConstraintResult,
) -> Point2 {
  self.acceleration
}

///|
pub fn Point2ConstraintResult::status(
  self : Point2ConstraintResult,
) -> ConstraintStatus2 {
  self.status
}

///|
pub fn Point2ConstraintResult::error(self : Point2ConstraintResult) -> Double {
  self.error
}

///|
pub fn constrain_point2_step(
  target : Point2,
  current : Point2,
  velocity : Point2,
  acceleration : Point2,
  dt : Double,
  limit : MotionLimitProfile2,
) -> Point2ConstraintResult {
  let x = limit_scalar_step(
    target.x(),
    current.x(),
    velocity.x(),
    acceleration.x(),
    dt,
    limit,
  )
  let y = limit_scalar_step(
    target.y(),
    current.y(),
    velocity.y(),
    acceleration.y(),
    dt,
    limit,
  )
  let status = match (x.status, y.status) {
    (ConstraintSatisfied, ConstraintSatisfied) => ConstraintSatisfied
    (JerkLimited, _) | (_, JerkLimited) => JerkLimited
    (AccelerationLimited, _) | (_, AccelerationLimited) => AccelerationLimited
    (VelocityLimited, _) | (_, VelocityLimited) => VelocityLimited
    (_, _) => PositionClamped
  }
  {
    value: point2(x=x.value, y=y.value),
    velocity: point2(x=x.velocity, y=y.velocity),
    acceleration: point2(x=x.acceleration, y=y.acceleration),
    status,
    error: @math.hypot(x.error, y.error),
  }
}

///|
pub struct Point3ConstraintResult {
  value : Point3
  velocity : Point3
  acceleration : Point3
  status : ConstraintStatus2
  error : Double
} derive(Debug)

///|
pub fn Point3ConstraintResult::value(self : Point3ConstraintResult) -> Point3 {
  self.value
}

///|
pub fn Point3ConstraintResult::velocity(
  self : Point3ConstraintResult,
) -> Point3 {
  self.velocity
}

///|
pub fn Point3ConstraintResult::acceleration(
  self : Point3ConstraintResult,
) -> Point3 {
  self.acceleration
}

///|
pub fn Point3ConstraintResult::status(
  self : Point3ConstraintResult,
) -> ConstraintStatus2 {
  self.status
}

///|
pub fn Point3ConstraintResult::error(self : Point3ConstraintResult) -> Double {
  self.error
}

///|
pub fn constrain_point3_step(
  target : Point3,
  current : Point3,
  velocity : Point3,
  acceleration : Point3,
  dt : Double,
  limit : MotionLimitProfile2,
) -> Point3ConstraintResult {
  let x = limit_scalar_step(
    target.x(),
    current.x(),
    velocity.x(),
    acceleration.x(),
    dt,
    limit,
  )
  let y = limit_scalar_step(
    target.y(),
    current.y(),
    velocity.y(),
    acceleration.y(),
    dt,
    limit,
  )
  let z = limit_scalar_step(
    target.z(),
    current.z(),
    velocity.z(),
    acceleration.z(),
    dt,
    limit,
  )
  let status = match (x.status, y.status, z.status) {
    (ConstraintSatisfied, ConstraintSatisfied, ConstraintSatisfied) =>
      ConstraintSatisfied
    (JerkLimited, _, _) | (_, JerkLimited, _) | (_, _, JerkLimited) =>
      JerkLimited
    (AccelerationLimited, _, _)
    | (_, AccelerationLimited, _)
    | (_, _, AccelerationLimited) => AccelerationLimited
    (VelocityLimited, _, _)
    | (_, VelocityLimited, _)
    | (_, _, VelocityLimited) => VelocityLimited
    (_, _, _) => PositionClamped
  }
  {
    value: point3(x=x.value, y=y.value, z=z.value),
    velocity: point3(x=x.velocity, y=y.velocity, z=z.velocity),
    acceleration: point3(x=x.acceleration, y=y.acceleration, z=z.acceleration),
    status,
    error: @math.hypot(@math.hypot(x.error, y.error), z.error),
  }
}

///|
pub struct SoftRange2 {
  minimum : Double
  maximum : Double
  softness : Double
  restitution : Double
} derive(Debug)

///|
pub fn soft_range2(
  minimum : Double,
  maximum : Double,
  softness? : Double = 0.0,
  restitution? : Double = 0.0,
) -> SoftRange2 {
  {
    minimum: minimum.min(maximum),
    maximum: maximum.max(minimum),
    softness: softness.max(0.0),
    restitution: clamp01(restitution),
  }
}

///|
pub fn SoftRange2::minimum(self : SoftRange2) -> Double {
  self.minimum
}

///|
pub fn SoftRange2::maximum(self : SoftRange2) -> Double {
  self.maximum
}

///|
pub fn SoftRange2::softness(self : SoftRange2) -> Double {
  self.softness
}

///|
pub fn SoftRange2::restitution(self : SoftRange2) -> Double {
  self.restitution
}

///|
pub fn SoftRange2::contains(self : SoftRange2, value : Double) -> Bool {
  value >= self.minimum && value <= self.maximum
}

///|
pub fn SoftRange2::resolve(
  self : SoftRange2,
  value : Double,
  velocity : Double,
) -> (Double, Double, ConstraintStatus2) {
  if self.contains(value) {
    return (value, velocity, ConstraintSatisfied)
  }
  if value < self.minimum {
    let penetration = self.minimum - value
    let correction = if self.softness == 0.0 {
      penetration
    } else {
      penetration.min(self.softness)
    }
    return (value + correction, -velocity * self.restitution, PositionClamped)
  }
  let penetration = value - self.maximum
  let correction = if self.softness == 0.0 {
    penetration
  } else {
    penetration.min(self.softness)
  }
  (value - correction, -velocity * self.restitution, PositionClamped)
}

///|
pub fn project_to_range(
  value : Double,
  minimum : Double,
  maximum : Double,
) -> Double {
  clamp(value, minimum.min(maximum), maximum.max(minimum))
}

///|
pub fn damp_to_range(
  value : Double,
  minimum : Double,
  maximum : Double,
  strength : Double,
) -> Double {
  let projected = project_to_range(value, minimum, maximum)
  value + (projected - value) * clamp01(strength)
}

///|
pub fn solve_distance_constraint(
  current : Point3,
  target : Point3,
  distance : Double,
) -> Point3 {
  let delta = point3(
    x=target.x() - current.x(),
    y=target.y() - current.y(),
    z=target.z() - current.z(),
  )
  let length = point3_length(delta)
  if length == 0.0 {
    return current
  }
  point3(
    x=current.x() + delta.x() * distance / length,
    y=current.y() + delta.y() * distance / length,
    z=current.z() + delta.z() * distance / length,
  )
}