# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020

struct Vector3
    x::Float64
    y::Float64
    z::Float64
    function Vector3()
        x=0.0
        y=0.0
        z=0.0
        new(x,y,z)
    end
    function Vector3(x,y,z)
       new(x,y,z)
    end
end

struct Quaternion
    x::Float64
    y::Float64
    z::Float64
    w::Float64
    function Quaternion()
        x=0.0
        y=0.0
        z=0.0
        w=1.0
        new(x,y,z,w)
    end
    function Quaternion(x,y,z,w)
        new(x,y,z,w)
    end
end

struct RotationMatrix
    te1::Float64
    te2::Float64
    te3::Float64
    te4::Float64
    te5::Float64
    te6::Float64
    te7::Float64
    te8::Float64
    te9::Float64
    te10::Float64
    te11::Float64
    te12::Float64
    te13::Float64
    te14::Float64
    te15::Float64
    te16::Float64
    function RotationMatrix()
        te1 =0.0
        te2 =0.0
        te3 =0.0
        te4 =0.0
        te5 =0.0
        te6 =0.0
        te7 =0.0
        te8 =0.0
        te9 =0.0
        te10=0.0
        te11=0.0
        te12=0.0
        te13=0.0
        te14=0.0
        te15=0.0
        te16=0.0
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
    function RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
end

+(f::Vector3, g::Vector3)=Vector3(f.x+g.x , f.y+g.y,f.z+g.z )
-(f::Vector3, g::Vector3)=Vector3(f.x-g.x , f.y-g.y,f.z-g.z )
*(f::Vector3, g::Vector3)=Vector3(f.x*g.x , f.y*g.y,f.z*g.z )

+(f::Vector3, g::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(f::Vector3, g::Number)=Vector3(f.x-g , f.y-g,f.z-g )
*(f::Vector3, g::Number)=Vector3(f.x*g , f.y*g,f.z*g )

+(g::Vector3, f::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(g::Vector3, f::Number)=Vector3(g-f.x , g-f.y,g-f.z )
*(g::Vector3, f::Number)=Vector3(f.x*g , f.y*g,f.z*g )

addX(f::Vector3, g::Number)=Vector3(f.x+g , f.y,f.z)
addY(f::Vector3, g::Number)=Vector3(f.x , f.y+g,f.z )
addZ(f::Vector3, g::Number)=Vector3(f.x , f.y,f.z+g )

function normalizeVector3(f::Vector3)
    leng=lengthVector3(f)
    if(leng>0.0)
        return Vector3(f.x/leng,f.y/leng,f.z/leng)
    else
        return Vector3(0.0,0.0,0.0)
    end
    
end

function lengthVector3(f::Vector3)
    leng=sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z))
    return convert(Float64,leng)
    
end

function normalizeQuaternion(f::Quaternion)
    l = sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z)+ (f.w * f.w))
    if l === 0 
        qx = 0
        qy = 0
        qz = 0
        qw = 1
    else 
        l = 1 / l
        qx = f.x * l
        qy = f.y * l
        qz = f.z * l
        qw = f.w * l
    end
    return Quaternion(qx,qy,qz,qw)
end

function normalizeQuaternion1!(fx::Float64,fy::Float64,fz::Float64,fw::Float64)
    l = sqrt((fx * fx) + (fy * fy) + (fz * fz)+ (fw * fw))
    if l === 0 
        qx = 0.0
        qy = 0.0
        qz = 0.0
        qw = 1.0
    else 
        l = 1.0 / l
        qx = fx * l
        qy = fy * l
        qz = fz * l
        qw = fw * l
    end
    return qx,qy,qz,qw
end

function dotVector3(f::Vector3, g::Vector3)
    return (f.x * g.x) + (f.y * g.y) + (f.z * g.z)
end

function Base.show(io::IO, v::Vector3)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z)")
end

function Base.show(io::IO, v::Quaternion)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z), w:$(v.z)")
end

Base.Broadcast.broadcastable(q::Vector3) = Ref(q)


######################utils##################################
function toAxisXVector3(pV::Vector3,axis::Vector3) #TODO CHANGE

    xaxis=Vector3(1.0,0.0,0.0)
    vector=normalizeVector3(axis)
    q=setFromUnitVectors(vector,xaxis)
    
    v=applyQuaternion1( pV ,q )
    
    return Vector3(v.x,v.y,v.z)
