/*---------------------------------------------------------------------------*
  Project:  NitroSDK - FX - demos - test
  File:     fx_append_vec.c

  Copyright 2003,2004 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.
  
  $Log: fx_append_vec.c,v $
  Revision 1.6  2004/11/02 08:32:54  takano_makoto
  fix comment.

  Revision 1.5  2004/04/12 10:05:26  takano_makoto
  Fixed a bug in BasicOperator_test_

  Revision 1.4  2004/04/07 01:27:57  yada
  Fixed header comment

  Revision 1.3  2004/02/05 07:09:02  yasu
  Changed SDK prefix iris -> nitro

  Revision 1.2  2004/01/29 01:35:09  kitani_toshikazu
  Fixed some bugs for debug build, restored appended test cases
 
  $NoKeywords: $
    
 *---------------------------------------------------------------------------*/
 
#ifndef  FX_APPEND_VEC_H_
#include "fx_append_vec.h"
#endif//  FX_APPEND_VEC_H_



#define FX2D( fx ) (double)FX_FX32_TO_F32( fx )
#define FX64C2D( fx ) (double)( (double)( fx ) / (double)( 0x1LL << FX64C_SHIFT ) )
#define FX_FX32_TO_FX64C(x) ( (fx64)(x) << ( FX64C_SHIFT - FX32_SHIFT ))




#define fxIntMax        ( FX32_MAX & FX32_INT_MASK )
#define fxLSD           0x1


// Input data
static const VecFx32        vInputData[] = 
{
    { fxLSD         , 0x0, 0x0 },   
    { fxLSD << 1    , 0x0, 0x0 },   
    { FX32_ONE      , 0x0, 0x0 },   
    { fxIntMax  >> 6, 0x0, 0x0 },   
    { fxIntMax  >> 5, 0x0, 0x0 },
    { fxIntMax  >> 4, 0x0, 0x0 },
    { fxIntMax  >> 3, 0x0, 0x0 },
    { fxIntMax  >> 2, 0x0, 0x0 },
    { fxIntMax  >> 1, 0x0, 0x0 },
    { fxIntMax      , 0x0, 0x0 },
    
    //Case that addition happens internally  
    { fxLSD,            fxLSD,              0x0 },  
    { fxLSD << 1,       fxLSD << 1,         0x0 },  
    { fxIntMax  >> 5,   fxIntMax    >> 5,   0x0 },
    { fxIntMax  >> 4,   fxIntMax    >> 4,   0x0 },
    { fxIntMax  >> 3,   fxIntMax    >> 3,   0x0 },
    { fxIntMax  >> 2,   fxIntMax    >> 2,   0x0 },
    { fxIntMax  >> 1,   fxIntMax    >> 1,   0x0 },
    { fxIntMax      ,   fxIntMax        ,   0x0 }
};






// Util
static void     VecFx32ToVecD_( const VecFx32 * src, VecD * dst );
static void     VecSubVecD_( const VecD* a, const VecD* b, VecD* ab );
static void     GetErrorVecD_( const VecD* a, const VecD* b, VecD* ab );
static void     CalcErrorBtweenVecFxAndVecD_( const VecFx32* a, const VecD* b, VecD* vError );
static double   FindMaxElement_( const VecD* v );



// double version mounting
static double   FX_Mul_d_( const fx32 a, const fx32 b );
static double   VEC_DotProduct_d_( const VecFx32* a, const VecFx32* b );
static void     VEC_CrossProduct_d_(const VecFx32 * a, const VecFx32 * b, VecD * axb);
static double   VEC_Mag_d_( const VecFx32 * pSrc );
static void     VEC_Normalize_d_( const VecFx32 * pSrc, VecD * pDst);

// Error calculation
static double   GetDotProductError_( const VecFx32  *a, const VecFx32   *b );
static double   GetCrossProductError_( const VecFx32* a, const VecFx32* b );
static double   GetMagError_( const VecFx32* a );
static void     GetNormalizeError_( const VecFx32* a, VecD* vError );

