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



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


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

#include <LinearMath/btVector3.h>


// reference code for testing purposes
static long mindot_ref(    const btSimdFloat4 *vertices, 
                       float *vec,
                       size_t count, 
                       float *dotResult );

#ifdef __arm__
    #define MAX_LOG2_SIZE   9
#else
    #define MAX_LOG2_SIZE   9
#endif
#define MAX_SIZE        (1U << MAX_LOG2_SIZE)
#define LOOPCOUNT 100

int Test_mindot(void)
{
    // Init an array flanked by guard pages
    btSimdFloat4 *data = (btSimdFloat4*) GuardCalloc( 1, MAX_SIZE * sizeof(btSimdFloat4), NULL );
    float *fp = (float*) data;
    long correct, test;
    btVector3 localScaling( 0.1f, 0.2f, 0.3f);
    size_t size;
    
    // Init the data
    size_t i;
    for( i = 0; i < MAX_SIZE; i++ )
    {
        fp[4*i] = (int32_t) RANDF_16;
        fp[4*i+1] = (int32_t) RANDF_16;
        fp[4*i+2] = (int32_t) RANDF_16;
        fp[4*i+3] = BT_NAN;     // w channel NaN
    }
    
    float correctDot, testDot;
    fp = (float*) localScaling;
	float maxRelativeError = 0.f;

    for( size = 1; size <= MAX_SIZE; size++ )
    {
        float *in = (float*)(data + MAX_SIZE - size);
        size_t position;
        
        for( position = 0; position < size; position++ )
        {
            float *biggest = in + position * 4;
            float old[4] = { biggest[0], biggest[1], biggest[2], biggest[3] };
            biggest[0] -= LARGE_FLOAT17;
            biggest[1] -= LARGE_FLOAT17;
            biggest[2] -= LARGE_FLOAT17;
            biggest[3] -= LARGE_FLOAT17;
            
            correctDot = BT_NAN;
            testDot = BT_NAN;
            correct = mindot_ref( (btSimdFloat4*) in, (float*) &localScaling, size, &correctDot);
            test = localScaling.minDot( (btVector3*) in, size, testDot);
            if( test < 0 || test >= size )
            {
                vlog( "Error @ %ld: index out of bounds! *%ld vs %ld \n", size, correct, test);
                continue;
            }
            if( correct != test )
			{
                vlog( "Error @ %ld: index misreported! *%ld vs %ld  (*%f, %f)\n", size, correct, test, 
                       fp[0] * in[4*correct] + fp[1] * in[4*correct+1]  + fp[2] * in[4*correct+2], 
                       fp[0] * in[4*test] + fp[1] * in[4*test+1]  + fp[2] * in[4*test+2] );
				return 1;
			}
            if( test != position )
			{
                vlog( "Biggest not found where it is supposed to be: *%ld vs %ld (*%f, %f)\n", position, test, 
                       fp[0] * in[4*test] + fp[1] * in[4*test+1]  + fp[2] * in[4*test+2],
                       fp[0] * in[4*position] + fp[1] * in[4*position+1]  + fp[2] * in[4*position+2]  );
				return 1;
			}

            if( correctDot != testDot )
			{
				float relativeError = btFabs((testDot - correctDot) / correctDot);
				if (relativeError>1e6)
				{
					vlog( "Error @ %ld: dotpr misreported! *%f vs %f    (*%f, %f)\n", size, correctDot, testDot, 
						   fp[0] * in[4*correct] + fp[1] * in[4*correct+1]  + fp[2] * in[4*correct+2], 
						   fp[0] * in[4*test] + fp[1] * in[4*test+1]  + fp[2] * in[4*test+2]  );
					return 1;
				} else
				{
					if (maxRelativeError < relativeError)
					{
						maxRelativeError = relativeError;
					}
				}
			}

            
            memcpy( biggest, old, 16 );
        }
    }
    
	if (maxRelativeError)
	{
		printf("Warning: relative error = %e\n", maxRelativeError);
	}
    uint64_t scalarTimes[33 + (MAX_LOG2_SIZE-5)];
    uint64_t vectorTimes[33 + (MAX_LOG2_SIZE-5)];
    size_t j, k;
    float *in = (float*) data;
    for( size = 1; size <= 32; size++ )
    {
        uint64_t startTime, bestTime, currentTime;
        
        bestTime = -1LL;
        scalarTimes[size] = 0;
        for (j = 0; j < 100; j++) {
            startTime = ReadTicks();
            for( k = 0; k < LOOPCOUNT; k++ )
                correct += mindot_ref( (btSimdFloat4*) in, (float*) &localScaling, size, &correctDot);
            currentTime = ReadTicks() - startTime;
            scalarTimes[size] += currentTime;
            if( currentTime < bestTime )
                bestTime = currentTime;
        }
        if( 0 == gReportAverageTimes )
            scalarTimes[size] = bestTime;        
        else
            scalarTimes[size] /= 100;
    }
    
    uint64_t *timep = &scalarTimes[33];
    for( size = 64; size <= MAX_SIZE; size *= 2 )
    {
        uint64_t startTime, bestTime, currentTime;
        
        bestTime = -1LL;
        timep[0] =0;
        for (j = 0; j < 100; j++) {
            startTime = ReadTicks();
            for( k = 0; k < LOOPCOUNT; k++ )
                correct += mindot_ref( (btSimdFloat4*) in, (float*) &localScaling, size, &correctDot);
            currentTime = ReadTicks() - startTime;
            timep[0] += currentTime;
            if( currentTime < bestTime )
                bestTime = currentTime;
        }
        if( 0 == gReportAverageTimes )
            timep[0] = bestTime;        
        else
            timep[0] /= 100;
        
        timep++;
    }
    
    for( size = 1; size <= 32; size++ )
    {
        uint64_t startTime, bestTime, currentTime;
        
        bestTime = -1LL;
        vectorTimes[size] = 0;
        for (j = 0; j < 100; j++) {
            startTime = ReadTicks();
            for( k = 0; k < LOOPCOUNT; k++ )
                test += localScaling.minDot( (btVector3*) in, size, testDot);
            currentTime = ReadTicks() - startTime;
            vectorTimes[size] += currentTime;
            if( currentTime < bestTime )
                bestTime = currentTime;
        }
        if( 0 == gReportAverageTimes )
            vectorTimes[size] = bestTime;        
        else
            vectorTimes[size] /= 100;
    }
    
    timep = &vectorTimes[33];
    for( size = 64; size <= MAX_SIZE; size *= 2 )
    {
        uint64_t startTime, bestTime, currentTime;
        
        bestTime = -1LL;
        timep[0] =0;
        for (j = 0; j < 100; j++) {
            startTime = ReadTicks();
            for( k = 0; k < LOOPCOUNT; k++ )
                test += localScaling.minDot( (btVector3*) in, size, testDot);
            currentTime = ReadTicks() - startTime;
            timep[0] += currentTime;
            if( currentTime < bestTime )
                bestTime = currentTime;
        }
        if( 0 == gReportAverageTimes )
            timep[0] = bestTime;        
        else
            timep[0] /= 100;
        
        timep++;
    }
    
    vlog( "Timing:\n" );
    vlog( " size\t    scalar\t    vector\n" );
    for( size = 1; size <= 32; size++ )
        vlog( "%5lu\t%10.2f\t%10.2f\n", size, TicksToCycles( scalarTimes[size] ) / LOOPCOUNT, TicksToCycles( vectorTimes[size] ) / LOOPCOUNT );
    size_t index = 33;
    for( size = 64; size <= MAX_SIZE; size *= 2 )
    {
        vlog( "%5lu\t%10.2f\t%10.2f\n", size, TicksToCycles( scalarTimes[index] ) / LOOPCOUNT, TicksToCycles( vectorTimes[index] ) / LOOPCOUNT );
        index++;
    }
    
    // Useless check to make sure that the timing loops are not optimized away
    if( test != correct )
        vlog( "Error: Test != correct: *%ld vs. %ld\n", correct, test);
    
    GuardFree(data);
    
    return 0;
}



static long mindot_ref(    const btSimdFloat4 *vertices, 
                       float *vec,
                       size_t count, 
                       float *dotResult )
{
    
    const float *dp = (const float*) vertices;
    float  minDot = BT_INFINITY;
    long i = 0;
    long ptIndex = -1;
    
    for( i = 0; i < count; i++ )
    {
        float dot = vec[0] * dp[0] + vec[1] * dp[1] + vec[2] * dp[2];   dp += 4;
        
        if( dot < minDot )
        {
            minDot = dot;
            ptIndex = i;
        }
    }
    
    *dotResult = minDot;
    
    return ptIndex;
}

#endif //BT_USE_SSE