# TODO: make own vmath module with proper quats and more matrix types

import vmath except Quat, quat

type RotationOrder* = enum
    Quaternion
    EulerXYZ
    EulerXZY
    EulerYXZ
    EulerYZX
    EulerZXY
    EulerZYX
    AxisAngle


type
    GQuat*[T] = object
        x*, y*, z*, w*: float32
    Quat* = GQuat[float32]
    DQuat* = GQuat[float64]

proc gquat*[T](x,y,z,w: T): GQuat[T] {.inline.} =
    GQuat[T](x:x, y:y, z:z, w:w)

proc quat*(x,y,z,w: float32): Quat {.inline.} =
    Quat(x:x, y:y, z:z, w:w)

proc dquat*(x,y,z,w: float64): DQuat {.inline.} =
    DQuat(x:x, y:y, z:z, w:w)

template quat*(): Quat = quat(0,0,0,1)
template dquat*(): DQuat = dquat(0,0,0,1)
template quat*(v: Vec4): Quat = cast[Quat](v)
template dquat*(v: DVec4): DQuat = cast[DQuat](v)

template `[]`*[T](q: GQuat[T], i: int): T = cast[array[4, T]](q)[i]
template `[]=`*[T](q: var GQuat[T], i: int, v: T) =
    case i:
        of 0: q.x = v
        of 1: q.y = v
        of 2: q.z = v
        of 3: q.w = v
        else: raise RangeDefect.newException "Index out of range"

# https://stackoverflow.com/questions/28673777/convert-quaternion-from-right-handed-to-left-handed-coordinate-system
# S=s, B=−b, C=−d and D=−c.
func swap_quat_handness*[T](q: GQuat[T]): GQuat[T] =
    # this swaps from right handed Z-up to left handed Y-up
    # and vice versa
    # TODO: rotate 90 to not change up-ness?
    GQuat[T](x: -q.x, y: -q.z, z: -q.y, w: q.w)

func to_quat*[T](m: GMat3[T]|GMat4[T]): GQuat[T] =
    # http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
    let trace = m[0,0] + m[1,1] + m[2,2]
    if trace > 0:
        let s = 0.5 / sqrt(trace + 1.0)
        result.w = 0.25 / s
        result.x = (m[1,2] - m[2,1]) * s
        result.y = (m[2,0] - m[0,2]) * s
        result.z = (m[0,1] - m[1,0]) * s
    elif (m[0,0] > m[1,1]) and (m[0,0] > m[2,2]):
        let s = 2.0 * sqrt(1.0 + m[0,0] - m[1,1] - m[2,2])
        result.w = (m[1,2] - m[2,1]) / s
        result.x = 0.25 * s
        result.y = (m[1,0] + m[0,1]) / s
        result.z = (m[2,0] + m[0,2]) / s
    elif m[1,1] > m[2,2]:
        let s = 2.0 * sqrt(1.0 + m[1,1] - m[0,0] - m[2,2])
        result.w = (m[2,0] - m[0,2]) / s
        result.x = (m[1,0] + m[0,1]) / s
        result.y = 0.25 * s
        result.z = (m[2,1] + m[1,2]) / s
    else:
        let s = 2.0 * sqrt(1.0 + m[2,2] - m[0,0] - m[1,1])
        result.w = (m[0,1] - m[1,0]) / s
        result.x = (m[2,0] + m[0,2]) / s
        result.y = (m[2,1] + m[1,2]) / s
        result.z = 0.25 * s

func inverse*[T](a: GQuat[T]): GQuat[T] =
    let dot = a.x * a.x + a.y * a.y + a.z * a.z + a.w * a.w
    if dot == 0:
        return
    return gquat[T](-a.x, -a.y, -a.z, a.w) * (1/dot)

proc normalize*[T](q: GQuat[T]): GQuat[T] =
    let length = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w)
    if length == 0:
        return q
    return gquat[T](q.x / length , q.y / length, q.z / length, q.w / length)

func to_mat[T,U](q: GQuat[T]): U =
    let xx = q.x * q.x * 2
    let yx = q.y * q.x * 2
    let yy = q.y * q.y * 2
    let zx = q.z * q.x * 2
    let zy = q.z * q.y * 2
    let zz = q.z * q.z * 2
    let wx = q.w * q.x * 2
    let wy = q.w * q.y * 2
    let wz = q.w * q.z * 2

    result[0,0] = 1 - yy - zz
    result[1,0] = yx - wz
    result[2,0] = zx + wy

    result[0,1] = yx + wz
    result[1,1] = 1 - xx - zz
    result[2,1] = zy - wx

    result[0,2] = zx - wy
    result[1,2] = zy + wx
    result[2,2] = 1 - xx - yy
    when U is GMat4[T]:
        result[3,3] = 1

template to_mat3*[T](q: GQuat[T]): GMat3[T] =
    to_mat[T,GMat3[T]](q)

template to_mat4*[T](q: GQuat[T]): GMat4[T] =
    to_mat[T,GMat4[T]](q)


