#include <stdio.h>

#include <assert.h>

#include <math.h>

#define write_string(x) printf(x)

#define write_int(x) printf( "%08X", x )

int do_fsca(int angle, float *a, float *b);

float do_fsrra( float param );

float do_fipr( float *vectora, float *vectorb );

float do_fipr2( float *vectora, float *vectorb );

void do_ftrv( float *matrix, float *vector );

void do_ftrv2( float *matrix, float *vector );

unsigned int get_fpscr();

void set_fpscr( unsigned int fpscr );

#define MAX_FLOAT_DIFF 0.00001

static inline float inttof( unsigned int i )

{

    return *((float *)&i);

}

static inline double longtod( unsigned long long l )

{

    return *((double *)&l);

}

static inline unsigned int ftoint( float f )

{

    return *((unsigned int *)&f);

}

static inline unsigned long long dtolong( double d )

{

    return *((unsigned long long *)&d);

}

#define QNANF inttof(0x7FBFFFFF)

#define QNAND longtod(0x7FF7FFFFFFFFFFFFLL)

int compare_float( float a, float b )

{

    if( ftoint(a) == ftoint(b) ) /* Exact bit-pattern match */

	return 1;

    float diff = (a>b?(a-b):(b-a));

    return diff < MAX_FLOAT_DIFF;

}

#define FTOINT(a) (*((unsigned int *)&a))

#define TEST_FEQUALS( a, b ) if( !compare_float(a,b) ) { printf( "Assertion failed at %s.%d: expected %.8f (%08X) but was %.8f (%08X)\n", __FILE__, __LINE__, a, FTOINT(a), b, FTOINT(b) ); return 1; }

int test_fsca( int angle, float expect_a, float expect_b )

{

    float a = 10.0;

    float b = 10.0;

    do_fsca(angle, &a, &b );

    TEST_FEQUALS(expect_a, a);

    TEST_FEQUALS(expect_b, b);

}

int test_fsrra( float f, float expect )

{

    float f2 = do_fsrra( f );

    TEST_FEQUALS( expect, f2 );

}

void test_coercion( float f )

{

    double d = (double)f;

    float q = (float)d;

    unsigned int i = (unsigned int)d;

    signed int j = (signed int)q;

    printf( "Coerce: %08X %08X%08X %08X %08X %08X\n", *((unsigned int *)&f),

    	    *((unsigned int *)&d),  *(((unsigned int *)&d)+1),

    	    *((unsigned int *)&q), i, j );

}

void test_doublearr( int len )

{

    double arr[len];

    unsigned int *iarr = (unsigned int *)&arr;

    int i;

    arr[0] = 2.5;

    for( i=1; i<len; i++ ) {

	arr[i] = (arr[i-1] * arr[i-1] + arr[0]) / 1.25 - 0.19;

    }

    printf( "arr: " );

    for( i=0; i<len; i++ ) {

	printf( "%08X", *iarr++ );

	printf( "%08X ", *iarr++ );

    }

    printf( "\n" );

}

void test_floatarr( int len )

{

    float arr[len];

    unsigned int *iarr = (unsigned int *)&arr;

    int i;

    arr[0] = 2.5;

    for( i=1; i<len; i++ ) {

	arr[i] = (arr[i-1] * arr[i-1] + arr[0]) / 1.25 - 0.19;

    }

    write_string( "arr: " );

    for( i=0; i<len; i++ ) {

	write_int( *iarr++ );

	write_string( " " );

    }

    write_string( "\n" );

}

float __attribute__((aligned(8))) matrix[16] = { 1.26, 2.34, 5.67, 3.497, -1.23, 43.43, -45.68, 9.12, 

			 12.1, 34.301, -297.354, 0.05, 0.123, 23.34, 9.99, 33.321 };

float __attribute__((aligned(8))) vec1[4] = { 5.65, 9.98, -34.12, 0.043 };

float __attribute__((aligned(8))) vec2[4] = { 3.45, -123, 4.54, 98.0909 };

float __attribute__((aligned(8))) vec3[4] = { 1.25, 2.25, 3.75, 5.12 };

float __attribute__((aligned(8))) vec4[4] = {-0.25, 8.9, -2.3, 9.19 };

void test_fipr( )

{

    float r = do_fipr( vec3, vec4 );

    write_string( "fipr: " );

    write_int( *(unsigned int *)&r );

    write_string( "\n" );

    r = do_fipr2( vec4, vec3 );

    write_string( "fipr: " );

    write_int( *(unsigned int *)&r );

    write_string( "\n" );

}

void test_ftrv( )

{

    do_ftrv( matrix, vec1 );

    write_string( "ftrv: " );

    write_int( *(unsigned int *)&vec1[0] );

    write_int( *(unsigned int *)&vec1[1] );

    write_int( *(unsigned int *)&vec1[2] );

    write_int( *(unsigned int *)&vec1[3] );

    write_string( "\n" );

    do_ftrv2( matrix, vec2 );

    write_string( "ftrv: " );

    write_int( *(unsigned int *)&vec2[0] );

    write_int( *(unsigned int *)&vec2[1] );

    write_int( *(unsigned int *)&vec2[2] );

    write_int( *(unsigned int *)&vec2[3] );

    write_string( "\n" );

}

int main()

{

    fprintf( stderr, "LL: %d\n", sizeof(unsigned long long) );

    write_int( get_fpscr() );

    write_string( "\n" );

    test_fsca( 0x00000000, 0, 1 );

    test_fsca( 0x00011234, 0.43205646, 0.90184659 );

    test_fsca( 0xFEDCBA98, -0.99121743, -0.13224234 );

    test_fsrra( 25.0, 0.2 );

    test_fsrra( 0.05, 4.4721361 );

    test_fsrra( -12.345, QNANF );

    test_coercion( -3.1415926 );

    test_coercion( 123456789012346567.890 );

    test_coercion( -2147483648.0 );

    test_coercion( 5234.1234 );

    test_doublearr( 5 );

    test_floatarr( 5 );

    test_fipr();

    test_ftrv();

    return 1;

}