//
//  Test_quat_aos_neon.cpp
//  BulletTest
//
//  Copyright (c) 2011 Apple Inc., Inc.
//


#include "LinearMath/btScalar.h"
#if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)

#include "Test_quat_aos_neon.h"
#include "vector.h"
#include "Utils.h"
#include "main.h"

#include <vectormath/vmInclude.h>


//typedef Vectormath::Aos::Vector3    vmVector3;
//typedef Vectormath::Aos::Quat       vmQuat;
//typedef Vectormath::Aos::Matrix3    vmMatrix3;
//typedef Vectormath::Aos::Transform3 vmTransform3;
//typedef Vectormath::Aos::Point3     vmPoint3;


typedef Vectormath::Aos::Vector4    vmVector4;

// reference code for testing purposes
ATTRIBUTE_ALIGNED16(class) Quat_ref
{
    float mX;
    float mY;
    float mZ;
    float mW;
    
public:
    // Default constructor; does no initialization
    // 
    inline Quat_ref( ) { };
    
    // Copy a quaternion
    // 
    inline Quat_ref( const Quat_ref & quat );
    
    // Construct a quaternion from x, y, z, and w elements
    // 
    inline Quat_ref( float x, float y, float z, float w );
    
    // Construct a quaternion from a 3-D vector and a scalar
    // 
    inline Quat_ref( const vmVector3 & xyz, float w );
    
    // Copy elements from a 4-D vector into a quaternion
    // 
    explicit inline Quat_ref( const vmVector4 & vec );
    
    // Convert a rotation matrix to a unit-length quaternion
    // 
    explicit inline Quat_ref( const vmMatrix3 & rotMat );
    
    // Set all elements of a quaternion to the same scalar value
    // 
    explicit inline Quat_ref( float scalar );
    
    // Assign one quaternion to another
    // 
    inline Quat_ref & operator =( const Quat_ref & quat );
    
    // Set the x, y, and z elements of a quaternion
    // NOTE: 
    // This function does not change the w element.
    // 
    inline Quat_ref & setXYZ( const vmVector3 & vec );
    
    // Get the x, y, and z elements of a quaternion
    // 
    inline const vmVector3 getXYZ( ) const;
    
    // Set the x element of a quaternion
    // 
    inline Quat_ref & setX( float x );
    
    // Set the y element of a quaternion
    // 
    inline Quat_ref & setY( float y );
    
    // Set the z element of a quaternion
    // 
    inline Quat_ref & setZ( float z );
    
    // Set the w element of a quaternion
    // 
    inline Quat_ref & setW( float w );
    
    // Get the x element of a quaternion
    // 
    inline float getX( ) const;
    
    // Get the y element of a quaternion
    // 
    inline float getY( ) const;
    
    // Get the z element of a quaternion
    // 
    inline float getZ( ) const;
    
    // Get the w element of a quaternion
    // 
    inline float getW( ) const;
    
    // Set an x, y, z, or w element of a quaternion by index
    // 
    inline Quat_ref & setElem( int idx, float value );
    
    // Get an x, y, z, or w element of a quaternion by index
    // 
    inline float getElem( int idx ) const;
    
    // Subscripting operator to set or get an element
    // 
    inline float & operator []( int idx );
    
    // Subscripting operator to get an element
    // 
    inline float operator []( int idx ) const;
    
    // Add two quaternions
    // 
    inline const Quat_ref operator +( const Quat_ref & quat ) const;
    
    // Subtract a quaternion from another quaternion
    // 
    inline const Quat_ref operator -( const Quat_ref & quat ) const;
    
    // Multiply two quaternions
    // 
    inline const Quat_ref operator *( const Quat_ref & quat ) const;
    
    // Multiply a quaternion by a scalar
    // 
    inline const Quat_ref operator *( float scalar ) const;
    
    // Divide a quaternion by a scalar
    // 
    inline const Quat_ref operator /( float scalar ) const;
    
    // Perform compound assignment and addition with a quaternion
    // 
    inline Quat_ref & operator +=( const Quat_ref & quat );
    
    // Perform compound assignment and subtraction by a quaternion
    // 
    inline Quat_ref & operator -=( const Quat_ref & quat );
    
    // Perform compound assignment and multiplication by a quaternion
    // 
    inline Quat_ref & operator *=( const Quat_ref & quat );
    
    // Perform compound assignment and multiplication by a scalar
    // 
    inline Quat_ref & operator *=( float scalar );
    
    // Perform compound assignment and division by a scalar
    // 
    inline Quat_ref & operator /=( float scalar );
    
    // Negate all elements of a quaternion
    // 
    inline const Quat_ref operator -( ) const;
    
    // Construct an identity quaternion
    // 
    static inline const Quat_ref identity( );
    
    // Construct a quaternion to rotate between two unit-length 3-D vectors
    // NOTE: 
    // The result is unpredictable if unitVec0 and unitVec1 point in opposite directions.
    // 
    static inline const Quat_ref rotation( const vmVector3 & unitVec0, const vmVector3 & unitVec1 );
    
    // Construct a quaternion to rotate around a unit-length 3-D vector
    // 
    static inline const Quat_ref rotation( float radians, const vmVector3 & unitVec );
    
    // Construct a quaternion to rotate around the x axis
    // 
    static inline const Quat_ref rotationX( float radians );
    
    // Construct a quaternion to rotate around the y axis
    // 
    static inline const Quat_ref rotationY( float radians );
    
    // Construct a quaternion to rotate around the z axis
    // 
    static inline const Quat_ref rotationZ( float radians );
    
};