end

function toAxisOriginalVector3(pV::Vector3,axis::Vector3)
    
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)

    q=setFromUnitVectors(xaxis,vector)

    v=applyQuaternion1( pV ,q )
    
    return Vector3(v.x,v.y,v.z)
end

function  toAxisXQuat(pQ::Quaternion,axis::Vector3)
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)

    q=setFromUnitVectors(vector,xaxis)
        
    pV=Vector3(pQ.x,pQ.y,pQ.z)
    
    v=applyQuaternion1( pV ,q )

    return Quaternion(v.x,v.y,v.z,pQ.w)
    
end

function toAxisOriginalQuat(pQ::Vector3,axis::Vector3)
    xaxis=Vector3(1.0,0.0,0.0)

    vector=normalizeVector3(axis)
    
    q=setFromUnitVectors(xaxis,vector)
    
    pV=Vector3(pQ.x,pQ.y,pQ.z)
    v=applyQuaternion1( pV ,q )
    
    return Quaternion(v.x,v.y,v.z,1.0)
    
end

function roundd(num,prec)
    num  =   convert(Float64,num*prec)
    num  =   floor(num+0.5)
    num  =   convert(Float64,num)
    num  =   num/prec
    return num
    
end
###########################################

function setFromUnitVectors(vFrom::Vector3, vTo::Vector3)
    # assumes direction vectors vFrom and vTo are normalized
    EPS = 0.000000001;
    r= dotVector3(vFrom,vTo)+1.0
    # r =  dot(vFrom,vTo)+1

    if r < EPS
        r = 0;
        if abs( vFrom.x ) > abs( vFrom.z ) 
            qx = - vFrom.y
            qy = vFrom.x
            qz = 0.0
            qw = r
        else 
            qx = 0.0
            qy = -(vFrom.z)
            qz = vFrom.y
            qw = r
        end
   else 
        # crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3
        qx = vFrom.y * vTo.z - vFrom.z * vTo.y
        qy = vFrom.z * vTo.x - vFrom.x * vTo.z
        qz = vFrom.x * vTo.y - vFrom.y * vTo.x
        qw = r

    end
    # qx= (qx==-0.0) ? 0.0 : qx #just commented out 5 august 20202
    # qy= (qy==-0.0) ? 0.0 : qy #just commented out 5 august 20202
    # qz= (qz==-0.0) ? 0.0 : qz #just commented out 5 august 20202
    # qw= (qw==-0.0) ? 0.0 : qw #just commented out 5 august 20202
        
    
    mx=qx*qx
    my=qy*qy
    mz=qz*qz
    mw=qw*qw
    mm=mx+my
    mm=mm+mz
    mm=mm+mw
    mm=convert(Float64,mm)#??????????????????? todo check later
    
    l=CUDA.sqrt(mm)
    
    #l = sqrt((qx * qx) + (qy * qy) + (qz * qz)+ (qw * qw))
    if l === 0 
        qx = 0.0
        qy = 0.0
        qz = 0.0
        qw = 1.0
    else 
        l = 1.0 / l
        qx = qx * l
        qy = qy * l
        qz = qz * l
        qw = qw * l
    end
    
    return Quaternion(qx,qy,qz,qw)
    
end

function quatToMatrix( quaternion::Quaternion)

    #te = RotationMatrix()
    
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w
    
    x2 = x + x
    y2 = y + y
    z2 = z + z
    xx = x * x2
    xy = x * y2
    xz = x * z2
    yy = y * y2
    yz = y * z2
    zz = z * z2
    wx = w * x2
    wy = w * y2
    wz = w * z2

    sx = 1.0
    sy = 1.0
    sz = 1.0

    te1 = ( 1.0 - ( yy + zz ) ) * sx
    te2 = ( xy + wz ) * sx
    te3 = ( xz - wy ) * sx
    te4 = 0.0

    te5 = ( xy - wz ) * sy
    te6 = ( 1.0 - ( xx + zz ) ) * sy
    te7 = ( yz + wx ) * sy
    te8 = 0.0

    te9 = ( xz + wy ) * sz
    te10 = ( yz - wx ) * sz
    te11 = ( 1.0 - ( xx + yy ) ) * sz
    te12 = 0.0

    te13 = 0.0 #position.x;
    te14 = 0.0 #position.y;
    te15 = 0.0 #position.z;
    te16 = 1.0
    
    te= RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)

    return te

