//
//  Test_3x3getRot.cpp
//  BulletTest
//
//  Copyright (c) 2011 Apple Inc.
//


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

#include "Test_3x3getRot.h"
#include "vector.h"
#include "Utils.h"
#include "main.h"
#include <math.h>
#include <string.h>

#include <LinearMath/btMatrix3x3.h>

#define LOOPCOUNT 1000
#define ARRAY_SIZE 128

static inline btSimdFloat4 rand_f4(void)
{
    return btAssign128( RANDF_m1p1, RANDF_m1p1, RANDF_m1p1, BT_NAN );      // w channel NaN
}

static inline btSimdFloat4 qtNAN_f4(void)
{
    return btAssign128( BT_NAN, BT_NAN, BT_NAN, BT_NAN );
}

static void M3x3getRot_ref( const btMatrix3x3 &m, btQuaternion &q )
{
    btVector3 m_el[3] = { m[0], m[1], m[2] };

    btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();

    btScalar temp[4];

    if (trace > btScalar(0.0)) 
    {
        btScalar s = btSqrt(trace + btScalar(1.0));
        temp[3]=(s * btScalar(0.5));
        s = btScalar(0.5) / s;

        temp[0]=((m_el[2].y() - m_el[1].z()) * s);
        temp[1]=((m_el[0].z() - m_el[2].x()) * s);
        temp[2]=((m_el[1].x() - m_el[0].y()) * s);
    } 
    else 
    {
        int i = m_el[0].x() < m_el[1].y() ? 
            (m_el[1].y() < m_el[2].z() ? 2 : 1) :
            (m_el[0].x() < m_el[2].z() ? 2 : 0); 
        int j = (i + 1) % 3;  
        int k = (i + 2) % 3;

        btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
        temp[i] = s * btScalar(0.5);
        s = btScalar(0.5) / s;

        temp[3] = (m_el[k][j] - m_el[j][k]) * s;
        temp[j] = (m_el[j][i] + m_el[i][j]) * s;
        temp[k] = (m_el[k][i] + m_el[i][k]) * s;
    }
    q.setValue(temp[0],temp[1],temp[2],temp[3]);
}

static int operator!= ( const btQuaternion &a, const btQuaternion &b )
{
    if( fabs(a.x() - b.x()) + 
        fabs(a.y() - b.y()) +
        fabs(a.z() - b.z()) +
        fabs(a.w() - b.w()) > FLT_EPSILON * 4)
        return 1;
    
    return 0;
}

int Test_3x3getRot(void)
{
    // Init an array flanked by guard pages
    btMatrix3x3     in1[ARRAY_SIZE];
    btQuaternion    out[ARRAY_SIZE];
    btQuaternion    out2[ARRAY_SIZE];
    
    // Init the data
    size_t i, j;
    for( i = 0; i < ARRAY_SIZE; i++ )
    {
        in1[i] = btMatrix3x3(rand_f4(), rand_f4(), rand_f4() );   
        out[i] = btQuaternion(qtNAN_f4());   
        out2[i] = btQuaternion(qtNAN_f4());
        
        M3x3getRot_ref(in1[i], out[i]);
        in1[i].getRotation(out2[i]);

        if( out[i] != out2[i] )
        {
            vlog( "Error - M3x3getRot result error! ");
            vlog( "failure @ %ld\n", i);
         	vlog( 	"\ncorrect = (%10.7f, %10.7f, %10.7f, %10.7f) "
					"\ntested  = (%10.7f, %10.7f, %10.7f, %10.7f) \n", 
					out[i].x(), out[i].y(), out[i].z(), out[i].w(),
					out2[i].x(), out2[i].y(), out2[i].z(), out2[i].w());
		
            return -1;
        }
    }
    
    uint64_t scalarTime, vectorTime;
    uint64_t startTime, bestTime, currentTime;
    bestTime = ~(bestTime&0);//-1ULL;
    scalarTime = 0;
    for (j = 0; j < LOOPCOUNT; j++) 
    {
        startTime = ReadTicks();
        for( i = 0; i < ARRAY_SIZE; i++ )
            M3x3getRot_ref(in1[i], out[i]);
        currentTime = ReadTicks() - startTime;
        scalarTime += currentTime;
        if( currentTime < bestTime )
            bestTime = currentTime;
    }
    if( 0 == gReportAverageTimes )
        scalarTime = bestTime;        
    else
        scalarTime /= LOOPCOUNT;
    
    bestTime = ~(bestTime&0);//-1ULL;
    vectorTime = 0;
    for (j = 0; j < LOOPCOUNT; j++) 
    {
        startTime = ReadTicks();
        for( i = 0; i < ARRAY_SIZE; i++ )
        {
            in1[i].getRotation(out2[i]);
        }
        currentTime = ReadTicks() - startTime;
        vectorTime += currentTime;
        if( currentTime < bestTime )
            bestTime = currentTime;
    }
    if( 0 == gReportAverageTimes )
        vectorTime = bestTime;        
    else
        vectorTime /= LOOPCOUNT;
    
    vlog( "Timing:\n" );
    vlog( "\t    scalar\t    vector\n" );
    vlog( "\t%10.2f\t%10.2f\n", TicksToCycles( scalarTime ) / ARRAY_SIZE, TicksToCycles( vectorTime ) / ARRAY_SIZE );
    
    return 0;
}

#endif//BT_USE_SSE