///|
/// Unit quaternion representation of 3D rotations: q = w + x*i + y*j + z*k.
pub(all) struct Quaternion {
  w : Double
  x : Double
  y : Double
  z : Double
} derive(Debug, Eq, Default)

///|
/// Creates a new quaternion with given components.
pub fn Quaternion::new(
  w : Double,
  x : Double,
  y : Double,
  z : Double,
) -> Quaternion {
  { w, x, y, z }
}

///|
/// Identity quaternion representing zero rotation (1, 0, 0, 0).
pub fn Quaternion::identity() -> Quaternion {
  { w: 1.0, x: 0.0, y: 0.0, z: 0.0 }
}

///|
/// Creates a quaternion from an axis of rotation and an angle (in radians).
pub fn Quaternion::from_axis_angle(
  axis : Point3D,
  angle : Double,
) -> Quaternion {
  let norm = axis.norm()
  if norm < 1.0e-12 {
    return Quaternion::identity()
  }
  let half_angle = angle * 0.5
  let s = @math.sin(half_angle) / norm
  { w: @math.cos(half_angle), x: axis.x * s, y: axis.y * s, z: axis.z * s }
}

///|
/// Computes Euclidean norm (magnitude) of quaternion.
pub fn Quaternion::norm(self : Quaternion) -> Double {
  (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
}

///|
/// Normalizes the quaternion to unit length.
pub fn Quaternion::normalized(self : Quaternion) -> Quaternion {
  let n = self.norm()
  if n > 1.0e-12 {
    { w: self.w / n, x: self.x / n, y: self.y / n, z: self.z / n }
  } else {
    Quaternion::identity()
  }
}

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

///|
/// Computes quaternion inverse (q* / ||q||^2).
pub fn Quaternion::inverse(self : Quaternion) -> Quaternion {
  let n_sq = self.w * self.w +
    self.x * self.x +
    self.y * self.y +
    self.z * self.z
  if n_sq > 1.0e-14 {
    {
      w: self.w / n_sq,
      x: -self.x / n_sq,
      y: -self.y / n_sq,
      z: -self.z / n_sq,
    }
  } else {
    Quaternion::identity()
  }
}

///|
/// Hamilton product of two quaternions (q1 * q2).
pub fn Quaternion::mul(self : Quaternion, other : Quaternion) -> Quaternion {
  {
    w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
    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,
  }
}

///|
/// Rotates a 3D vector by this quaternion: v' = q * (0, v) * q^(-1).
pub fn Quaternion::rotate_point(self : Quaternion, p : Point3D) -> Point3D {
  let q = self.normalized()
  // Efficient formula: v' = v + 2*r x (r x v + w*v), where r = (x, y, z)
  let rx = q.x
  let ry = q.y
  let rz = q.z
  let w = q.w

  let tx = 2.0 * (ry * p.z - rz * p.y + w * p.x)
  let ty = 2.0 * (rz * p.x - rx * p.z + w * p.y)
  let tz = 2.0 * (rx * p.y - ry * p.x + w * p.z)

  {
    x: p.x + (ry * tz - rz * ty),
    y: p.y + (rz * tx - rx * tz),
    z: p.z + (rx * ty - ry * tx),
  }
}

///|
/// Converts quaternion to 3x3 rotation matrix.
pub fn Quaternion::to_rotation_matrix(self : Quaternion) -> Matrix3x3 {
  let q = self.normalized()
  let w = q.w
  let x = q.x
  let y = q.y
  let z = q.z

  let xx = x * x
  let yy = y * y
  let zz = z * z
  let xy = x * y
  let xz = x * z
  let yz = y * z
  let wx = w * x
  let wy = w * y
  let wz = w * z

  Matrix3x3::from_array([
    1.0 - 2.0 * (yy + zz),
    2.0 * (xy - wz),
    2.0 * (xz + wy),
    2.0 * (xy + wz),
    1.0 - 2.0 * (xx + zz),
    2.0 * (yz - wx),
    2.0 * (xz - wy),
    2.0 * (yz + wx),
    1.0 - 2.0 * (xx + yy),
  ])
}

///|
/// Converts 3x3 rotation matrix to quaternion using Shepperd's algorithm.
pub fn Quaternion::from_rotation_matrix(rot : Matrix3x3) -> Quaternion {
  let r00 = rot.get(0, 0)
  let r01 = rot.get(0, 1)
  let r02 = rot.get(0, 2)
  let r10 = rot.get(1, 0)
  let r11 = rot.get(1, 1)
  let r12 = rot.get(1, 2)
  let r20 = rot.get(2, 0)
  let r21 = rot.get(2, 1)
  let r22 = rot.get(2, 2)

  let trace = r00 + r11 + r22
  if trace > 0.0 {
    let s = (trace + 1.0).sqrt() * 2.0
    { w: 0.25 * s, x: (r21 - r12) / s, y: (r02 - r20) / s, z: (r10 - r01) / s }
  } else if r00 > r11 && r00 > r22 {
    let s = (1.0 + r00 - r11 - r22).sqrt() * 2.0
    { w: (r21 - r12) / s, x: 0.25 * s, y: (r01 + r10) / s, z: (r02 + r20) / s }
  } else if r11 > r22 {
    let s = (1.0 + r11 - r00 - r22).sqrt() * 2.0
    { w: (r02 - r20) / s, x: (r01 + r10) / s, y: 0.25 * s, z: (r12 + r21) / s }
  } else {
    let s = (1.0 + r22 - r00 - r11).sqrt() * 2.0
    { w: (r10 - r01) / s, x: (r02 + r20) / s, y: (r12 + r21) / s, z: 0.25 * s }
  }
}

///|
/// Converts quaternion to Rodrigues rotation vector.
pub fn Quaternion::to_rvec(self : Quaternion) -> Point3D {
  let q = self.normalized()
  let v_norm = (q.x * q.x + q.y * q.y + q.z * q.z).sqrt()
  if v_norm < 1.0e-12 {
    { x: 0.0, y: 0.0, z: 0.0 }
  } else {
    let angle = 2.0 * @math.atan2(v_norm, q.w)
    let s = angle / v_norm
    { x: q.x * s, y: q.y * s, z: q.z * s }
  }
}

///|
/// Creates a quaternion from Rodrigues rotation vector.
pub fn Quaternion::from_rvec(rvec : Point3D) -> Quaternion {
  let theta = rvec.norm()
  if theta < 1.0e-12 {
    Quaternion::identity()
  } else {
    Quaternion::from_axis_angle(rvec, theta)
  }
}

///|
/// Converts quaternion to Euler angles (Yaw, Pitch, Roll in radians, ZYX convention).
pub fn Quaternion::to_euler_zyx(self : Quaternion) -> (Double, Double, Double) {
  let q = self.normalized()
  let sin_pitch = 2.0 * (q.w * q.y - q.z * q.x)
  let pitch = if sin_pitch.abs() >= 1.0 {
    if sin_pitch > 0.0 {
      1.5707963267948966
    } else {
      -1.5707963267948966
    }
  } else {
    @math.asin(sin_pitch)
  }

  let roll = @math.atan2(
    2.0 * (q.w * q.x + q.y * q.z),
    1.0 - 2.0 * (q.x * q.x + q.y * q.y),
  )
  let yaw = @math.atan2(
    2.0 * (q.w * q.z + q.x * q.y),
    1.0 - 2.0 * (q.y * q.y + q.z * q.z),
  )

  (yaw, pitch, roll)
}

///|
/// Creates quaternion from Euler angles (Yaw around Z, Pitch around Y, Roll around X).
pub fn Quaternion::from_euler_zyx(
  yaw : Double,
  pitch : Double,
  roll : Double,
) -> Quaternion {
  let cy = @math.cos(yaw * 0.5)
  let sy = @math.sin(yaw * 0.5)
  let cp = @math.cos(pitch * 0.5)
  let sp = @math.sin(pitch * 0.5)
  let cr = @math.cos(roll * 0.5)
  let sr = @math.sin(roll * 0.5)

  {
    w: cr * cp * cy + sr * sp * sy,
    x: sr * cp * cy - cr * sp * sy,
    y: cr * sp * cy + sr * cp * sy,
    z: cr * cp * sy - sr * sp * cy,
  }
}

///|
/// Spherical Linear Interpolation (SLERP) between two quaternions with factor t in [0, 1].
pub fn Quaternion::slerp(
  q1 : Quaternion,
  q2 : Quaternion,
  t : Double,
) -> Quaternion {
  let q1n = q1.normalized()
  let mut q2n = q2.normalized()

  let mut dot = q1n.w * q2n.w + q1n.x * q2n.x + q1n.y * q2n.y + q1n.z * q2n.z
  if dot < 0.0 {
    q2n = { w: -q2n.w, x: -q2n.x, y: -q2n.y, z: -q2n.z }
    dot = -dot
  }

  if dot > 0.9995 {
    // Linear interpolation when very close
    let w = q1n.w + t * (q2n.w - q1n.w)
    let x = q1n.x + t * (q2n.x - q1n.x)
    let y = q1n.y + t * (q2n.y - q1n.y)
    let z = q1n.z + t * (q2n.z - q1n.z)
    return { w, x, y, z }.normalized()
  }

  let theta_0 = @math.acos(dot)
  let theta = theta_0 * t
  let sin_theta = @math.sin(theta)
  let sin_theta_0 = @math.sin(theta_0)

  let s0 = @math.cos(theta_0 - theta) - dot * sin_theta / sin_theta_0
  let s1 = sin_theta / sin_theta_0

  {
    w: s0 * q1n.w + s1 * q2n.w,
    x: s0 * q1n.x + s1 * q2n.x,
    y: s0 * q1n.y + s1 * q2n.y,
    z: s0 * q1n.z + s1 * q2n.z,
  }.normalized()
}

///|
/// SO(3) Lie algebra exponential map: so(3) -> SO(3) (vector omega to Quaternion).
pub fn so3_exp(omega : Point3D) -> Quaternion {
  Quaternion::from_rvec(omega)
}

///|
/// SO(3) Lie algebra logarithm map: SO(3) -> so(3) (Quaternion to vector omega).
pub fn so3_log(q : Quaternion) -> Point3D {
  q.to_rvec()
}