inline Quat_ref::Quat_ref( const Quat_ref & quat )
{
    mX = quat.mX;
    mY = quat.mY;
    mZ = quat.mZ;
    mW = quat.mW;
}

inline Quat_ref::Quat_ref( float _x, float _y, float _z, float _w )
{
    mX = _x;
    mY = _y;
    mZ = _z;
    mW = _w;
}

inline Quat_ref::Quat_ref( const vmVector3 & xyz, float _w )
{
    this->setXYZ( xyz );
    this->setW( _w );
}

inline Quat_ref::Quat_ref( const vmVector4 & vec )
{
    mX = vec.getX();
    mY = vec.getY();
    mZ = vec.getZ();
    mW = vec.getW();
}

inline Quat_ref::Quat_ref( float scalar )
{
    mX = scalar;
    mY = scalar;
    mZ = scalar;
    mW = scalar;
}

inline const Quat_ref Quat_ref::identity( )
{
    return Quat_ref( 0.0f, 0.0f, 0.0f, 1.0f );
}


inline void loadXYZW_ref( Quat_ref & quat, const float * fptr )
{
    quat = Quat_ref( fptr[0], fptr[1], fptr[2], fptr[3] );
}

inline void storeXYZW_ref( const Quat_ref & quat, float * fptr )
{
    fptr[0] = quat.getX();
    fptr[1] = quat.getY();
    fptr[2] = quat.getZ();
    fptr[3] = quat.getW();
}

inline Quat_ref & Quat_ref::operator =( const Quat_ref & quat )
{
    mX = quat.mX;
    mY = quat.mY;
    mZ = quat.mZ;
    mW = quat.mW;
    return *this;
}

inline Quat_ref & Quat_ref::setXYZ( const vmVector3 & vec )
{
    mX = vec.getX();
    mY = vec.getY();
    mZ = vec.getZ();
    return *this;
}

inline const vmVector3 Quat_ref::getXYZ( ) const
{
    return vmVector3( mX, mY, mZ );
}

inline Quat_ref & Quat_ref::setX( float _x )
{
    mX = _x;
    return *this;
}

inline float Quat_ref::getX( ) const
{
    return mX;
}

inline Quat_ref & Quat_ref::setY( float _y )
{
    mY = _y;
    return *this;
}

inline float Quat_ref::getY( ) const
{
    return mY;
}

inline Quat_ref & Quat_ref::setZ( float _z )
{
    mZ = _z;
    return *this;
}

inline float Quat_ref::getZ( ) const
{
    return mZ;
}

