#include "./versor_set_values.h"

#include <math.h>

#include "./../../helpers.h"

// ==================== FP32 ==================== //

static const int _TEST_FP32_VERSOR_DATA_AMOUNT = 4;
static const BGC_FP32_Quaternion _TEST_FP32_VERSOR_DATA_LIST[] = {
    { 1.0f, 2.0f, 3.0f, 4.0f },
    { 4.0f, 3.0f, 2.0f, 1.0f },
    { -1.0f, 0.0f, 0.0f, 0.0f },
    { 1.0f, 0.0f, 1.0f, 0.0f }
};

void test_versor_set_values_fp32()
{
    float versor_module, ratio;
    BGC_FP32_Versor versor;

    print_testing_name("bgc_fp32_versor_make");

    for (int i = 0; i < _TEST_FP32_VERSOR_DATA_AMOUNT; i++) {
        bgc_fp32_versor_make(
            _TEST_FP32_VERSOR_DATA_LIST[i].s0,
            _TEST_FP32_VERSOR_DATA_LIST[i].x1,
            _TEST_FP32_VERSOR_DATA_LIST[i].x2,
            _TEST_FP32_VERSOR_DATA_LIST[i].x3,
            &versor
        );

        versor_module = sqrtf(versor._s0 * versor._s0 + versor._x1 * versor._x1 + versor._x2 * versor._x2 + versor._x3 * versor._x3);

        if (!bgc_fp32_is_unit(versor_module)) {
            print_testing_error("Versor module is not equal to one.");
            return;
        }

        if (bgc_fp32_is_zero(_TEST_FP32_VERSOR_DATA_LIST[i].s0)) {
            continue;
        }

        ratio = _TEST_FP32_VERSOR_DATA_LIST[i].s0 / versor._s0;

        if (!bgc_fp32_is_zero(_TEST_FP32_VERSOR_DATA_LIST[i].x1) && !bgc_fp32_are_close(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x1 / versor._x1)) {
            print_testing_error("Versor was not normalized proportionally (x1).");
            return;
        }

        if (!bgc_fp32_is_zero(_TEST_FP32_VERSOR_DATA_LIST[i].x2) && !bgc_fp32_are_close(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x2 / versor._x2)) {
            print_testing_error("Versor was not normalized proportionally (x2).");
            return;
        }

        if (!bgc_fp32_is_zero(_TEST_FP32_VERSOR_DATA_LIST[i].x3) && !bgc_fp32_are_close(ratio, _TEST_FP32_VERSOR_DATA_LIST[i].x3 / versor._x3)) {
            print_testing_error("Versor was not normalized proportionally (x3).");
            return;
        }
    }

    print_testing_success();
}

// ==================== FP64 ==================== //

static const int _TEST_FP64_VERSOR_DATA_AMOUNT = 4;
static const BGC_FP64_Quaternion _TEST_FP64_VERSOR_DATA_LIST[] = {
    { 1.0, 2.0, 3.0, 4.0 },
    { 4.0, 3.0, 2.0, 1.0 },
    { -1.0, 0.0, 0.0, 0.0 },
    { 1.0, 0.0, 1.0, 0.0 }
};

void test_versor_set_values_fp64()
{
    double versor_module, ratio;
    BGC_FP64_Versor versor;

    print_testing_name("bgc_fp64_versor_make");

    for (int i = 0; i < _TEST_FP64_VERSOR_DATA_AMOUNT; i++) {
        bgc_fp64_versor_make(
            _TEST_FP64_VERSOR_DATA_LIST[i].s0,
            _TEST_FP64_VERSOR_DATA_LIST[i].x1,
            _TEST_FP64_VERSOR_DATA_LIST[i].x2,
            _TEST_FP64_VERSOR_DATA_LIST[i].x3,
            &versor
        );

        versor_module = sqrt(versor._s0 * versor._s0 + versor._x1 * versor._x1 + versor._x2 * versor._x2 + versor._x3 * versor._x3);

        if (!bgc_fp64_is_unit(versor_module)) {
            print_testing_error("Versor module is not equal to one.");
            return;
        }

        if (bgc_fp64_is_zero(_TEST_FP64_VERSOR_DATA_LIST[i].s0)) {
            continue;
        }

        ratio = _TEST_FP64_VERSOR_DATA_LIST[i].s0 / versor._s0;

        if (!bgc_fp64_is_zero(_TEST_FP64_VERSOR_DATA_LIST[i].x1) && !bgc_fp64_are_close(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x1 / versor._x1)) {
            print_testing_error("Versor was not normalized proportionally (x1).");
            return;
        }

        if (!bgc_fp64_is_zero(_TEST_FP64_VERSOR_DATA_LIST[i].x2) && !bgc_fp64_are_close(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x2 / versor._x2)) {
            print_testing_error("Versor was not normalized proportionally (x2).");
            return;
        }

        if (!bgc_fp64_is_zero(_TEST_FP64_VERSOR_DATA_LIST[i].x3) && !bgc_fp64_are_close(ratio, _TEST_FP64_VERSOR_DATA_LIST[i].x3 / versor._x3)) {
            print_testing_error("Versor was not normalized proportionally (x3).");
            return;
        }
    }

    print_testing_success();
}

void test_versor_set_values()
{
    test_versor_set_values_fp32();
    test_versor_set_values_fp64();
}