// Tests
static void VEC_Mag_test_();
static void VEC_Normalize_test_();
static void VEC_DotCrossProduct_test_();
static void BasicOperator_test_();
static void Unary_operator_test_( fx32 vf1 );
static void Binary_operator_test_( fx32 vf1, fx32 vf2 );


//------------------------------------------------------------------------------
// Test additional items entry
void VECTest_Appended() 
{
    OS_Printf( "**================== VECTest_Appended start ====================\n");
    BasicOperator_test_();
    VEC_DotCrossProduct_test_();
    VEC_Mag_test_();
    VEC_Normalize_test_();
    OS_Printf( "**================== VECTest_Appended done ====================\n");
}

//==============================================================================
// Display related

//------------------------------------------------------------------------------
static void printVecFx32Msg( const char* msg, const VecFx32* v )
{
    SDK_NULL_ASSERT(msg);
    SDK_NULL_ASSERT(v);
    
    OutDetail( msg );
    OutDetail("( %.5f, %.5f, %.5f )", FX_FX32_TO_F32(v->x), FX_FX32_TO_F32(v->y), FX_FX32_TO_F32(v->z) );
    OutDetail( "\n" );
}
//------------------------------------------------------------------------------
static void printVecDMsg( const char* msg, const VecD* v )
{
    SDK_NULL_ASSERT(msg);
    SDK_NULL_ASSERT(v);
    
    OutDetail( msg );
    OutDetail("( %.5f, %.5f, %.5f )", v->x, v->y, v->z );
    OutDetail( "\n" );
}



//==============================================================================
// Misc.
//------------------------------------------------------------------------------
static void VecFx32ToVecD_( const VecFx32 * src, VecD * dst )
{
    SDK_NULL_ASSERT(src);
    SDK_NULL_ASSERT(dst);
    
    dst->x = (double)FX_FX32_TO_F32( src->x );
    dst->y = (double)FX_FX32_TO_F32( src->y );
    dst->z = (double)FX_FX32_TO_F32( src->z );
}

//------------------------------------------------------------------------------
static void VecSubVecD_( const VecD* a, const VecD* b, VecD* ab )
{
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    SDK_NULL_ASSERT(ab);
    
    ab->x = a->x - b->x;
    ab->y = a->y - b->y;
    ab->z = a->z - b->z;
}

//------------------------------------------------------------------------------
static void GetErrorVecD_( const VecD* a, const VecD* b, VecD* ab )
{
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    SDK_NULL_ASSERT(ab);
    
    VecSubVecD_( a, b, ab );
    
    ab->x = fabs( ab->x );
    ab->y = fabs( ab->y );
    ab->z = fabs( ab->z );
}

//------------------------------------------------------------------------------
static void CalcErrorBtweenVecFxAndVecD_( const VecFx32* a, const VecD* b, VecD* vError )
{
    VecD        aD;
    
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    SDK_NULL_ASSERT(vError);
    
    // fx => double
    VecFx32ToVecD_( a, &aD );
    
    // error = a - b
    GetErrorVecD_( &aD, b, vError );
}

//------------------------------------------------------------------------------
// Get element with maximum absolute 
static double FindMaxElement_( const VecD* v )
{
    
    double      max = 0.0;
    
    SDK_NULL_ASSERT(v);
    
    if( fabs( v->x ) > fabs( v->y ) )
    {
        max = fabs( v->x );
    }else{
        max = fabs( v->y );
    }
    
    if( fabs( v->z ) > max )
        max = fabs( v->z );
    
    return max;
}