end

function  setFromRotationMatrix(m::RotationMatrix)
    m11 = convert(Float64,m.te1 )
    m12 = convert(Float64,m.te5 )
    m13 = convert(Float64,m.te9 )
    m21 = convert(Float64,m.te2 )
    m22 = convert(Float64,m.te6 )
    m23 = convert(Float64,m.te10)
    m31 = convert(Float64,m.te3 )
    m32 = convert(Float64,m.te7 )
    m33 = convert(Float64,m.te11)
    

    y = CUDA.asin( clamp( m13, -1.0, 1.0 ) ) ##check if has to be changed to cuda

    if ( abs( m13 ) < 0.9999999999 ) 
        
        x = CUDA.atan2( - m23, m33 )
        z = CUDA.atan2( - m12, m11 )#-m12, m11
    else

        x = CUDA.atan2( m32, m22 )
        z = 0.0;

    end
    
    return Vector3(x,y,z)
    
end

function setQuaternionFromEuler(euler::Vector3)
    x=euler.x
    y=euler.y
    z=euler.z
    
    
    c1 = CUDA.cos( x / 2.0 )
    c2 = CUDA.cos( y / 2.0 )
    c3 = CUDA.cos( z / 2.0 )

    s1 = CUDA.sin( x / 2.0 )
    s2 = CUDA.sin( y / 2.0 )
    s3 = CUDA.sin( z / 2.0 )
    
   
    x = s1 * c2 * c3 + c1 * s2 * s3
    y = c1 * s2 * c3 - s1 * c2 * s3
    z = c1 * c2 * s3 + s1 * s2 * c3
    w = c1 * c2 * c3 - s1 * s2 * s3
        
    return Quaternion(x,y,z,w)
end

function applyQuaternion1(e::Vector3,q2::Quaternion)
    x = e.x
    y = e.y
    z = e.z

    qx = q2.x
    qy = q2.y
    qz = q2.z
    qw = q2.w

    # calculate quat * vector

    ix = qw * x + qy * z - qz * y
    iy = qw * y + qz * x - qx * z
    iz = qw * z + qx * y - qy * x
    iw = - qx * x - qy * y - qz * z

    # calculate result * inverse quat

    xx = ix * qw + iw * - qx + iy * - qz - iz * - qy
    yy = iy * qw + iw * - qy + iz * - qx - ix * - qz
    zz = iz * qw + iw * - qz + ix * - qy - iy * - qx
    
    d=15

    return Vector3(xx,yy,zz)
end
##########################################
function conjugate(q::Quaternion)
    x= (-q.x==-0) ? 0.0 : -q.x
    y= (-q.y==-0) ? 0.0 : -q.y
    z= (-q.z==-0) ? 0.0 : -q.z
    w=q.w
    x=-q.x
    y=-q.y
    z=-q.z
    x=convert(Float64,x)
    y=convert(Float64,y)
    z=convert(Float64,z)
    w=convert(Float64,w)
    return Quaternion(x,y,z,w)
end

function RotateVec3D(a::Quaternion, f::Vector3)   
    fx= (f.x==-0) ? 0 : f.x
    fy= (f.y==-0) ? 0 : f.y
    fz= (f.z==-0) ? 0 : f.z
    # fx= f.x
    # fy= f.y
    # fz= f.z
    tw = fx*a.x + fy*a.y + fz*a.z
    tx = fx*a.w - fy*a.z + fz*a.y
    ty = fx*a.z + fy*a.w - fz*a.x
    tz = -fx*a.y + fy*a.x + fz*a.w

    return Vector3((a.w*tx+a.x*tw+a.y*tz-a.z*ty),(a.w*ty-a.x*tz+a.y*tw+a.z*tx),(a.w*tz+a.x*ty-a.y*tx+a.z*tw))
end
#!< Returns a vector representing the specified vector "f" rotated by this quaternion. @param[in] f The vector to transform.

