package ocelot.desktop.geometry

object Quaternion {
  def apply(v: Vector3D, w: Float): Quaternion = {
    Quaternion(v.x, v.y, v.z, w)
  }

  def identity: Quaternion = Quaternion(0, 0, 0, 1)

  def axisAngle(axis: Vector3D, angle: Float): Quaternion = {
    if (angle.abs < 1e-7) return Quaternion.identity
    val _axis = if (axis.length < 1e-7) Vector3D.AxisY else axis
    val cos = math.cos(angle * 0.5).toFloat
    val sin = math.sin(angle * 0.5).toFloat
    Quaternion(_axis.normalize * sin, cos)
  }

  def euler(euler: Vector3D): Quaternion = {
    val c1 = math.cos(euler.z * 0.5).toFloat
    val c2 = math.cos(euler.y * 0.5).toFloat
    val c3 = math.cos(euler.x * 0.5).toFloat
    val s1 = math.sin(euler.z * 0.5).toFloat
    val s2 = math.sin(euler.y * 0.5).toFloat
    val s3 = math.sin(euler.x * 0.5).toFloat

    val x = c1 * c2 * s3 - s1 * s2 * c3
    val y = c1 * s2 * c3 + s1 * c2 * s3
    val z = s1 * c2 * c3 - c1 * s2 * s3
    val w = c1 * c2 * c3 + s1 * s2 * s3

    Quaternion(x, y, z, w)
  }

  def between(v1: Vector3D, v2: Vector3D): Quaternion = {
    val v = v1.cross(v2)
    val w = v1.length * v2.length + v1.dot(v2)
    Quaternion(v, w)
  }
}

case class Quaternion(x: Float, y: Float, z: Float, w: Float) {
  def xyz: Vector3D = Vector3D(x, y, z)

  def *(s: Float): Quaternion = Quaternion(x * s, y * s, z * s, w * s)

  def /(s: Float): Quaternion = Quaternion(x / s, y / s, z / s, w / s)

  def unary_-(): Quaternion = Quaternion(-x, -y, -z, -w)

  def +(rhs: Quaternion): Quaternion = Quaternion(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w)

  def *(rhs: Vector3D): Vector3D = basis * rhs

  def *(rhs: Quaternion): Quaternion = Quaternion(
    y * rhs.z - z * rhs.y + x * rhs.w + w * rhs.x,
    z * rhs.x - x * rhs.z + y * rhs.w + w * rhs.y,
    x * rhs.y - y * rhs.x + z * rhs.w + w * rhs.z,
    w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
  )

  def conj: Quaternion = Quaternion(-x, -y, -z, w)

  def norm: Float = math.sqrt(x * x + y * y + z * z + w * w).toFloat

  def normalize: Quaternion = {
    val n = norm
    if (n > 0) this / n else this
  }

  def inverse: Quaternion = conj / norm

  def basis: Basis3D = Basis3D(
    Vector3D(1 - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w),
    Vector3D(2 * x * y - 2 * z * w, 1 - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * w),
    Vector3D(2 * x * z + 2 * y * w, 2 * y * z - 2 * x * w, 1 - 2 * x * x - 2 * y * y),
  )

  def dot(that: Quaternion): Float = x * that.x + y * that.y + z * that.z + w * that.w

  def lerp(that: Quaternion, alpha: Float): Quaternion = {
    (this * (1 - alpha) + that).normalize
  }

  def slerp(that: Quaternion, alpha: Float): Quaternion = {
    var q1 = this.normalize
    val q2 = that.normalize
    var dot = q1.dot(q2)

    if (dot < 0) {
      q1 = -q1
      dot = -dot
    }

    if (dot > 0.9995) {
      return q1.lerp(q2, alpha)
    }

    val theta = math.acos(dot)
    val sinTheta = math.sin(dot)

    val f1 = (math.sin((1 - alpha) * theta) / sinTheta).toFloat
    val f2 = (math.sin(alpha * theta) / sinTheta).toFloat

    (q1 * f1 + q2 * f2).normalize
  }

  override def toString: String = s"Quaternion [$x, $y, $z, $w]"
}