inline Quat_ref & Quat_ref::setW( float _w )
{
    mW = _w;
    return *this;
}

inline float Quat_ref::getW( ) const
{
    return mW;
}

inline Quat_ref & Quat_ref::setElem( int idx, float value )
{
    *(&mX + idx) = value;
    return *this;
}

inline float Quat_ref::getElem( int idx ) const
{
    return *(&mX + idx);
}

inline float & Quat_ref::operator []( int idx )
{
    return *(&mX + idx);
}

inline float Quat_ref::operator []( int idx ) const
{
    return *(&mX + idx);
}

inline const Quat_ref Quat_ref::operator +( const Quat_ref & quat ) const
{
    return Quat_ref(
                ( mX + quat.mX ),
                ( mY + quat.mY ),
                ( mZ + quat.mZ ),
                ( mW + quat.mW )
                );
}

inline const Quat_ref Quat_ref::operator -( const Quat_ref & quat ) const
{
    return Quat_ref(
                ( mX - quat.mX ),
                ( mY - quat.mY ),
                ( mZ - quat.mZ ),
                ( mW - quat.mW )
                );
}

inline const Quat_ref Quat_ref::operator *( float scalar ) const
{
    return Quat_ref(
                ( mX * scalar ),
                ( mY * scalar ),
                ( mZ * scalar ),
                ( mW * scalar )
                );
}

inline Quat_ref & Quat_ref::operator +=( const Quat_ref & quat )
{
    *this = *this + quat;
    return *this;
}

inline Quat_ref & Quat_ref::operator -=( const Quat_ref & quat )
{
    *this = *this - quat;
    return *this;
}

inline Quat_ref & Quat_ref::operator *=( float scalar )
{
    *this = *this * scalar;
    return *this;
}

inline const Quat_ref Quat_ref::operator /( float scalar ) const
{
    return Quat_ref(
                ( mX / scalar ),
                ( mY / scalar ),
                ( mZ / scalar ),
                ( mW / scalar )
                );
}

inline Quat_ref & Quat_ref::operator /=( float scalar )
{
    *this = *this / scalar;
    return *this;
}

inline const Quat_ref Quat_ref::operator -( ) const
{
    return Quat_ref(
                -mX,
                -mY,
                -mZ,
                -mW
                );
}

inline const Quat_ref operator *( float scalar, const Quat_ref & quat )
{
    return quat * scalar;
}

inline float dot( const Quat_ref & quat0, const Quat_ref & quat1 )
{
    float result;
    result = ( quat0.getX() * quat1.getX() );
    result = ( result + ( quat0.getY() * quat1.getY() ) );
    result = ( result + ( quat0.getZ() * quat1.getZ() ) );
    result = ( result + ( quat0.getW() * quat1.getW() ) );
    return result;
}

inline const Quat_ref lerp( float t, const Quat_ref & quat0, const Quat_ref & quat1 )
{
    return ( quat0 + ( ( quat1 - quat0 ) * t ) );
}

inline const Quat_ref slerp( float t, const Quat_ref & unitQuat0, const Quat_ref & unitQuat1 )
{
    Quat_ref start;
    float recipSinAngle, scale0, scale1, cosAngle, angle;
    cosAngle = dot( unitQuat0, unitQuat1 );
    if ( cosAngle < 0.0f ) {
        cosAngle = -cosAngle;
        start = ( -unitQuat0 );
    } else {
        start = unitQuat0;
    }
    if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
        angle = acosf( cosAngle );
        recipSinAngle = ( 1.0f / sinf( angle ) );
        scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
        scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
    } else {
        scale0 = ( 1.0f - t );
        scale1 = t;
    }
    return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
}

inline const Quat_ref squad( float t, const Quat_ref & unitQuat0, const Quat_ref & unitQuat1, const Quat_ref & unitQuat2, const Quat_ref & unitQuat3 )
{
    Quat_ref tmp0, tmp1;
    tmp0 = slerp( t, unitQuat0, unitQuat3 );
    tmp1 = slerp( t, unitQuat1, unitQuat2 );
    return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
}

