#include <math.h>

#include "angle.h"
#include "versor.h"

const BgcVersorFP32 BGC_IDLE_VERSOR_FP32 = { 1.0f, 0.0f, 0.0f, 0.0f };

const BgcVersorFP64 BGC_IDLE_VERSOR_FP64 = { 1.0, 0.0, 0.0, 0.0 };

extern inline void bgc_versor_reset_fp32(BgcVersorFP32* versor);
extern inline void bgc_versor_reset_fp64(BgcVersorFP64* versor);

extern inline void bgc_versor_set_values_fp32(const float s0, const float x1, const float x2, const float x3, BgcVersorFP32* versor);
extern inline void bgc_versor_set_values_fp64(const double s0, const double x1, const double x2, const double x3, BgcVersorFP64* versor);

extern inline void bgc_versor_set_rotation_fp32(const BgcRotation3FP32* rotation, BgcVersorFP32* result);
extern inline void bgc_versor_set_rotation_fp64(const BgcRotation3FP64* rotation, BgcVersorFP64* result);

extern inline void bgc_versor_copy_fp32(const BgcVersorFP32* source, BgcVersorFP32* destination);
extern inline void bgc_versor_copy_fp64(const BgcVersorFP64* source, BgcVersorFP64* destination);

extern inline void bgc_versor_swap_fp32(BgcVersorFP32* versor1, BgcVersorFP32* versor2);
extern inline void bgc_versor_swap_fp64(BgcVersorFP64* versor1, BgcVersorFP64* versor2);

extern inline int bgc_versor_is_identity_fp32(const BgcVersorFP32* versor);
extern inline int bgc_versor_is_identity_fp64(const BgcVersorFP64* versor);

extern inline void bgc_versor_convert_fp64_to_fp32(const BgcVersorFP64* source, BgcVersorFP32* destination);
extern inline void bgc_versor_convert_fp32_to_fp64(const BgcVersorFP32* source, BgcVersorFP64* destination);

extern inline void bgc_versor_shorten_fp32(const BgcVersorFP32* versor, BgcVersorFP32* shortened);
extern inline void bgc_versor_shorten_fp64(const BgcVersorFP64* versor, BgcVersorFP64* shortened);

extern inline void bgc_versor_invert_fp32(const BgcVersorFP32* versor, BgcVersorFP32* inverted);
extern inline void bgc_versor_invert_fp64(const BgcVersorFP64* versor, BgcVersorFP64* inverted);

extern inline void bgc_versor_combine_fp32(const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result);
extern inline void bgc_versor_combine_fp64(const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result);

extern inline void bgc_versor_combine3_fp32(const BgcVersorFP32* third, const BgcVersorFP32* second, const BgcVersorFP32* first, BgcVersorFP32* result);
extern inline void bgc_versor_combine3_fp64(const BgcVersorFP64* third, const BgcVersorFP64* second, const BgcVersorFP64* first, BgcVersorFP64* result);

extern inline void bgc_versor_exclude_fp32(const BgcVersorFP32* base, const BgcVersorFP32* excludant, BgcVersorFP32* difference);
extern inline void bgc_versor_exclude_fp64(const BgcVersorFP64* base, const BgcVersorFP64* excludant, BgcVersorFP64* difference);

extern inline void bgc_versor_get_rotation_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix);
extern inline void bgc_versor_get_rotation_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix);

extern inline void bgc_versor_get_reverse_matrix_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* matrix);
extern inline void bgc_versor_get_reverse_matrix_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* matrix);

extern inline void bgc_versor_get_both_matrixes_fp32(const BgcVersorFP32* versor, BgcMatrix3x3FP32* rotation, BgcMatrix3x3FP32* reverse);
extern inline void bgc_versor_get_both_matrixes_fp64(const BgcVersorFP64* versor, BgcMatrix3x3FP64* rotation, BgcMatrix3x3FP64* reverse);