//------------------------------------------------------------------------------
// double version
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
static double FX_Mul_d_( const fx32 a, const fx32 b )
{
    return (double)(FX_FX32_TO_F32(a)) * (double)(FX_FX32_TO_F32(b));
}
//------------------------------------------------------------------------------
static double VEC_DotProduct_d_( const VecFx32* a, const VecFx32* b )
{
    VecD        da,db;
    
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    
    
    VecFx32ToVecD_( a, &da );
    VecFx32ToVecD_( b, &db );
    
    return da.x * db.x + da.y * db.y + da.z * db.z;
}
//------------------------------------------------------------------------------
static void VEC_CrossProduct_d_(const VecFx32 * a, const VecFx32 * b, VecD * axb)
{
    VecD        da,db;
    
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    SDK_NULL_ASSERT(axb);
    
    VecFx32ToVecD_( a, &da );
    VecFx32ToVecD_( b, &db );
    
    axb->x = da.y * db.z - da.z * db.y;
    axb->y = da.z * db.x - da.x * db.z;
    axb->z = da.x * db.y - da.y * db.x;             
}
//------------------------------------------------------------------------------
static double VEC_Mag_d_( const VecFx32 * pSrc )
{
    VecD        pSrcD;
    
    SDK_NULL_ASSERT(pSrc);

    
    VecFx32ToVecD_( pSrc, &pSrcD );
    
    return sqrt( pSrcD.x * pSrcD.x + pSrcD.y * pSrcD.y + pSrcD.z * pSrcD.z );
}
//------------------------------------------------------------------------------
static void VEC_Normalize_d_( const VecFx32 * pSrc, VecD * pDst)
{
    double      t = 0.0;
    VecD        pSrcD;
    
    SDK_NULL_ASSERT(pSrc);
    SDK_NULL_ASSERT(pDst);
    
    
    VecFx32ToVecD_( pSrc, &pSrcD );
    
    t = VEC_Mag_d_( pSrc );
    SDK_ASSERTMSG(t > 0, "*pSrc is (0, 0, 0)");
    
    t = 1.0 / t;
    
    pDst->x = t * pSrcD.x;
    pDst->y = t * pSrcD.y;
    pDst->z = t * pSrcD.z;
}


//==============================================================================
// Error calculation 
//
//------------------------------------------------------------------------------
static double GetDotProductError_( const VecFx32    *a, const VecFx32   *b )
{
    double  resD = 0.0;
    double  resFX = 0.0;
    
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    
    resD    = VEC_DotProduct_d_( a, b );
    resFX   = (double)( FX_FX32_TO_F32( VEC_DotProduct( a, b ) ) );

    return fabs( resFX - resD );
}

//------------------------------------------------------------------------------
static double GetCrossProductError_( const VecFx32* a, const VecFx32* b )
{
    VecFx32     resFX;
    VecD        vError;
    
    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(b);
    
    
    VEC_CrossProduct_d_( a, b, &vError );
    VEC_CrossProduct( a, b, &resFX );
    
    CalcErrorBtweenVecFxAndVecD_( &resFX, &vError, &vError );
    
    return FindMaxElement_( &vError );
}

//------------------------------------------------------------------------------
static double GetMagError_( const VecFx32* a )
{
    double  resD = 0.0;
    double  resFX = 0.0;
    
    SDK_NULL_ASSERT(a);
    
    resD    = VEC_Mag_d_( a );
    resFX   = (double)( FX_FX32_TO_F32( VEC_Mag( a ) ) );

    
    OutDetail("**** VEC_Mag ::: FX     Result => %.10f\n" );
    OutDetail("**** VEC_Mag ::: double Result => %.10f\n" );
        
    
    return fabs( resFX - resD );
}