function RotateVec3DInv(a::Quaternion, f::Vector3)  
    fx=f.x
    fy=f.y
    fz=f.z
    tw = a.x*fx + a.y*fy + a.z*fz
    tx = a.w*fx - a.y*fz + a.z*fy
    ty = a.w*fy + a.x*fz - a.z*fx
    tz = a.w*fz - a.x*fy + a.y*fx
    return Vector3((tw*a.x + tx*a.w + ty*a.z - tz*a.y),(tw*a.y - tx*a.z + ty*a.w + tz*a.x),(tw*a.z + tx*a.y - ty*a.x + tz*a.w))
end
#!< Returns a vector representing the specified vector "f" rotated by the inverse of this quaternion. This is the opposite of RotateVec3D. @param[in] f The vector to transform.

function ToRotationVector(a::Quaternion) 
    x=convert(Float64,a.x)
    y=convert(Float64,a.y)
    z=convert(Float64,a.z)
    w=convert(Float64,a.w)
    if (w >= 1.0 || w <= -1.0) 
        return Vector3(0.0,0.0,0.0)
    end
    squareLength = 1.0-w*w; # because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when 
    SLTHRESH_ACOS2SQRT= 2.4e-3; # SquareLength threshhold for when we can use square root optimization for acos. From SquareLength = 1-w*w. (calculate according to 1.0-W_THRESH_ACOS2SQRT*W_THRESH_ACOS2SQRT

    if (squareLength < SLTHRESH_ACOS2SQRT) # ???????
        # if (squareLength==0.0)
        #     w=1.0-1e-12
        #     squareLength = 1.0-w*w
        # end
        xx=x*(2.0*CUDA.sqrt((2.0-2.0*w)/squareLength))
        yy=y*(2.0*CUDA.sqrt((2.0-2.0*w)/squareLength))
        zz=z*(2.0*CUDA.sqrt((2.0-2.0*w)/squareLength))
        xx=convert(Float64,xx)
        yy=convert(Float64,yy)
        zz=convert(Float64,zz)
 
        return Vector3(xx,yy,zz) ; # acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6
    else 
        xx=x*(2.0*CUDA.acos(w)/CUDA.sqrt(squareLength))
        yy=y*(2.0*CUDA.acos(w)/CUDA.sqrt(squareLength))
        zz=z*(2.0*CUDA.acos(w)/CUDA.sqrt(squareLength))
        xx=convert(Float64,xx)
        yy=convert(Float64,yy)
        zz=convert(Float64,zz)

        return Vector3(xx,yy,zz)
    end
    
    # if (a.w >= 1.0 || a.w <= -1.0) 
    #     return Vector3(0.0,0.0,0.0)
    # end
    # squareLength = 1.0-a.w*a.w; #because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when 
    # if (squareLength < SLTHRESH_ACOS2SQRT) 
    #     return Vec3D<T>(x, y, z)*2.0*sqrt((2-2*a.w)/squareLength); #acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6
    # else 
    #     return Vec3D<T>(x, y, z)*2.0*acos(a.w)/sqrt(squareLength);
    # end
            
end 
# !< Returns a rotation vector representing this quaternion rotation. Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/

function FromRotationVector(VecIn::Vector3)
    theta=VecIn*Vector3(0.5,0.5,0.5)
    ntheta=CUDA.sqrt((theta.x * theta.x) + (theta.y * theta.y) + (theta.z * theta.z))
    thetaMag2=ntheta*ntheta
    
    DBL_EPSILONx24 =5.328e-15
    if thetaMag2*thetaMag2 < DBL_EPSILONx24
        qw=1.0 - 0.5*thetaMag2
		s=1.0 - thetaMag2 / 6.0
    else
        thetaMag = CUDA.sqrt(thetaMag2)
		qw=CUDA.cos(thetaMag)
		s=CUDA.sin(thetaMag) / thetaMag
    end
    qx=theta.x*s
    qy=theta.y*s
    qz=theta.z*s
    
    qx=convert(Float64,qx)
    qy=convert(Float64,qy)
    qz=convert(Float64,qz)
    qw=convert(Float64,qw)
    
    return Quaternion(qx,qy,qz,qw)
end