inline float norm( const Quat_ref & quat )
{
    float result;
    result = ( quat.getX() * quat.getX() );
    result = ( result + ( quat.getY() * quat.getY() ) );
    result = ( result + ( quat.getZ() * quat.getZ() ) );
    result = ( result + ( quat.getW() * quat.getW() ) );
    return result;
}

inline float length( const Quat_ref & quat )
{
    return ::sqrtf( norm( quat ) );
}

inline const Quat_ref normalize( const Quat_ref & quat )
{
    float lenSqr, lenInv;
    lenSqr = norm( quat );
    lenInv = ( 1.0f / sqrtf( lenSqr ) );
    return Quat_ref(
                ( quat.getX() * lenInv ),
                ( quat.getY() * lenInv ),
                ( quat.getZ() * lenInv ),
                ( quat.getW() * lenInv )
                );
}

inline const Quat_ref Quat_ref::rotation( const vmVector3 & unitVec0, const vmVector3 & unitVec1 )
{
    float cosHalfAngleX2, recipCosHalfAngleX2;
    cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f + dot( unitVec0, unitVec1 ) ) ) );
    recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
    return Quat_ref( ( cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
}

inline const Quat_ref Quat_ref::rotation( float radians, const vmVector3 & unitVec )
{
    float s, c, angle;
    angle = ( radians * 0.5f );
    s = sinf( angle );
    c = cosf( angle );
    return Quat_ref( ( unitVec * s ), c );
}

inline const Quat_ref Quat_ref::rotationX( float radians )
{
    float s, c, angle;
    angle = ( radians * 0.5f );
    s = sinf( angle );
    c = cosf( angle );
    return Quat_ref( s, 0.0f, 0.0f, c );
}

inline const Quat_ref Quat_ref::rotationY( float radians )
{
    float s, c, angle;
    angle = ( radians * 0.5f );
    s = sinf( angle );
    c = cosf( angle );
    return Quat_ref( 0.0f, s, 0.0f, c );
}

inline const Quat_ref Quat_ref::rotationZ( float radians )
{
    float s, c, angle;
    angle = ( radians * 0.5f );
    s = sinf( angle );
    c = cosf( angle );
    return Quat_ref( 0.0f, 0.0f, s, c );
}

inline const Quat_ref Quat_ref::operator *( const Quat_ref & quat ) const
{
    return Quat_ref(
                ( ( ( ( mW * quat.mX ) + ( mX * quat.mW ) ) + ( mY * quat.mZ ) ) - ( mZ * quat.mY ) ),
                ( ( ( ( mW * quat.mY ) + ( mY * quat.mW ) ) + ( mZ * quat.mX ) ) - ( mX * quat.mZ ) ),
                ( ( ( ( mW * quat.mZ ) + ( mZ * quat.mW ) ) + ( mX * quat.mY ) ) - ( mY * quat.mX ) ),
                ( ( ( ( mW * quat.mW ) - ( mX * quat.mX ) ) - ( mY * quat.mY ) ) - ( mZ * quat.mZ ) )
                );
}

inline Quat_ref & Quat_ref::operator *=( const Quat_ref & quat )
{
    *this = *this * quat;
    return *this;
}

inline const vmVector3 rotate( const Quat_ref & quat, const vmVector3 & vec )
{
    float tmpX, tmpY, tmpZ, tmpW;
    tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
    tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
    tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
    tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
    return vmVector3(
                   ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
                   ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
                   ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
                   );
}

inline const Quat_ref conj( const Quat_ref & quat )
{
    return Quat_ref( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
}

inline const Quat_ref select( const Quat_ref & quat0, const Quat_ref & quat1, bool select1 )
{
    return Quat_ref(
                ( select1 )? quat1.getX() : quat0.getX(),
                ( select1 )? quat1.getY() : quat0.getY(),
                ( select1 )? quat1.getZ() : quat0.getZ(),
                ( select1 )? quat1.getW() : quat0.getW()
                );
}



#define LOOPCOUNT 1000
#define NUM_CYCLES 10000
#define DATA_SIZE 1024

int Test_quat_aos_neon(void)
{
       
    return 0;
}

#endif