//------------------------------------------------------------------------------
static void GetNormalizeError_( const VecFx32* a, VecD* vError )
{
    VecFx32     resFX;

    SDK_NULL_ASSERT(a);
    SDK_NULL_ASSERT(vError);
    
    VEC_Normalize   ( a, &resFX );
    VEC_Normalize_d_( a, vError );


    OutDetail       ("===================================================================================\n");
    printVecFx32Msg ("**** VEC_Mag :::        Input  => ", a );
    printVecFx32Msg ("                 FX     Result => ", &resFX );
    printVecDMsg    ("                 double Result => ", vError );

    
    CalcErrorBtweenVecFxAndVecD_( &resFX, vError, vError );
    

    OutDetail       ("-----------------------------------------------------------------------------------\n");
    printVecDMsg    ("                 | FX-double | => ", vError );
    OutDetail("\n");

}

//------------------------------------------------------------------------------
static BOOL IsResultVecValid( const VecD* result )
{
    SDK_NULL_ASSERT( result );
    return ( FindMaxElement_( result ) < CRITICAL_ERROR_THRESHOLD ) ? TRUE : FALSE;
}

//==============================================================================
// test Method
//==============================================================================
// Apply VEC_Mag_test_for different size of vector
// In case of finding unexpected error, output message and quit the application.
static void VEC_Mag_test_()
{
    int         i = 0;
    double      err = 0.0;
    VecFx32*    v = NULL;
    
    OS_Printf( "==================== VEC_Mag_test_ begin ====================\n");
    
    // OK Cases
    OS_Printf("------------ Must be OK case \n" );
    for( i = 0;i < 9; i++ )
        MUST_SUCCEED_ASSERT( GetMagError_( &vInputData[i] ) < CRITICAL_ERROR_THRESHOLD, 1 );
    
    // NG Case 
    OS_Printf("------------ Must be NG case \n" );
    MUST_FAIL_ASSERT( GetMagError_( &vInputData[9] ) < CRITICAL_ERROR_THRESHOLD, 2 );
    
    // Case that addition happens internally
    // OK Cases
    OS_Printf("------------ Must be OK case \n" );
    for( i = 10;i < 16; i++ )
        MUST_SUCCEED_ASSERT( GetMagError_( &vInputData[i] ) < CRITICAL_ERROR_THRESHOLD, 3 );
        
    
    // NG Case 
    OS_Printf("------------ Must be NG case \n" );
    MUST_FAIL_ASSERT( GetMagError_( &vInputData[16] ) < CRITICAL_ERROR_THRESHOLD, 4 );
    MUST_FAIL_ASSERT( GetMagError_( &vInputData[17] ) < CRITICAL_ERROR_THRESHOLD, 5 );
    
    OS_Printf( "==================== VEC_Mag_test_ done ====================\n");
}


//------------------------------------------------------------------------------
// Compare the output error to double version for normalized function of vector. 
// Output message when an unexpected error is found. 
//
static void VEC_Normalize_test_()
{
    int         i = 0;
    VecD    testResult; 

    OS_Printf( "==================== VEC_Normalize_test_ begin ====================\n");
    OS_Printf("------------ Must SUCCEED case \n" );
    for( i = 0;i < 7; i++ )
    {
        GetNormalizeError_( &vInputData[i], &testResult );
        MUST_SUCCEED_ASSERT( IsResultVecValid( &testResult ), 1 );
    }
    
    
    for( i = 11;i < 14; i++ )
    {
        GetNormalizeError_( &vInputData[i], &testResult );
        MUST_SUCCEED_ASSERT( IsResultVecValid( &testResult ), 2 );
    }
    
    OS_Printf("------------ Must FAIL case \n" );
    // NG Case          very large 
    for( i = 7;i < 10; i++ )
    {
        GetNormalizeError_( &vInputData[i], &testResult );
        MUST_FAIL_ASSERT( IsResultVecValid( &testResult ), 3 );
    }
    
    // NG Case          very small 
    GetNormalizeError_( &vInputData[10], &testResult );
    MUST_FAIL_ASSERT( IsResultVecValid( &testResult ), 4 );
    
    // NG Case          very large 
    for( i = 14;i < 18; i++ )
    {
        GetNormalizeError_( &vInputData[i], &testResult );
        MUST_FAIL_ASSERT( IsResultVecValid( &testResult ), 5 );
    }
    
    OS_Printf( "==================== VEC_Normalize_test_ done ====================\n");
}