extern inline void bgc_versor_turn_vector_fp32(const BgcVersorFP32* versor, const BgcVector3FP32* vector, BgcVector3FP32* result);
extern inline void bgc_versor_turn_vector_fp64(const BgcVersorFP64* versor, const BgcVector3FP64* vector, BgcVector3FP64* result);

extern inline void bgc_versor_turn_vector_back_fp32(const BgcVersorFP32* versor, const BgcVector3FP32* vector, BgcVector3FP32* result);
extern inline void bgc_versor_turn_vector_back_fp64(const BgcVersorFP64* versor, const BgcVector3FP64* vector, BgcVector3FP64* result);

extern inline int bgc_versor_are_close_fp32(const BgcVersorFP32* versor1, const BgcVersorFP32* versor2);
extern inline int bgc_versor_are_close_fp64(const BgcVersorFP64* versor1, const BgcVersorFP64* versor2);

// ================= Normalize ================== //

void _bgc_versor_normalize_fp32(const float square_modulus, _BgcDarkTwinVersorFP32* twin)
{
    // (square_modulus != square_modulus) is true when square_modulus is NaN

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32 || square_modulus != square_modulus) {
        twin->s0 = 1.0f;
        twin->x1 = 0.0f;
        twin->x2 = 0.0f;
        twin->x3 = 0.0f;
        return;
    }

    const float multiplier = sqrtf(1.0f / square_modulus);

    twin->s0 *= multiplier;
    twin->x1 *= multiplier;
    twin->x2 *= multiplier;
    twin->x3 *= multiplier;
}

void _bgc_versor_normalize_fp64(const double square_modulus, _BgcDarkTwinVersorFP64* twin)
{
    // (square_modulus != square_modulus) is true when square_modulus is NaN

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64 || square_modulus != square_modulus) {
        twin->s0 = 1.0;
        twin->x1 = 0.0;
        twin->x2 = 0.0;
        twin->x3 = 0.0;
        return;
    }

    const double multiplier = sqrt(1.0 / square_modulus);

    twin->s0 *= multiplier;
    twin->x1 *= multiplier;
    twin->x2 *= multiplier;
    twin->x3 *= multiplier;
}

// ================== Set Turn ================== //

void bgc_versor_set_turn_fp32(const float x1, const float x2, const float x3, const float angle, const BgcAngleUnitEnum unit, BgcVersorFP32* result)
{
    const float square_vector = x1 * x1 + x2 * x2 + x3 * x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32) {
        bgc_versor_reset_fp32(result);
        return;
    }

    const float half_angle = bgc_angle_to_radians_fp32(0.5f * angle, unit);

    const float sine = sinf(half_angle);

    if (bgc_is_zero_fp32(sine)) {
        bgc_versor_reset_fp32(result);
        return;
    }

    const float multiplier = sine / sqrtf(square_vector);

    bgc_versor_set_values_fp32(cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
}

void bgc_versor_set_turn_fp64(const double x1, const double x2, const double x3, const double angle, const BgcAngleUnitEnum unit, BgcVersorFP64* result)
{
    const double square_vector = x1 * x1 + x2 * x2 + x3 * x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64) {
        bgc_versor_reset_fp64(result);
        return;
    }

    const double half_angle = bgc_angle_to_radians_fp64(0.5 * angle, unit);

    const double sine = sin(half_angle);

    if (bgc_is_zero_fp64(sine)) {
        bgc_versor_reset_fp64(result);
        return;
    }

    const double multiplier = sine / sqrt(square_vector);

    bgc_versor_set_values_fp64(cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier, result);
}

// ========= Make Direction Difference ========== //

