package ocelot.desktop.geometry

object Basis3D {
  def identity: Basis3D = Basis3D(Vector3D.AxisX, Vector3D.AxisY, Vector3D.AxisZ)

  def rotate(q: Quaternion): Basis3D = q.basis

  def rotate(axis: Vector3D, angle: Float): Basis3D = Basis3D.rotate(Quaternion.axisAngle(axis, angle))

  def scale(v: Vector3D): Basis3D = Basis3D(Vector3D(v.x, 0, 0), Vector3D(0, v.y, 0), Vector3D(0, 0, v.z))

  def scale(s: Float): Basis3D = Basis3D(Vector3D(s, 0, 0), Vector3D(0, s, 0), Vector3D(0, 0, s))

  def lookingAt(target: Vector3D, up: Vector3D = Vector3D.Up): Basis3D = {
    val v_z = target.normalize
    val v_x = up.cross(v_z).normalize
    val v_y = v_z.cross(v_x)
    Basis3D(v_x, v_y, v_z)
  }
}

case class Basis3D(x: Vector3D, y: Vector3D, z: Vector3D) {
  def *(s: Float): Basis3D = Basis3D(x * s, y * s, z * s)

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

  def *(rhs: Vector3D): Vector3D = Vector3D(
    x.x * rhs.x + y.x * rhs.y + z.x * rhs.z,
    x.y * rhs.x + y.y * rhs.y + z.y * rhs.z,
    x.z * rhs.x + y.z * rhs.y + z.z * rhs.z,
  )

  def *(rhs: Basis3D): Basis3D = Basis3D(
    Vector3D(
      x.x * rhs.x.x + y.x * rhs.x.y + z.x * rhs.x.z,
      x.y * rhs.x.x + y.y * rhs.x.y + z.y * rhs.x.z,
      x.z * rhs.x.x + y.z * rhs.x.y + z.z * rhs.x.z,
    ),
    Vector3D(
      x.x * rhs.y.x + y.x * rhs.y.y + z.x * rhs.y.z,
      x.y * rhs.y.x + y.y * rhs.y.y + z.y * rhs.y.z,
      x.z * rhs.y.x + y.z * rhs.y.y + z.z * rhs.y.z,
    ),
    Vector3D(
      x.x * rhs.z.x + y.x * rhs.z.y + z.x * rhs.z.z,
      x.y * rhs.z.x + y.y * rhs.z.y + z.y * rhs.z.z,
      x.z * rhs.z.x + y.z * rhs.z.y + z.z * rhs.z.z,
    ),
  )

  def det: Float = x.x * (y.y * z.z - z.y * y.z) -
    x.y * (y.x * z.z - y.z * z.x) +
    x.z * (y.x * z.y - y.y * z.x)

  def inverse: Basis3D = {
    val s = 1f / det

    Basis3D(
      Vector3D(
        (y.y * z.z - z.y * y.z) * s,
        (x.z * z.y - x.y * z.z) * s,
        (x.y * y.z - x.z * y.y) * s,
      ),
      Vector3D(
        (y.z * z.x - y.x * z.z) * s,
        (x.x * z.z - x.z * z.x) * s,
        (y.x * x.z - x.x * y.z) * s,
      ),
      Vector3D(
        (y.x * z.y - z.x * y.y) * s,
        (z.x * x.y - x.x * z.y) * s,
        (x.x * y.y - y.x * x.y) * s,
      ),
    )
  }

  def up: Vector3D = this * Vector3D.Up

  def down: Vector3D = this * Vector3D.Down

  def left: Vector3D = this * Vector3D.Left

  def right: Vector3D = this * Vector3D.Right

  def forward: Vector3D = this * Vector3D.Forward

  def back: Vector3D = this * Vector3D.Back

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