//------------------------------------------------------------------------------
// Calculate value of inner/outer product for parallel/orthogonal vector, and check the error with double calculation. 
// If an unexpected error is found, output message and quit the application. 
// When defining FX_TEST__SHOW_DETAIL, output the detail information to debug console. 
static void VEC_DotCrossProduct_test_()
{
    VecFx32     v1;
    VecFx32     v2;
    VecFx32     v3;
    
    int i = 0, j = 0;
    const int angD  = 182;      // minute angle change (about 1 degree)
    const int ang45 = 8192;     
    double  errDot = 0.0;
    double  errCross = 0.0;
    
    OS_Printf       ( "==================== VEC_DotCrossProduct_test_ begin ====================\n");
    // Changing the angle slightly
    // (because of an ideal orthogonal and parallel vector at i = 0, there is no error)  
    for( i = 1; i < 5; i++ )
    {
        v1.x = FX_CosIdx( ang45 );          v1.y =  FX_SinIdx( ang45 );         v1.z = 0x0;
        v2.x = FX_CosIdx( ang45 + angD*i ); v2.y = -FX_SinIdx( ang45 + angD*i );v2.z = 0x0; // orthogonal
        v3.x = FX_CosIdx( ang45 + angD*i ); v3.y =  FX_SinIdx( ang45 + angD*i );v3.z = 0x0; // parallel
        
        // lengthen out 
        for( j = 0; j < 7 ; j++ )
        {
            v1.x = FX_Mul( v1.x, FX32_ONE << j );v1.y = FX_Mul( v1.y, FX32_ONE << j );
            v2.x = FX_Mul( v2.x, FX32_ONE << j );v2.y = FX_Mul( v2.y, FX32_ONE << j );
            v3.x = FX_Mul( v3.x, FX32_ONE << j );v3.y = FX_Mul( v3.y, FX32_ONE << j );
            
            // Calculate the maximum error with double mounting
            errDot      = GetDotProductError_( &v1, &v2 );
            errCross    = GetCrossProductError_( &v1, &v3 );
            
            
            // Confirm success and failure
            if( j < 5 )
            {
                MUST_SUCCEED_ASSERT(( errDot   < CRITICAL_ERROR_THRESHOLD ) &&
                                    ( errCross < CRITICAL_ERROR_THRESHOLD ), 1 );
            }else{
                // When FX32_ONE << 5, a large amount of error occurs to the 
                // result of CrossProduct. Therefore, this trial is supposed to fail.
                // Also, a parallel vector of the same length in the opposite direction 
                // can be calculated with a larger value.
                // A further check of the inside of CrossProduct is expected. 
                MUST_FAIL_ASSERT(( errDot   < CRITICAL_ERROR_THRESHOLD ) &&
                                 ( errCross < CRITICAL_ERROR_THRESHOLD ), 2 );
            }

            // Output the detail information
            {
                OutDetail       ( "------------ input ---------------------\n");
                printVecFx32Msg ( "v1                      = ", &v1 );
                printVecFx32Msg ( "v2(perpendicular to v1) = ", &v2 );
                printVecFx32Msg ( "v3(parallel To v1)      = ", &v3 );
                
                OutDetail       ( "v1 . v2 Error   = %.10f \n", errDot );
                OutDetail       ( "v1 X v3 Error   = %.10f \n", errCross );
            }
        }
    }
    
    OS_Printf       ( "==================== VEC_DotCrossProduct_test_ done ====================\n");
}