inline int _bgc_versor_make_direction_turn_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, const float square_modulus_product, BgcVersorFP32* result)
{
    BgcVector3FP32 orthogonal_axis;

    bgc_vector3_get_cross_product_fp32(start, end, &orthogonal_axis);

    const float scalar_product = bgc_vector3_get_scalar_product_fp32(start, end);
    const float square_modulus = bgc_vector3_get_square_modulus_fp32(&orthogonal_axis);
    const float square_sine = square_modulus / square_modulus_product;

    if (square_sine > BGC_SQUARE_EPSYLON_FP32) {
        const float cosine = scalar_product / sqrtf(square_modulus_product);
        const float angle = 0.5f * atan2f(sqrtf(square_sine), cosine);

        const float multiplier = sinf(angle) * sqrtf(1.0f / square_modulus);

        bgc_versor_set_values_fp32(cosf(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
        return BGC_SOME_TURN;
    }

    if (scalar_product < 0.0f) {
        return BGC_OPPOSITE;
    }

    bgc_versor_reset_fp32(result);
    return BGC_ZERO_TURN;
}

inline int _bgc_versor_make_direction_turn_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, const double square_modulus_product, BgcVersorFP64* result)
{
    BgcVector3FP64 orthogonal_axis;

    bgc_vector3_get_cross_product_fp64(start, end, &orthogonal_axis);

    const double scalar_product = bgc_vector3_get_scalar_product_fp64(start, end);
    const double square_modulus = bgc_vector3_get_square_modulus_fp64(&orthogonal_axis);
    const double square_sine = square_modulus / square_modulus_product;

    if (square_sine > BGC_SQUARE_EPSYLON_FP64) {
        const double cosine = scalar_product / sqrt(square_modulus_product);
        const double angle = 0.5 * atan2(sqrt(square_sine), cosine);

        const double multiplier = sin(angle) * sqrt(1.0f / square_modulus);

        bgc_versor_set_values_fp64(cos(angle), orthogonal_axis.x1 * multiplier, orthogonal_axis.x2 * multiplier, orthogonal_axis.x3 * multiplier, result);
        return BGC_SOME_TURN;
    }

    if (scalar_product < 0.0) {
        return BGC_OPPOSITE;
    }

    bgc_versor_reset_fp64(result);
    return BGC_ZERO_TURN;
}

int bgc_versor_make_direction_difference_fp32(const BgcVector3FP32* start, const BgcVector3FP32* end, BgcVersorFP32* result)
{
    const float start_square_modulus = bgc_vector3_get_square_modulus_fp32(start);
    const float end_square_modulus = bgc_vector3_get_square_modulus_fp32(end);

    if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP32 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
        bgc_versor_reset_fp32(result);
        return BGC_ZERO_TURN;
    }

    return _bgc_versor_make_direction_turn_fp32(start, end, start_square_modulus * end_square_modulus, result);
}

int bgc_versor_make_direction_difference_fp64(const BgcVector3FP64* start, const BgcVector3FP64* end, BgcVersorFP64* result)
{
    const double start_square_modulus = bgc_vector3_get_square_modulus_fp64(start);
    const double end_square_modulus = bgc_vector3_get_square_modulus_fp64(end);

    if (start_square_modulus <= BGC_SQUARE_EPSYLON_FP64 || end_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
        bgc_versor_reset_fp64(result);
        return BGC_ZERO_TURN;
    }

    return _bgc_versor_make_direction_turn_fp64(start, end, start_square_modulus * end_square_modulus, result);
}

// =============== Set Directions =============== //

inline int _bgc_versor_validate_basis_fp32(const float primary_square_modulus, const float auxiliary_square_modulus, const float orthogonal_square_modulus)
{
    if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
        //TODO: add error code for: primary_vector is zero
        return BGC_FAILED;
    }

    if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
        //TODO: add error code for: auxiliary_vector is zero
        return BGC_FAILED;
    }

    if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP32) {
        //TODO: add error code for: primary_vector and auxiliary_vector are parallel
        return BGC_FAILED;
    }

    return BGC_SUCCESS;
}

inline int _bgc_versor_validate_basis_fp64(const double primary_square_modulus, const double auxiliary_square_modulus, const double orthogonal_square_modulus)
{
    if (primary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
        //TODO: add error code for: primary_vector is zero
        return BGC_FAILED;
    }

    if (auxiliary_square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
        //TODO: add error code for: auxiliary_vector is zero
        return BGC_FAILED;
    }

    if (orthogonal_square_modulus / (primary_square_modulus * auxiliary_square_modulus) <= BGC_SQUARE_EPSYLON_FP64) {
        //TODO: add error code for: primary_vector and auxiliary_vector are parallel
        return BGC_FAILED;
    }

    return BGC_SUCCESS;
}