function multiplyQuaternions(q::Quaternion,f::Quaternion)
    x=convert(Float64,q.x)
    y=convert(Float64,q.y)
    z=convert(Float64,q.z)
    w=convert(Float64,q.w)
    x1=convert(Float64,w*convert(Float64,f.x) + x*convert(Float64,f.w) + y*convert(Float64,f.z) - z*convert(Float64,f.y))
    y1=convert(Float64,w*convert(Float64,f.y) - x*convert(Float64,f.z) + y*convert(Float64,f.w) + z*convert(Float64,f.x))
    z1=convert(Float64,w*convert(Float64,f.z) + x*convert(Float64,f.y) - y*convert(Float64,f.x) + z*convert(Float64,f.w))
    w1=convert(Float64,w*convert(Float64,f.w) - x*convert(Float64,f.x) - y*convert(Float64,f.y) - z*convert(Float64,f.z))
    # w1=convert(Float64,1.0*convert(Float64,f.w) - 1.0*convert(Float64,f.x) + 1.0*convert(Float64,f.y) - 1.0*convert(Float64,f.z)) #todo wrong fix
	return Quaternion(x1,y1,z1,w1 ); #!< overload quaternion multiplication.
end
#####################################################
function FromAngleToPosX(angle::Quaternion,RotateFrom::Vector3) #highly optimized at the expense of readability
    if (RotateFrom.x==0.0 && RotateFrom.y==0.0 && RotateFrom.z==0.0) 
        return Quaternion(angle.x,angle.y,angle.z,angle.w ) #leave off if it slows down too much!!
    end

    #Catch and handle small angle:
    YoverX = RotateFrom.y/RotateFrom.x;
    ZoverX = RotateFrom.z/RotateFrom.x;
    SMALL_ANGLE_RAD= 1.732e-2 #/Angles less than this get small angle approximations. To get: Root solve atan(t)/t-1+MAX_ERROR_PERCENT. From: MAX_ERROR_PERCENT = (t-atan(t))/t 
    SMALL_ANGLE_W= 0.9999625 #//quaternion W value corresponding to a SMALL_ANGLE_RAD. To calculate, cos(SMALL_ANGLE_RAD*0.5).
    PI=3.14159265358979
    DISCARD_ANGLE_RAD=1e-7#define DISCARD_ANGLE_RAD 1e-7 //Anything less than this angle can be considered 0


    if (YoverX<SMALL_ANGLE_RAD && YoverX>-SMALL_ANGLE_RAD && ZoverX<SMALL_ANGLE_RAD && ZoverX>-SMALL_ANGLE_RAD)#Intercept small angle and zero angle
        x=0.0
        y=0.5*ZoverX
        z=-0.5*YoverX
        w = 1.0+0.5*(-y*y-z*z) #w=sqrt(1-x*x-y*y), small angle sqrt(1+x) ~= 1+x/2 at x near zero.
        return Quaternion(x,y,z,w )
    end

    #more accurate non-small angle:
    RotFromNorm = Vector3(RotateFrom.x,RotateFrom.y,RotateFrom.z);

    RotFromNorm=normalizeVector3(RotFromNorm); #Normalize the input...

    theta = CUDA.acos(RotFromNorm.x); #because RotFromNorm is normalized, 1,0,0 is normalized, and A.B = |A||B|cos(theta) = cos(theta)
    if (theta > PI-DISCARD_ANGLE_RAD) 
        w=0.0
        x=0.0
        y=1.0
        z=0.0;
        return Quaternion(x,y,z,w )
    end #180 degree rotation (about y axis, since the vector must be pointing in -x direction

    AxisMagInv = 1.0/CUDA.sqrt(RotFromNorm.z*RotFromNorm.z+RotFromNorm.y*RotFromNorm.y);
    #Here theta is the angle, axis is RotFromNorm.Cross(Vec3D(1,0,0)) = Vec3D(0, RotFromNorm.z/AxisMagInv, -RotFromNorm.y/AxisMagInv), which is still normalized. (super rolled together)
    a = 0.5*theta;
    s = CUDA.sin(a);
    w=CUDA.cos(a); 
    x=0; 
    y=RotFromNorm.z*AxisMagInv*s; 
    z = -RotFromNorm.y*AxisMagInv*s; #angle axis function, reduced
    return Quaternion(x,y,z,w )
end
#  //!< Overwrites this quaternion with the calculated rotation that would transform the specified RotateFrom vector to point in the positve X direction. Note: function changes this quaternion.  @param[in] RotateFrom An arbitrary direction vector. Does not need to be normalized.