#ifndef NO_FLOAT
#include <math.h>
#endif

#include <fix16.h>
#include "interface.h"
#include <stdio.h>

/* Autogenerated testcases */
#include "testcases.c"

/* Tools for profiling */

typedef struct {
    uint32_t min;
    uint32_t max;
    uint32_t sum;
    uint32_t count;
} cyclecount_t;

// Initializer for cyclecount_t structure.
// Max is initialized to 0 and min is 2^32-1 so that first call to cyclecount_update will set them.
#define CYCLECOUNT_INIT {0xFFFFFFFF, 0, 0, 0}

// Update cyclecount_t structure after a single measurement has been made.
static void cyclecount_update(cyclecount_t *data, uint32_t cycles)
{
    if (cycles < data->min)
        data->min = cycles;
    if (cycles > data->max)
        data->max = cycles;
    
    data->sum += cycles;
    data->count++; 
}

#define MEASURE(variable, statement) { \
    start_timing(); \
    statement; \
    cyclecount_update(&variable, end_timing()); \
}

#define PRINT(variable, label) { \
    print_value(label " min", variable.min); \
    print_value(label " max", variable.max); \
    print_value(label " avg", variable.sum / variable.count); \
}

static cyclecount_t exp_cycles = CYCLECOUNT_INIT;
static cyclecount_t sqrt_cycles = CYCLECOUNT_INIT;
static cyclecount_t add_cycles = CYCLECOUNT_INIT;
static cyclecount_t sub_cycles = CYCLECOUNT_INIT;
static cyclecount_t div_cycles = CYCLECOUNT_INIT;
static cyclecount_t mul_cycles = CYCLECOUNT_INIT;

#ifndef NO_FLOAT
static cyclecount_t float_sqrtf_cycles = CYCLECOUNT_INIT;
static cyclecount_t float_add_cycles = CYCLECOUNT_INIT;
static cyclecount_t float_sub_cycles = CYCLECOUNT_INIT;
static cyclecount_t float_div_cycles = CYCLECOUNT_INIT;
static cyclecount_t float_mul_cycles = CYCLECOUNT_INIT;
#endif

static fix16_t delta(fix16_t result, fix16_t expected)
{
#ifdef FIXMATH_NO_OVERFLOW
    // Ignore overflow errors when the detection is turned off
    if (expected == fix16_minimum)
        return 0;
#endif

    if (result >= expected)
    {
        return result - expected;
    }
    else
    {
        return expected - result;
    }
}

#ifdef FIXMATH_NO_ROUNDING
const fix16_t max_delta = 1;
#else
const fix16_t max_delta = 0;
#endif

int main()
{
    int i;
    interface_init();
    
    start_timing();
    print_value("Timestamp bias", end_timing());
    
    for (i = 0; i < TESTCASES1_COUNT; i++)
    {
        fix16_t input = testcases1[i].a;
        fix16_t result;
        fix16_t expected = testcases1[i].sqrt;
        MEASURE(sqrt_cycles, result = fix16_sqrt(input));
        
        if (input > 0 && delta(result, expected) > max_delta)
        {
            print_value("Failed SQRT, i", i);
            print_value("Failed SQRT, input", input);
            print_value("Failed SQRT, output", result);
            print_value("Failed SQRT, expected", expected);
        }
        
        expected = testcases1[i].exp;
        MEASURE(exp_cycles, result = fix16_exp(input));
        
        if (delta(result, expected) > 400)
        {
            print_value("Failed EXP, i", i);
            print_value("Failed EXP, input", input);
            print_value("Failed EXP, output", result);
            print_value("Failed EXP, expected", expected);
        }
    }
    PRINT(sqrt_cycles, "fix16_sqrt");
    PRINT(exp_cycles, "fix16_exp");

    for (i = 0; i < TESTCASES2_COUNT; i++)
    {
        fix16_t a = testcases2[i].a;
        fix16_t b = testcases2[i].b;
        volatile fix16_t result;
        
        fix16_t expected = testcases2[i].add;
        MEASURE(add_cycles, result = fix16_add(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed ADD, i", i);
            print_value("Failed ADD, a", a);
            print_value("Failed ADD, b", b);
            print_value("Failed ADD, output", result);
            print_value("Failed ADD, expected", expected);
        }
        
        expected = testcases2[i].sub;
        MEASURE(sub_cycles, result = fix16_sub(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed SUB, i", i);
            print_value("Failed SUB, a", a);
            print_value("Failed SUB, b", b);
            print_value("Failed SUB, output", result);
            print_value("Failed SUB, expected", expected);
        }
        
        expected = testcases2[i].mul;
        MEASURE(mul_cycles, result = fix16_mul(a, b));
        if (delta(result, expected) > max_delta)
        {
            print_value("Failed MUL, i", i);
            print_value("Failed MUL, a", a);
            print_value("Failed MUL, b", b);
            print_value("Failed MUL, output", result);
            print_value("Failed MUL, expected", expected);
        }
        
        if (b != 0)
        {
            expected = testcases2[i].div;
            MEASURE(div_cycles, result = fix16_div(a, b));
            if (delta(result, expected) > max_delta)
            {
                print_value("Failed DIV, i", i);
                print_value("Failed DIV, a", a);
                print_value("Failed DIV, b", b);
                print_value("Failed DIV, output", result);
                print_value("Failed DIV, expected", expected);
            }
        }
    }
    PRINT(add_cycles, "fix16_add");
    PRINT(sub_cycles, "fix16_sub");
    PRINT(mul_cycles, "fix16_mul");
    PRINT(div_cycles, "fix16_div");
    
    /* Compare with floating point performance */
#ifndef NO_FLOAT
    for (i = 0; i < TESTCASES1_COUNT; i++)
    {
        float input = fix16_to_float(testcases1[i].a);
        volatile float result;
        MEASURE(float_sqrtf_cycles, result = sqrtf(input));
    }
    PRINT(float_sqrtf_cycles, "float sqrtf");
    
    for (i = 0; i < TESTCASES2_COUNT; i++)
    {
        float a = fix16_to_float(testcases2[i].a);
        float b = fix16_to_float(testcases2[i].b);
        volatile float result;
        MEASURE(float_add_cycles, result = a + b);
        MEASURE(float_sub_cycles, result = a - b);
        MEASURE(float_mul_cycles, result = a * b);
        
        if (b != 0)
        {
            MEASURE(float_div_cycles, result = a / b);
        }
    }
    PRINT(float_add_cycles, "float add");
    PRINT(float_sub_cycles, "float sub");
    PRINT(float_mul_cycles, "float mul");
    PRINT(float_div_cycles, "float div");
#endif    

    return 0;
}