int bgc_versor_make_basis_difference_fp32(
    const BgcVector3FP32* initial_primary_direction,
    const BgcVector3FP32* initial_auxiliary_direction,
    const BgcVector3FP32* final_primary_direction,
    const BgcVector3FP32* final_auxiliary_direction,
    BgcVersorFP32* result
)
{
    BgcVector3FP32 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;

    // Step 1: Validate initial basis:
    bgc_vector3_get_cross_product_fp32(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);

    const float initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_primary_direction);
    const float initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(initial_auxiliary_direction);
    const float initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&initial_orthogonal_direction);

    const int initial_basis_valudation = _bgc_versor_validate_basis_fp32(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);

    if (initial_basis_valudation != BGC_SUCCESS) {
        return initial_basis_valudation;
    }

    // Step 1: Validate final basis:
    bgc_vector3_get_cross_product_fp32(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);

    const float final_primary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_primary_direction);
    const float final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp32(final_auxiliary_direction);
    const float final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp32(&final_orthogonal_direction);

    const int final_basis_valudation = _bgc_versor_validate_basis_fp32(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);

    if (final_basis_valudation != BGC_SUCCESS) {
        return final_basis_valudation;
    }

    // Step 3: Validate normalize orthogonal vectors:
    bgc_vector3_divide_fp32(&initial_orthogonal_direction, sqrtf(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
    bgc_vector3_divide_fp32(&final_orthogonal_direction, sqrtf(final_orthogonal_square_modulus), &final_orthogonal_direction);

    BgcVersorFP32 turn1, turn2;

    // Step 4: Find turn1
    int turn1_code = _bgc_versor_make_direction_turn_fp32(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);

    if (turn1_code == BGC_OPPOSITE) {
        bgc_versor_set_values_fp32(0.0f, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
    }

    bgc_versor_turn_vector_fp32(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);

    // Step 5: Find turn2:
    int turn2_code = _bgc_versor_make_direction_turn_fp32(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);

    if (turn2_code == BGC_OPPOSITE) {
        const float turn2_multiplier = sqrtf(1.0f / final_primary_square_modulus);

        bgc_versor_set_values_fp32(0.0f,
            final_primary_direction->x1 * turn2_multiplier,
            final_primary_direction->x2 * turn2_multiplier,
            final_primary_direction->x3 * turn2_multiplier,
            &turn2
        );
    }

    // Step 6: Combine turn1 and turn2:
    bgc_versor_combine_fp32(&turn2, &turn1, result);

    return BGC_SUCCESS;
}

int bgc_versor_make_basis_difference_fp64(
    const BgcVector3FP64* initial_primary_direction,
    const BgcVector3FP64* initial_auxiliary_direction,
    const BgcVector3FP64* final_primary_direction,
    const BgcVector3FP64* final_auxiliary_direction,
    BgcVersorFP64* result
)
{
    BgcVector3FP64 initial_orthogonal_direction, turned_orthogonal_direction, final_orthogonal_direction;

    // Step 1: Validate initial basis:
    bgc_vector3_get_cross_product_fp64(initial_primary_direction, initial_auxiliary_direction, &initial_orthogonal_direction);

    const double initial_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_primary_direction);
    const double initial_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(initial_auxiliary_direction);
    const double initial_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&initial_orthogonal_direction);

    const int initial_basis_valudation = _bgc_versor_validate_basis_fp64(initial_primary_square_modulus, initial_auxiliary_square_modulus, initial_orthogonal_square_modulus);

    if (initial_basis_valudation != BGC_SUCCESS) {
        return initial_basis_valudation;
    }

    // Step 1: Validate final basis:
    bgc_vector3_get_cross_product_fp64(final_primary_direction, final_auxiliary_direction, &final_orthogonal_direction);

    const double final_primary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_primary_direction);
    const double final_auxiliary_square_modulus = bgc_vector3_get_square_modulus_fp64(final_auxiliary_direction);
    const double final_orthogonal_square_modulus = bgc_vector3_get_square_modulus_fp64(&final_orthogonal_direction);

    const int final_basis_valudation = _bgc_versor_validate_basis_fp64(final_primary_square_modulus, final_auxiliary_square_modulus, final_orthogonal_square_modulus);

    if (final_basis_valudation != BGC_SUCCESS) {
        return final_basis_valudation;
    }

    // Step 3: Validate normalize orthogonal vectors:
    bgc_vector3_divide_fp64(&initial_orthogonal_direction, sqrt(initial_orthogonal_square_modulus), &initial_orthogonal_direction);
    bgc_vector3_divide_fp64(&final_orthogonal_direction, sqrt(final_orthogonal_square_modulus), &final_orthogonal_direction);

    BgcVersorFP64 turn1, turn2;

    // Step 4: Find turn1
    int turn1_code = _bgc_versor_make_direction_turn_fp64(initial_primary_direction, final_primary_direction, initial_primary_square_modulus * final_primary_square_modulus, &turn1);

    if (turn1_code == BGC_OPPOSITE) {
        bgc_versor_set_values_fp64(0.0, initial_orthogonal_direction.x1, initial_orthogonal_direction.x2, initial_orthogonal_direction.x3, &turn1);
    }

    bgc_versor_turn_vector_fp64(&turn1, &initial_orthogonal_direction, &turned_orthogonal_direction);

    // Step 5: Find turn2:
    int turn2_code = _bgc_versor_make_direction_turn_fp64(&turned_orthogonal_direction, &final_orthogonal_direction, 1.0f, &turn2);

    if (turn2_code == BGC_OPPOSITE) {
        const double turn2_multiplier = sqrt(1.0 / final_primary_square_modulus);

        bgc_versor_set_values_fp64(0.0,
            final_primary_direction->x1 * turn2_multiplier,
            final_primary_direction->x2 * turn2_multiplier,
            final_primary_direction->x3 * turn2_multiplier,
            &turn2
        );
    }

    // Step 6: Combine turn1 and turn2:
    bgc_versor_combine_fp64(&turn2, &turn1, result);

    return BGC_SUCCESS;
}