//------------------------------------------------------------------------------
// Basic operation function test 
// Display the error between fixed decimal operation function and operation carried out with double on debug console.   
// 
static void BasicOperator_test_()
{
    int i = 0, j = 0;
    fx32            vf1 = 0x0, vf2 = 0x0;
    
    OS_Printf( "==================== BasicOperator_test_ begin ====================\n");
    
    // unary_operators
    OS_Printf( "------------ unary_operators begin ---------------------\n");   
    for( i = 0;i < 32; i += 4 )
    {
        vf1 += 0x1 << i;
        Unary_operator_test_( vf1 );
    }
    OS_Printf( "------------ unary_operators done ---------------------\n");    
    // binary_operators
    OS_Printf( "------------ binary_operators begin ---------------------\n");
    vf1 = 0x0;
    for( i = 0; i < 32; i += 4 )
    {
        vf1 += 0x1 << i;
        vf2 = 0;
        for( j = 0;j < 32; j++ )
        {
            vf2 += 0x1 << j;
            Binary_operator_test_( vf1, vf2 );
        }   
    }
    OS_Printf( "------------ binary_operators done ---------------------\n");
    
    OS_Printf( "==================== BasicOperator_test_ done ====================\n");
}



//------------------------------------------------------------------------------
// Fixed decimal monadic operation function test
// The error between normal operation and operation using double type with the input value                                                         
// as the parameter is displayed on the debug console.                                 
// 
static void Unary_operator_test_( fx32 vf1 )
{
    double  vd1 = (double)FX_FX32_TO_F32( vf1 );
    
    double  diff = 0.0;
    
    // input fixed-point value 
    OutDetail       ( "                 v = %.10f \n", vd1 );
    
    // FX_Sqrt
    diff = fabs( FX2D( FX_Sqrt( vf1 ) ) - sqrt( vd1 ) );
    OutDetail       ( "sqrt( v1 )   Error = %.10f \n", diff );
    
    // FX_InvSqrt
    diff = fabs( FX2D( FX_InvSqrt( vf1 ) ) - 1.0 / sqrt( vd1 ) );
    OutDetail       ( "InvSqrt( v1 )Error = %.10f \n", diff );
    
    // FX_Inv
    diff = fabs( FX2D( FX_Inv( vf1 ) ) - 1.0 / vd1 );
    OutDetail       ( "1.0 / v1     Error = %.10f \n", diff );
        
    // FX_InvFx64c
    diff = fabs( FX64C2D( FX_InvFx64c( vf1 ) ) - 1.0 / vd1 );
    OutDetail       ( "1.0 / v1 _64 Error = %.10f \n", diff );
    
}

//------------------------------------------------------------------------------
// Fixed decimal binary operation function test
// The error between normal operation and operation using double type with the input value 
// as the parameter is displayed on the debug console. 
//
static void Binary_operator_test_( fx32 vf1, fx32 vf2 )
{
    double  vd1 = (double)FX_FX32_TO_F32( vf1 );
    double  vd2 = (double)FX_FX32_TO_F32( vf2 );
    double  diff = 0.0;
    
    // input fixed-point value 
    OutDetail       ( "v1 = %.10f, v2 = %.10f\n", vd1, vd2 );
    
    // FX_Mul
    diff = fabs( FX2D( FX_Mul( vf1, vf2 ) ) - vd1 * vd2 );
    OutDetail       ( "v1 * v2     Error = %.10f \n", diff );
    
    // FX_Div
    diff = fabs( FX2D( FX_Div( vf1, vf2 ) ) - vd1 / vd2 );
    OutDetail       ( "v1 / v2     Error = %.10f \n", diff );
    
    // FX_Mul32x64c
    diff = fabs( FX2D( FX_Mul32x64c( vf1, FX_FX32_TO_FX64C( vf2 ) ) ) - vd1 * vd2 );
    OutDetail       ( "v1 * v2 _64 Error = %.10f \n", diff );
    
    // FX_DivFx64c
    diff = fabs( FX64C2D( FX_DivFx64c( vf1,  vf2  ) ) - vd1 / vd2 );
    OutDetail       ( "v1 / v2 _64 Error = %.10f \n", diff );
    
}


