///|
/// A unit-friendly quaternion for camera, scene, and object orientation.
pub(all) struct Quaternion {
  x : Double
  y : Double
  z : Double
  w : Double
} derive(Debug)

///|
pub fn quaternion(
  x~ : Double,
  y~ : Double,
  z~ : Double,
  w~ : Double,
) -> Quaternion {
  { x, y, z, w }
}

///|
pub fn Quaternion::identity() -> Quaternion {
  { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
}

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

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

///|
pub fn Quaternion::z(self : Quaternion) -> Double {
  self.z
}

///|
pub fn Quaternion::w(self : Quaternion) -> Double {
  self.w
}

///|
pub fn Quaternion::length_squared(self : Quaternion) -> Double {
  self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
}

///|
pub fn Quaternion::length(self : Quaternion) -> Double {
  self.length_squared().sqrt()
}

///|
pub fn Quaternion::normalized(self : Quaternion) -> Quaternion {
  let length = self.length()
  if length <= 0.000000000001 {
    Quaternion::identity()
  } else {
    {
      x: self.x / length,
      y: self.y / length,
      z: self.z / length,
      w: self.w / length,
    }
  }
}

///|
pub fn Quaternion::conjugate(self : Quaternion) -> Quaternion {
  { x: -self.x, y: -self.y, z: -self.z, w: self.w }
}

///|
pub fn Quaternion::dot(self : Quaternion, other : Quaternion) -> Double {
  self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w
}

///|
pub fn Quaternion::negated(self : Quaternion) -> Quaternion {
  { x: -self.x, y: -self.y, z: -self.z, w: -self.w }
}

///|
pub fn Quaternion::multiply(
  self : Quaternion,
  other : Quaternion,
) -> Quaternion {
  {
    x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
    y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
    z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
    w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
  }
}

///|
pub fn Quaternion::slerp(
  self : Quaternion,
  other : Quaternion,
  amount : Double,
) -> Quaternion {
  let first = self.normalized()
  let mut second = other.normalized()
  let mut cosine = first.dot(second)
  if cosine < 0.0 {
    second = second.negated()
    cosine = -cosine
  }
  if cosine > 0.9995 {
    return quaternion(
      x=first.x + (second.x - first.x) * amount,
      y=first.y + (second.y - first.y) * amount,
      z=first.z + (second.z - first.z) * amount,
      w=first.w + (second.w - first.w) * amount,
    ).normalized()
  }
  let angle = @math.acos(cosine.max(-1.0).min(1.0))
  let sine = @math.sin(angle)
  if sine.abs() < 0.000000001 {
    first
  } else {
    let first_weight = @math.sin((1.0 - amount) * angle) / sine
    let second_weight = @math.sin(amount * angle) / sine
    quaternion(
      x=first.x * first_weight + second.x * second_weight,
      y=first.y * first_weight + second.y * second_weight,
      z=first.z * first_weight + second.z * second_weight,
      w=first.w * first_weight + second.w * second_weight,
    ).normalized()
  }
}

///|
pub fn Quaternion::from_axis_angle(axis : Point3, angle : Double) -> Quaternion {
  let unit = point3_normalized(axis)
  let half = angle / 2.0
  let sine = @math.sin(half)
  quaternion(
    x=unit.x() * sine,
    y=unit.y() * sine,
    z=unit.z() * sine,
    w=@math.cos(half),
  ).normalized()
}

///|
pub fn Quaternion::from_euler(
  pitch : Double,
  yaw : Double,
  roll : Double,
) -> Quaternion {
  let half_pitch = pitch / 2.0
  let half_yaw = yaw / 2.0
  let half_roll = roll / 2.0
  let sp = @math.sin(half_pitch)
  let cp = @math.cos(half_pitch)
  let sy = @math.sin(half_yaw)
  let cy = @math.cos(half_yaw)
  let sr = @math.sin(half_roll)
  let cr = @math.cos(half_roll)
  quaternion(
    x=sp * cy * cr - cp * sy * sr,
    y=cp * sy * cr + sp * cy * sr,
    z=cp * cy * sr - sp * sy * cr,
    w=cp * cy * cr + sp * sy * sr,
  ).normalized()
}

///|
pub fn Quaternion::rotate(self : Quaternion, value : Point3) -> Point3 {
  let rotation = self.normalized()
  let vector = quaternion(x=value.x(), y=value.y(), z=value.z(), w=0.0)
  let rotated = rotation.multiply(vector).multiply(rotation.conjugate())
  point3(x=rotated.x, y=rotated.y, z=rotated.z)
}

///|
pub fn Quaternion::forward(self : Quaternion) -> Point3 {
  self.rotate(point3(x=0.0, y=0.0, z=-1.0))
}

///|
pub fn Quaternion::up(self : Quaternion) -> Point3 {
  self.rotate(point3(x=0.0, y=1.0, z=0.0))
}

///|
pub fn Quaternion::right(self : Quaternion) -> Point3 {
  self.rotate(point3(x=1.0, y=0.0, z=0.0))
}

///|
/// A 4x4 affine matrix stored in row-major order.
pub(all) struct Matrix4 {
  m00 : Double
  m01 : Double
  m02 : Double
  m03 : Double
  m10 : Double
  m11 : Double
  m12 : Double
  m13 : Double
  m20 : Double
  m21 : Double
  m22 : Double
  m23 : Double
  m30 : Double
  m31 : Double
  m32 : Double
  m33 : Double
} derive(Debug)

///|
pub fn Matrix4::identity() -> Matrix4 {
  {
    m00: 1.0,
    m01: 0.0,
    m02: 0.0,
    m03: 0.0,
    m10: 0.0,
    m11: 1.0,
    m12: 0.0,
    m13: 0.0,
    m20: 0.0,
    m21: 0.0,
    m22: 1.0,
    m23: 0.0,
    m30: 0.0,
    m31: 0.0,
    m32: 0.0,
    m33: 1.0,
  }
}

///|
pub fn Matrix4::translation(value : Point3) -> Matrix4 {
  let result = Matrix4::identity()
  { ..result, m03: value.x(), m13: value.y(), m23: value.z() }
}

///|
pub fn Matrix4::scale(value : Point3) -> Matrix4 {
  let result = Matrix4::identity()
  { ..result, m00: value.x(), m11: value.y(), m22: value.z() }
}

///|
pub fn Matrix4::from_quaternion(value : Quaternion) -> Matrix4 {
  let q = value.normalized()
  let xx = q.x * q.x
  let yy = q.y * q.y
  let zz = q.z * q.z
  let xy = q.x * q.y
  let xz = q.x * q.z
  let yz = q.y * q.z
  let wx = q.w * q.x
  let wy = q.w * q.y
  let wz = q.w * q.z
  {
    m00: 1.0 - 2.0 * (yy + zz),
    m01: 2.0 * (xy - wz),
    m02: 2.0 * (xz + wy),
    m03: 0.0,
    m10: 2.0 * (xy + wz),
    m11: 1.0 - 2.0 * (xx + zz),
    m12: 2.0 * (yz - wx),
    m13: 0.0,
    m20: 2.0 * (xz - wy),
    m21: 2.0 * (yz + wx),
    m22: 1.0 - 2.0 * (xx + yy),
    m23: 0.0,
    m30: 0.0,
    m31: 0.0,
    m32: 0.0,
    m33: 1.0,
  }
}

///|
pub fn Matrix4::multiply(self : Matrix4, other : Matrix4) -> Matrix4 {
  {
    m00: self.m00 * other.m00 +
    self.m01 * other.m10 +
    self.m02 * other.m20 +
    self.m03 * other.m30,
    m01: self.m00 * other.m01 +
    self.m01 * other.m11 +
    self.m02 * other.m21 +
    self.m03 * other.m31,
    m02: self.m00 * other.m02 +
    self.m01 * other.m12 +
    self.m02 * other.m22 +
    self.m03 * other.m32,
    m03: self.m00 * other.m03 +
    self.m01 * other.m13 +
    self.m02 * other.m23 +
    self.m03 * other.m33,
    m10: self.m10 * other.m00 +
    self.m11 * other.m10 +
    self.m12 * other.m20 +
    self.m13 * other.m30,
    m11: self.m10 * other.m01 +
    self.m11 * other.m11 +
    self.m12 * other.m21 +
    self.m13 * other.m31,
    m12: self.m10 * other.m02 +
    self.m11 * other.m12 +
    self.m12 * other.m22 +
    self.m13 * other.m32,
    m13: self.m10 * other.m03 +
    self.m11 * other.m13 +
    self.m12 * other.m23 +
    self.m13 * other.m33,
    m20: self.m20 * other.m00 +
    self.m21 * other.m10 +
    self.m22 * other.m20 +
    self.m23 * other.m30,
    m21: self.m20 * other.m01 +
    self.m21 * other.m11 +
    self.m22 * other.m21 +
    self.m23 * other.m31,
    m22: self.m20 * other.m02 +
    self.m21 * other.m12 +
    self.m22 * other.m22 +
    self.m23 * other.m32,
    m23: self.m20 * other.m03 +
    self.m21 * other.m13 +
    self.m22 * other.m23 +
    self.m23 * other.m33,
    m30: self.m30 * other.m00 +
    self.m31 * other.m10 +
    self.m32 * other.m20 +
    self.m33 * other.m30,
    m31: self.m30 * other.m01 +
    self.m31 * other.m11 +
    self.m32 * other.m21 +
    self.m33 * other.m31,
    m32: self.m30 * other.m02 +
    self.m31 * other.m12 +
    self.m32 * other.m22 +
    self.m33 * other.m32,
    m33: self.m30 * other.m03 +
    self.m31 * other.m13 +
    self.m32 * other.m23 +
    self.m33 * other.m33,
  }
}

///|
pub fn Matrix4::transform_point(self : Matrix4, value : Point3) -> Point3 {
  let x = self.m00 * value.x() +
    self.m01 * value.y() +
    self.m02 * value.z() +
    self.m03
  let y = self.m10 * value.x() +
    self.m11 * value.y() +
    self.m12 * value.z() +
    self.m13
  let z = self.m20 * value.x() +
    self.m21 * value.y() +
    self.m22 * value.z() +
    self.m23
  let w = self.m30 * value.x() +
    self.m31 * value.y() +
    self.m32 * value.z() +
    self.m33
  if w == 0.0 || w == 1.0 {
    point3(x~, y~, z~)
  } else {
    point3(x=x / w, y=y / w, z=z / w)
  }
}

///|
/// A compact affine transform for scene graphs and camera rigs.
pub(all) struct Transform3D {
  position : Point3
  rotation : Quaternion
  scale : Point3
} derive(Debug)

///|
pub fn transform3d(
  position~ : Point3,
  rotation~ : Quaternion,
  scale~ : Point3,
) -> Transform3D {
  { position, rotation: rotation.normalized(), scale }
}

///|
pub fn Transform3D::identity() -> Transform3D {
  transform3d(
    position=point3(x=0.0, y=0.0, z=0.0),
    rotation=Quaternion::identity(),
    scale=point3(x=1.0, y=1.0, z=1.0),
  )
}

///|
pub fn Transform3D::position(self : Transform3D) -> Point3 {
  self.position
}

///|
pub fn Transform3D::rotation(self : Transform3D) -> Quaternion {
  self.rotation
}

///|
pub fn Transform3D::scale(self : Transform3D) -> Point3 {
  self.scale
}

///|
pub fn Transform3D::matrix(self : Transform3D) -> Matrix4 {
  Matrix4::translation(self.position).multiply(
    Matrix4::from_quaternion(self.rotation).multiply(Matrix4::scale(self.scale)),
  )
}

///|
pub fn Transform3D::apply(self : Transform3D, value : Point3) -> Point3 {
  let rotated = self.rotation.rotate({
    x: value.x() * self.scale.x(),
    y: value.y() * self.scale.y(),
    z: value.z() * self.scale.z(),
  })
  point3(
    x=rotated.x() + self.position.x(),
    y=rotated.y() + self.position.y(),
    z=rotated.z() + self.position.z(),
  )
}

///|
pub fn Transform3D::combine(
  self : Transform3D,
  child : Transform3D,
) -> Transform3D {
  transform3d(
    position=self.apply(child.position),
    rotation=self.rotation.multiply(child.rotation).normalized(),
    scale=point3(
      x=self.scale.x() * child.scale.x(),
      y=self.scale.y() * child.scale.y(),
      z=self.scale.z() * child.scale.z(),
    ),
  )
}

///|
pub fn Transform3D::inverse(self : Transform3D) -> Transform3D {
  let inverse_scale = point3(
    x=if self.scale.x() == 0.0 { 0.0 } else { 1.0 / self.scale.x() },
    y=if self.scale.y() == 0.0 { 0.0 } else { 1.0 / self.scale.y() },
    z=if self.scale.z() == 0.0 { 0.0 } else { 1.0 / self.scale.z() },
  )
  let inverse_rotation = self.rotation.conjugate().normalized()
  let translated = point3(
    x=-self.position.x() * inverse_scale.x(),
    y=-self.position.y() * inverse_scale.y(),
    z=-self.position.z() * inverse_scale.z(),
  )
  transform3d(
    position=inverse_rotation.rotate(translated),
    rotation=inverse_rotation,
    scale=inverse_scale,
  )
}

///|
pub fn Transform3D::lerp(
  self : Transform3D,
  other : Transform3D,
  amount : Double,
) -> Transform3D {
  let ratio = clamp01(amount)
  transform3d(
    position=self.position.lerp(other.position, ratio),
    rotation=self.rotation.slerp(other.rotation, ratio),
    scale=self.scale.lerp(other.scale, ratio),
  )
}

///|
pub fn Transform3D::with_position(
  self : Transform3D,
  position : Point3,
) -> Transform3D {
  { ..self, position, }
}

///|
pub fn Transform3D::with_rotation(
  self : Transform3D,
  rotation : Quaternion,
) -> Transform3D {
  { ..self, rotation: rotation.normalized() }
}

///|
pub fn Transform3D::with_scale(
  self : Transform3D,
  scale : Point3,
) -> Transform3D {
  { ..self, scale, }
}

///|
/// A keyframe for a 3D transform track.
pub struct Transform3Keyframe {
  time : Double
  value : Transform3D
  curve : Curve
} derive(Debug)

///|
pub fn transform3_keyframe(
  time : Double,
  value~ : Transform3D,
  curve? : Curve = Curve::builtin(Linear),
) -> Transform3Keyframe raise MotionError {
  ensure_finite(time)
  { time, value, curve }
}

///|
pub fn Transform3Keyframe::time(self : Transform3Keyframe) -> Double {
  self.time
}

///|
pub fn Transform3Keyframe::value(self : Transform3Keyframe) -> Transform3D {
  self.value
}

///|
pub fn Transform3Keyframe::curve(self : Transform3Keyframe) -> Curve {
  self.curve
}

///|
/// Interpolates translation, scale, and rotation independently.
pub struct Transform3Track {
  frames : Array[Transform3Keyframe]
} derive(Debug)

///|
pub fn Transform3Track::new(
  frames : Array[Transform3Keyframe],
) -> Transform3Track raise MotionError {
  if frames.length() == 0 {
    raise MotionError::EmptyTrack
  }
  for index in 1.. Array[Transform3Keyframe] {
  self.frames.copy()
}

///|
pub fn Transform3Track::length(self : Transform3Track) -> Int {
  self.frames.length()
}

///|
pub fn Transform3Track::start_time(self : Transform3Track) -> Double {
  self.frames[0].time
}

///|
pub fn Transform3Track::end_time(self : Transform3Track) -> Double {
  self.frames[self.frames.length() - 1].time
}

///|
pub fn Transform3Track::duration(self : Transform3Track) -> Double {
  self.end_time() - self.start_time()
}

///|
fn Transform3Track::inside(
  self : Transform3Track,
  time : Double,
) -> Transform3D {
  let last = self.frames.length() - 1
  if time <= self.frames[0].time {
    return self.frames[0].value
  }
  if time >= self.frames[last].time {
    return self.frames[last].value
  }
  for index in 0.. Transform3D {
  let mapped = map_track_time(time, self.start_time(), self.end_time(), mode)
  self.inside(mapped)
}

///|
pub fn Transform3Track::velocity(
  self : Transform3Track,
  time : Double,
) -> Point3 {
  let delta = (self.duration() / 10000.0).max(0.000001)
  let before = self.sample(time - delta).position()
  let after = self.sample(time + delta).position()
  point3(
    x=(after.x() - before.x()) / (2.0 * delta),
    y=(after.y() - before.y()) / (2.0 * delta),
    z=(after.z() - before.z()) / (2.0 * delta),
  )
}

///|
pub fn Transform3Track::sample_many(
  self : Transform3Track,
  count : Int,
  mode? : Extrapolation = Clamp,
) -> Array[Transform3D] raise MotionError {
  if count < 2 {
    raise MotionError::InvalidSampleCount(count)
  }
  let result : Array[Transform3D] = []
  for index in 0.. Transform3Track raise MotionError {
  ensure_finite(offset)
  let frames : Array[Transform3Keyframe] = []
  for frame in self.frames {
    frames.push(
      transform3_keyframe(
        frame.time + offset,
        value=frame.value,
        curve=frame.curve,
      ),
    )
  }
  Transform3Track::new(frames)
}

///|
pub fn Transform3Track::scale_time(
  self : Transform3Track,
  factor : Double,
) -> Transform3Track raise MotionError {
  ensure_finite(factor)
  if factor <= 0.0 {
    raise MotionError::InvalidDuration(factor)
  }
  let frames : Array[Transform3Keyframe] = []
  for frame in self.frames {
    frames.push(
      transform3_keyframe(
        frame.time * factor,
        value=frame.value,
        curve=frame.curve,
      ),
    )
  }
  Transform3Track::new(frames)
}

///|
pub fn Transform3Track::map_values(
  self : Transform3Track,
  mapper : (Transform3D) -> Transform3D,
) -> Transform3Track raise MotionError {
  let frames : Array[Transform3Keyframe] = []
  for frame in self.frames {
    frames.push(
      transform3_keyframe(
        frame.time,
        value=mapper(frame.value),
        curve=frame.curve,
      ),
    )
  }
  Transform3Track::new(frames)
}