// =============== Get Exponation =============== //

void bgc_versor_get_exponation_fp32(const BgcVersorFP32* base, const float exponent, BgcVersorFP32* power)
{
    const float square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
        bgc_versor_reset_fp32(power);
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

    const float angle = atan2f(vector_modulus, base->s0) * exponent;

    const float multiplier = sinf(angle) / vector_modulus;

    bgc_versor_set_values_fp32(cosf(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power);
}

void bgc_versor_get_exponation_fp64(const BgcVersorFP64* base, const double exponent, BgcVersorFP64* power)
{
    const double square_vector = base->x1 * base->x1 + base->x2 * base->x2 + base->x3 * base->x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
        bgc_versor_reset_fp64(power);
        return;
    }

    const double vector_modulus = sqrt(square_vector);

    const double angle = atan2(vector_modulus, base->s0) * exponent;

    const double multiplier = sin(angle) / vector_modulus;

    bgc_versor_set_values_fp64(cos(angle), base->x1 * multiplier, base->x2 * multiplier, base->x3 * multiplier, power);
}

// ============ Sphere Interpolation ============ //

void bgc_versor_spherically_interpolate_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, const float phase, BgcVersorFP32* result)
{
    const float delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3);
    const float delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3);
    const float delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1);
    const float delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2);

    const float square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;

    // square_vector != square_vector means checking for NaN value at square_vector
    if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
        bgc_versor_copy_fp32(end, result);
        return;
    }

    // Calculating of the turning which fits the phase:
    const float vector_modulus = sqrtf(square_vector);
    const float angle = atan2f(vector_modulus, delta_s0) * phase;
    const float multiplier = sinf(angle) / vector_modulus;

    const float turn_s0 = cosf(angle);
    const float turn_x1 = delta_x1 * multiplier;
    const float turn_x2 = delta_x2 * multiplier;
    const float turn_x3 = delta_x3 * multiplier;

    // Combining of starting orientation with the turning
    bgc_versor_set_values_fp32(
        (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3),
        (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3),
        (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1),
        (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2),
        result
    );
}

