#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <basic-geometry.h>

#ifdef _WIN64
#include <windows.h>
#else
#include <time.h>
#endif // _WINDOWS_

BgFP32Versor * allocate_versors(const unsigned int amount)
{
    return calloc(amount, sizeof(BgFP32Versor));
}

BgFP32Versor * make_zero_versors(const unsigned int amount)
{
    BgFP32Versor * list = allocate_versors(amount);

    if (list == 0) {
        return 0;
    }

    for (unsigned int i = 0; i < amount; i++) {
        bg_fp32_versor_reset(&list[i]);
    }

    return list;
}

BgFP32Versor * make_random_versors(const unsigned int amount)
{
    BgFP32Versor * list = allocate_versors(amount);

    if (list == 0) {
        return 0;
    }

    for (unsigned int i = 0; i < amount; i++) {
        bg_fp32_versor_set_values(
            (2.0f * rand()) / RAND_MAX - 1.0f,
            (2.0f * rand()) / RAND_MAX - 1.0f,
            (2.0f * rand()) / RAND_MAX - 1.0f,
            (2.0f * rand()) / RAND_MAX - 1.0f,
            &list[i]
        );
    }

    return list;
}

void print_versor(const BgFP32Versor* versor)
{
    printf("Versor (%f, %f, %f, %f); Delta = %e\n", versor->s0, versor->x1, versor->x2, versor->x3, bg_fp32_versor_get_modulus(versor) - 1.0f);
}

void print_vector(const BgFP32Vector3* vector)
{
    printf("(%f, %f, %f) / %f\n", vector->x1,  vector->x2,  vector->x3,  bg_fp32_vector3_get_modulus(vector));
}
/*
int main()
{
    const unsigned int amount = 1000000;

#ifdef _WIN64
    ULONGLONG now;
    now = GetTickCount64();
    srand((unsigned int)(now & 0xfffffff));
#else
    struct timespec now;
    clock_gettime(CLOCK_REALTIME, &now);
    srand((unsigned int)(now.tv_nsec & 0xfffffff));
#endif // _WIN64

    BgFP32Versor * versors = make_random_versors(amount);

    if (versors == 0) {
        printf("Cannot allocate memory for versors");
        return 0;
    }

    BgFP32Vector3 initial, result;

    bg_fp32_vector3_set_values(1, 2, 3, &initial);
    bg_fp32_vector3_copy(&initial, &result);

#ifdef _WIN64
    ULONGLONG start, end;
    start = GetTickCount64();
#else
    struct timespec start, end;
    clock_gettime(CLOCK_REALTIME, &start);
#endif // _WIN64
    for (unsigned int i = 0; i < amount; i++) {
        bg_fp32_versor_turn2(&versors[i], &result, &result);
    }

    for (unsigned int i = amount; i > 0; i--) {
        bg_fp32_versor_turn_back2(&versors[i - 1], &result, &result);
    }

#ifdef _WIN64
    end = GetTickCount64();

    printf("Time: %lld\n", end - start);
#else
    clock_gettime(CLOCK_REALTIME, &end);

    printf("Time: %lf\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001);
#endif // _WIN64


    print_vector(&initial);
    print_vector(&result);

    free(versors);
    return 0;
}
*/

int main()
{
    const unsigned int amount = 1000000;

#ifdef _WIN64
    ULONGLONG now, start, end;
    now = GetTickCount64();
    srand((unsigned int)(now & 0xfffffff));
#else
    struct timespec now, start, end;
    clock_gettime(0, &now);
    srand((unsigned int)(now.tv_nsec & 0xfffffff));
#endif // _WIN64

    BgFP32Versor * versors1 = make_random_versors(amount);

    if (versors1 == 0) {
        printf("Cannot allocate memory for versors1");
        return 0;
    }

    BgFP32Versor * versors2 = make_random_versors(amount);

    if (versors2 == 0) {
        printf("Cannot allocate memory for versors2");
        free(versors1);
        return 0;
    }

    BgFP32Versor * results = make_zero_versors(amount);

    if (results == 0) {
        printf("Cannot allocate memory for results");
        free(versors2);
        free(versors1);
        return 0;
    }

#ifdef _WIN64
    end = GetTickCount64();
    printf("Setup time: %lld\n", end - now);

    start = GetTickCount64();
#else
    clock_gettime(CLOCK_REALTIME, &end);
    printf("Time: %lf\n", (end.tv_sec - now.tv_sec) * 1000.0 + (end.tv_nsec - now.tv_nsec) * 0.000001);

    clock_gettime(CLOCK_REALTIME, &start);
#endif // _WIN64
    for (int j = 0; j < 1000; j++) {
        for (unsigned int i = 0; i < amount; i++) {
            bg_fp32_versor_combine(&versors1[i], &versors2[i], &results[i]);
        }
    }

#ifdef _WIN64
    end = GetTickCount64();

    printf("Time: %lld\n", end - start);
#else
    clock_gettime(CLOCK_REALTIME, &end);

    printf("Time: %lf\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001);
#endif // _WIN64

    print_versor(versors1 + 10);
    print_versor(versors2 + 10);
    print_versor(results + 10);

    free(results);
    free(versors2);
    free(versors1);
    return 0;
}