func `*`*[T](q: GQuat[T], a: GVec3[T]): GVec3[T] =
  # calculate quat * vec
  let ix = q.w * a.x + q.y * a.z - q.z * a.y
  let iy = q.w * a.y + q.z * a.x - q.x * a.z
  let iz = q.w * a.z + q.x * a.y - q.y * a.x
  let iw = -q.x * a.x - q.y * a.y - q.z * a.z

  # calculate result * inverse quat
  result.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y
  result.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z
  result.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x

proc `*`*[T](a, b: GQuat[T]): GQuat[T] =
    GQuat[T](
        x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
        y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
        z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
        w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
    )

proc `*`*[T](q: GQuat[T], s: T): GQuat[T] =
    GQuat[T](x: q.x*s, y: q.y*s, z: q.z*s, w: q.w*s)

template to_tuple*[T](v: GQuat[T]): (T, T, T, T) =
    # (v.x, v.y, v.z, v.w)
    cast[(T,T,T,T)](v)


# http://stackoverflow.com/questions/1031005/is-there-an-algorithm-for-converting-quaternion-rotations-to-euler-angle-rotatio

proc threeaxisrot[T](r11, r12, r21, r31, r32: T): GVec3[T] =
    gvec3[T](arctan2(r31, r32), arcsin (r21), arctan2(r11, r12))

# NOTE: It uses Blender's convention for euler rotations:
# XYZ means that to convert back to quat you must rotate Z, then Y, then X

proc to_euler_XYZ*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(2*(q.x*q.y + q.w*q.z),
                q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                -2*(q.x*q.z - q.w*q.y),
                2*(q.y*q.z + q.w*q.x),
                q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z)

proc to_euler_XZY*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(-2*(q.x*q.z - q.w*q.y),
                q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                2*(q.x*q.y + q.w*q.z),
                -2*(q.y*q.z - q.w*q.x),
                q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z).xzy

proc to_euler_YXZ*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(-2*(q.x*q.y - q.w*q.z),
                q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                2*(q.y*q.z + q.w*q.x),
                -2*(q.x*q.z - q.w*q.y),
                q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z).yxz

proc to_euler_YZX*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(2*(q.y*q.z + q.w*q.x),
                q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                -2*(q.x*q.y - q.w*q.z),
                2*(q.x*q.z + q.w*q.y),
                q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z).yzx

proc to_euler_ZXY*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(2*(q.x*q.z + q.w*q.y),
                q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                -2*(q.y*q.z - q.w*q.x),
                2*(q.x*q.y + q.w*q.z),
                q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z).zxy

proc to_euler_ZYX*[T](q: GQuat[T]): GVec3[T] =
    threeaxisrot(-2*(q.y*q.z - q.w*q.x),
                q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                2*(q.x*q.z + q.w*q.y),
                -2*(q.x*q.y - q.w*q.z),
                q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z).zyx

proc rotateX*[T](q: GQuat[T], rad: T): GQuat[T] =
    let rad = rad * 0.5
    let x = sin(rad)
    let w = cos(rad)
    result.x = q.x * w + q.w * x
    result.y = q.y * w + q.z * x
    result.z = q.z * w - q.y * x
    result.w = q.w * w - q.x * x

proc rotateY*[T](q: GQuat[T], rad: T): GQuat[T] =
    let rad = rad * 0.5
    let y = sin(rad)
    let w = cos(rad)
    result.x = q.x * w - q.z * y
    result.y = q.y * w + q.w * y
    result.z = q.z * w + q.x * y
    result.w = q.w * w - q.y * y

proc rotateZ*[T](q: GQuat[T], rad: T): GQuat[T] =
    let rad = rad * 0.5
    let z = sin(rad)
    let w = cos(rad)
    result.x = q.x * w + q.y * z
    result.y = q.y * w - q.x * z
    result.z = q.z * w + q.w * z
    result.w = q.w * w - q.z * z


proc to_quat*[T](euler: GVec3[T], order: RotationOrder): GQuat[T] =
    var q = quat()
    case order:
        of Quaternion:
            raise newException(ValueError, "invalid euler order")
        of EulerXYZ:
            q = q.rotateZ euler.z
            q = q.rotateY euler.y
            q = q.rotateX euler.x
        of EulerXZY:
            q = q.rotateY euler.y
            q = q.rotateZ euler.z
            q = q.rotateX euler.x
        of EulerYXZ:
            q = q.rotateZ euler.z
            q = q.rotateX euler.x
            q = q.rotateY euler.y
        of EulerYZX:
            q = q.rotateX euler.x
            q = q.rotateZ euler.z
            q = q.rotateY euler.y
        of EulerZXY:
            q = q.rotateY euler.y
            q = q.rotateX euler.x
            q = q.rotateZ euler.z
        of EulerZYX:
            q = q.rotateX euler.x
            q = q.rotateY euler.y
            q = q.rotateZ euler.z
        of AxisAngle:
            raise newException(ValueError, "invalid euler order")
    return q

proc rotationTo*(a,b: Vec3): Quat =
    let dot = dot(a, b)
    if dot < -0.999999:
        var v = cross(vec3(1,0,0), a)
        if v.length < 0.000001:
            v = cross(vec3(0,1,0), a)
        return cast[Quat](fromAxisAngle(v.normalize, PI))
    elif dot > 0.999999:
        return quat(0,0,0,1)
    else:
        let v = cross(a, b)
        return quat(v.x, v.y, v.z, 1.0+dot).normalize