void bgc_versor_spherically_interpolate_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, const double phase, BgcVersorFP64* result)
{
    const double delta_s0 = (end->s0 * start->s0 + end->x1 * start->x1) + (end->x2 * start->x2 + end->x3 * start->x3);
    const double delta_x1 = (end->x1 * start->s0 + end->x3 * start->x2) - (end->s0 * start->x1 + end->x2 * start->x3);
    const double delta_x2 = (end->x2 * start->s0 + end->x1 * start->x3) - (end->s0 * start->x2 + end->x3 * start->x1);
    const double delta_x3 = (end->x3 * start->s0 + end->x2 * start->x1) - (end->s0 * start->x3 + end->x1 * start->x2);

    const double square_vector = delta_x1 * delta_x1 + delta_x2 * delta_x2 + delta_x3 * delta_x3;

    // square_vector != square_vector means checking for NaN value at square_vector
    if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
        bgc_versor_copy_fp64(end, result);
        return;
    }

    // Calculating of the turning which fits the phase:
    const double vector_modulus = sqrt(square_vector);
    const double angle = atan2(vector_modulus, delta_s0) * phase;
    const double multiplier = sin(angle) / vector_modulus;

    const double turn_s0 = cos(angle);
    const double turn_x1 = delta_x1 * multiplier;
    const double turn_x2 = delta_x2 * multiplier;
    const double turn_x3 = delta_x3 * multiplier;

    // Combining of starting orientation with the turning
    bgc_versor_set_values_fp64(
        (turn_s0 * start->s0 - turn_x1 * start->x1) - (turn_x2 * start->x2 + turn_x3 * start->x3),
        (turn_x1 * start->s0 + turn_s0 * start->x1) - (turn_x3 * start->x2 - turn_x2 * start->x3),
        (turn_x2 * start->s0 + turn_s0 * start->x2) - (turn_x1 * start->x3 - turn_x3 * start->x1),
        (turn_x3 * start->s0 + turn_s0 * start->x3) - (turn_x2 * start->x1 - turn_x1 * start->x2),
        result
    );
}

// ================ Get Rotation ================ //

void bgc_versor_get_rotation_fp32(const BgcVersorFP32* versor, BgcRotation3FP32* result)
{
    const float square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP32) {
        bgc_rotation3_reset_fp32(result);
        return;
    }

    const float vector_modulus = sqrtf(square_modulus);

    const float multiplier = 1.0f / vector_modulus;

    result->radians = 2.0f * atan2f(vector_modulus, versor->s0);

    result->axis.x1 = versor->x1 * multiplier;
    result->axis.x2 = versor->x2 * multiplier;
    result->axis.x3 = versor->x3 * multiplier;
}

void bgc_versor_get_rotation_fp64(const BgcVersorFP64* versor, BgcRotation3FP64* result)
{
    const double square_modulus = versor->x1 * versor->x1 + versor->x2 * versor->x2 + versor->x3 * versor->x3;

    if (square_modulus <= BGC_SQUARE_EPSYLON_FP64) {
        bgc_rotation3_reset_fp64(result);
        return;
    }

    const double vector_modulus = sqrt(square_modulus);

    const double multiplier = 1.0 / vector_modulus;

    result->radians = 2.0 * atan2(vector_modulus, versor->s0);

    result->axis.x1 = versor->x1 * multiplier;
    result->axis.x2 = versor->x2 * multiplier;
    result->axis.x3 = versor->x3 * multiplier;
}