#include <math.h>

#include "angle.h"
#include "turn3.h"

const BGC_FP32_Turn3 BGC_FP32_IDLE_TURN3 = {{ 1.0f, 0.0f, 0.0f, 0.0f }};

const BGC_FP64_Turn3 BGC_FP64_IDLE_TURN3 = {{ 1.0, 0.0, 0.0, 0.0 }};

extern inline void bgc_fp32_turn3_reset(BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_set_raw_values(BGC_FP32_Turn3* turn, const float s0, const float x1, const float x2, const float x3);
extern inline void bgc_fp64_turn3_set_raw_values(BGC_FP64_Turn3* turn, const double s0, const double x1, const double x2, const double x3);

extern inline void bgc_fp32_turn3_get_quaternion(BGC_FP32_Quaternion* quaternion, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_quaternion(BGC_FP64_Quaternion* quaternion, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_set_quaternion(BGC_FP32_Turn3* turn, const BGC_FP32_Quaternion* quaternion);
extern inline void bgc_fp64_turn3_set_quaternion(BGC_FP64_Turn3* turn, const BGC_FP64_Quaternion* quaternion);

extern inline void bgc_fp32_turn3_copy(BGC_FP32_Turn3* destination, const BGC_FP32_Turn3* source);
extern inline void bgc_fp64_turn3_copy(BGC_FP64_Turn3* destination, const BGC_FP64_Turn3* source);

extern inline void bgc_fp32_turn3_swap(BGC_FP32_Turn3* turn1, BGC_FP32_Turn3* turn2);
extern inline void bgc_fp64_turn3_swap(BGC_FP64_Turn3* turn1, BGC_FP64_Turn3* turn2);

extern inline int bgc_fp32_turn3_is_idle(const BGC_FP32_Turn3* turn);
extern inline int bgc_fp64_turn3_is_idle(const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_convert_to_fp64(BGC_FP64_Turn3* destination, const BGC_FP32_Turn3* source);
extern inline void bgc_fp64_turn3_convert_to_fp32(BGC_FP32_Turn3* destination, const BGC_FP64_Turn3* source);

extern inline void bgc_fp32_turn3_shorten(BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_shorten(BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_get_shortened(BGC_FP32_Turn3* shortened, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_shortened(BGC_FP64_Turn3* shortened, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_alternate(BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_alternate(BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_get_alternative(BGC_FP32_Turn3* alternative, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_alternative(BGC_FP64_Turn3* alternative, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_revert(BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_revert(BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_get_reverse(BGC_FP32_Turn3* inverse, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_reverse(BGC_FP64_Turn3* inverse, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* combination, const BGC_FP32_Turn3* first, const BGC_FP32_Turn3* second);
extern inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* combination, const BGC_FP64_Turn3* first, const BGC_FP64_Turn3* second);

extern inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* combination, const BGC_FP32_Turn3* first, const BGC_FP32_Turn3* second, const BGC_FP32_Turn3* third);
extern inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* combination, const BGC_FP64_Turn3* first, const BGC_FP64_Turn3* second, const BGC_FP64_Turn3* third);

extern inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* difference, const BGC_FP32_Turn3* base, const BGC_FP32_Turn3* excludant);
extern inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* difference, const BGC_FP64_Turn3* base, const BGC_FP64_Turn3* excludant);

extern inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_get_reverse_matrix(BGC_FP32_Matrix3x3* matrix, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_reverse_matrix(BGC_FP64_Matrix3x3* matrix, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_get_both_matrices(BGC_FP32_Matrix3x3* rotation, BGC_FP32_Matrix3x3* reverse, const BGC_FP32_Turn3* turn);
extern inline void bgc_fp64_turn3_get_both_matrices(BGC_FP64_Matrix3x3* rotation, BGC_FP64_Matrix3x3* reverse, const BGC_FP64_Turn3* turn);

extern inline void bgc_fp32_turn3_vector(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* vector);
extern inline void bgc_fp64_turn3_vector(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* vector);

extern inline void bgc_fp32_turn3_vector_back(BGC_FP32_Vector3* turned_vector, const BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* vector);
extern inline void bgc_fp64_turn3_vector_back(BGC_FP64_Vector3* turned_vector, const BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* vector);

extern inline int bgc_fp32_turn3_are_close(const BGC_FP32_Turn3* turn1, const BGC_FP32_Turn3* turn2);
extern inline int bgc_fp64_turn3_are_close(const BGC_FP64_Turn3* turn1, const BGC_FP64_Turn3* turn2);

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

void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* turn, const float square_modulus)
{
    if (square_modulus <= BGC_FP32_SQUARE_EPSILON || isnan(square_modulus)) {
        bgc_fp32_turn3_reset(turn);
        return;
    }

    bgc_fp32_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrtf(1.0f / square_modulus));
}

void _bgc_fp64_turn3_normalize(BGC_FP64_Turn3* turn, const double square_modulus)
{
    if (square_modulus <= BGC_FP64_SQUARE_EPSILON || isnan(square_modulus)) {
        bgc_fp64_turn3_reset(turn);
        return;
    }

    bgc_fp64_quaternion_multiply_by_real(&turn->_versor, &turn->_versor, sqrt(1.0 / square_modulus));
}


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

float bgc_fp32_turn3_get_rotation(BGC_FP32_Vector3* axis, const BGC_FP32_Turn3* turn, const int angle_unit)
{
    const float square_vector_modulus = turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3;

    if (square_vector_modulus <= BGC_FP32_SQUARE_EPSILON) {
        bgc_fp32_vector3_reset(axis);
        return 0.0f;
    }

    const float vector_modulus = sqrtf(square_vector_modulus);

    const float multiplier = 1.0f / vector_modulus;

    axis->x1 = turn->_versor.x1 * multiplier;
    axis->x2 = turn->_versor.x2 * multiplier;
    axis->x3 = turn->_versor.x3 * multiplier;

    return 2.0f * atan2f(vector_modulus, turn->_versor.s0);
}

double bgc_fp64_turn3_get_rotation(BGC_FP64_Vector3* axis, const BGC_FP64_Turn3* turn, const int angle_unit)
{
    const double square_vector_modulus = turn->_versor.x1 * turn->_versor.x1 + turn->_versor.x2 * turn->_versor.x2 + turn->_versor.x3 * turn->_versor.x3;

    if (square_vector_modulus <= BGC_FP64_SQUARE_EPSILON) {
        bgc_fp64_vector3_reset(axis);
        return 0.0;
    }

    const double vector_modulus = sqrt(square_vector_modulus);

    const double multiplier = 1.0 / vector_modulus;

    axis->x1 = turn->_versor.x1 * multiplier;
    axis->x2 = turn->_versor.x2 * multiplier;
    axis->x3 = turn->_versor.x3 * multiplier;

    return 2.0 * atan2(vector_modulus, turn->_versor.s0);
}

// ================ Set Rotation ================ //

void bgc_fp32_turn3_set_rotation(BGC_FP32_Turn3* turn, const float x1, const float x2, const float x3, const float angle, const int angle_unit)
{
    const float square_vector = x1 * x1 + x2 * x2 + x3 * x3;

    if (square_vector <= BGC_FP32_SQUARE_EPSILON) {
        bgc_fp32_turn3_reset(turn);
        return;
    }

    const float half_angle = bgc_fp32_angle_to_radians(0.5f * angle, angle_unit);

    const float sine = sinf(half_angle);

    if (bgc_fp32_is_zero(sine)) {
        bgc_fp32_turn3_reset(turn);
        return;
    }

    const float multiplier = sine / sqrtf(square_vector);

    bgc_fp32_quaternion_make(&turn->_versor, cosf(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);

    const float square_modulus = bgc_fp32_quaternion_get_square_modulus(&turn->_versor);

    if (!bgc_fp32_is_square_unit(square_modulus)) {
        _bgc_fp32_turn3_normalize(turn, square_modulus);
    }
}

void bgc_fp64_turn3_set_rotation(BGC_FP64_Turn3* turn, const double x1, const double x2, const double x3, const double angle, const int angle_unit)
{
    const double square_vector = x1 * x1 + x2 * x2 + x3 * x3;

    if (square_vector <= BGC_FP64_SQUARE_EPSILON) {
        bgc_fp64_turn3_reset(turn);
        return;
    }

    const double half_angle = bgc_fp64_angle_to_radians(0.5 * angle, angle_unit);

    const double sine = sin(half_angle);

    if (bgc_fp64_is_zero(sine)) {
        bgc_fp64_turn3_reset(turn);
        return;
    }

    const double multiplier = sine / sqrt(square_vector);

    bgc_fp64_quaternion_make(&turn->_versor, cos(half_angle), x1 * multiplier, x2 * multiplier, x3 * multiplier);

    const double square_modulus = bgc_fp64_quaternion_get_square_modulus(&turn->_versor);

    if (!bgc_fp64_is_square_unit(square_modulus)) {
        _bgc_fp64_turn3_normalize(turn, square_modulus);
    }
}

// ========= Find Direction Difference ========== //

int bgc_fp32_turn3_find_direction_difference(BGC_FP32_Turn3* turn, const BGC_FP32_Vector3* first, const BGC_FP32_Vector3* second)
{
    const float first_square_modulus = bgc_fp32_vector3_get_square_modulus(first);

    bgc_fp32_turn3_reset(turn);

    if (first_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
    }

    const float second_square_modulus = bgc_fp32_vector3_get_square_modulus(second);

    if (second_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
    }

    BGC_FP32_Vector3 axis;

    bgc_fp32_vector3_get_cross_product(&axis, first, second);

    const float square_product = first_square_modulus * second_square_modulus;
    const float dot_product = bgc_fp32_vector3_get_dot_product(first, second);
    const float axis_square_modulus = bgc_fp32_vector3_get_square_modulus(&axis);

    if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON * square_product) {
        if (dot_product < 0.0f) {
            return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
        }

        return BGC_SUCCESS;
    }

    const float axis_modulus = sqrtf(axis_square_modulus);
    const float trigonometry_fix = sqrtf(1.0f / square_product);

    const float angle = 0.5f * atan2f(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);

    const float vector_multiplier = sinf(angle) / axis_modulus;

    bgc_fp32_turn3_set_raw_values(turn, cosf(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);

    return BGC_SUCCESS;
}

int bgc_fp64_turn3_find_direction_difference(BGC_FP64_Turn3* turn, const BGC_FP64_Vector3* first, const BGC_FP64_Vector3* second)
{
    const double first_square_modulus = bgc_fp64_vector3_get_square_modulus(first);

    bgc_fp64_turn3_reset(turn);

    if (first_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        return BGC_ERROR_TURN3_FIRST_VECTOR_ZERO;
    }

    const double second_square_modulus = bgc_fp64_vector3_get_square_modulus(second);

    if (second_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        return BGC_ERROR_TURN3_SECOND_VECTOR_ZERO;
    }

    BGC_FP64_Vector3 axis;

    bgc_fp64_vector3_get_cross_product(&axis, first, second);

    const double square_product = first_square_modulus * second_square_modulus;
    const double dot_product = bgc_fp64_vector3_get_dot_product(first, second);
    const double axis_square_modulus = bgc_fp64_vector3_get_square_modulus(&axis);

    if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON * square_product) {
        bgc_fp64_turn3_reset(turn);
        if (dot_product < 0.0) {
            return BGC_ERROR_TURN3_VECTORS_OPPOSITE;
        }

        return BGC_SUCCESS;
    }

    const double axis_modulus = sqrt(axis_square_modulus);
    const double trigonometry_fix = sqrt(1.0 / square_product);

    const double angle = 0.5 * atan2(axis_modulus * trigonometry_fix, dot_product * trigonometry_fix);

    const double vector_multiplier = sin(angle) / axis_modulus;

    bgc_fp64_turn3_set_raw_values(turn, cos(angle), axis.x1 * vector_multiplier, axis.x2 * vector_multiplier, axis.x3 * vector_multiplier);

    return BGC_SUCCESS;
}

// ============ Make Orthogonal Pair ============ //

static inline int _bgc_fp32_turn3_get_orthogonal_pair(BGC_FP32_Vector3* unit_main, BGC_FP32_Vector3* unit_branch, const BGC_FP32_Vector3* main, const BGC_FP32_Vector3* branch)
{
    const float main_square_modulus = bgc_fp32_vector3_get_square_modulus(main);

    if (main_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_EMPTY_MAIN;
    }

    const float branch_square_modulus = bgc_fp32_vector3_get_square_modulus(branch);

    if (branch_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_EMPTY_BRANCH;
    }

    bgc_fp32_vector3_multiply_by_real(unit_main, main, sqrtf(1.0f / main_square_modulus));

    bgc_fp32_vector3_add_scaled(unit_branch, branch, unit_main, -bgc_fp32_vector3_get_dot_product(branch, unit_main));

    const float orthogonal_square_modulus = bgc_fp32_vector3_get_square_modulus(unit_branch);

    if (orthogonal_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_PAIR_PARALLEL;
    }

    bgc_fp32_vector3_multiply_by_real(unit_branch, unit_branch, sqrtf(1.0f / orthogonal_square_modulus));

    return BGC_SUCCESS;
}

static inline int _bgc_fp64_turn3_get_orthogonal_pair(BGC_FP64_Vector3* unit_main, BGC_FP64_Vector3* unit_branch, const BGC_FP64_Vector3* main, const BGC_FP64_Vector3* branch)
{
    const double main_square_modulus = bgc_fp64_vector3_get_square_modulus(main);

    if (main_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_EMPTY_MAIN;
    }

    const double branch_square_modulus = bgc_fp64_vector3_get_square_modulus(branch);

    if (branch_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_EMPTY_BRANCH;
    }

    bgc_fp64_vector3_multiply_by_real(unit_main, main, sqrt(1.0 / main_square_modulus));

    bgc_fp64_vector3_add_scaled(unit_branch, branch, unit_main, -bgc_fp64_vector3_get_dot_product(branch, unit_main));

    const double orthogonal_square_modulus = bgc_fp64_vector3_get_square_modulus(unit_branch);

    if (orthogonal_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        return _BGC_ERROR_TURN3_PAIR_PARALLEL;
    }

    bgc_fp64_vector3_multiply_by_real(unit_branch, unit_branch, sqrt(1.0 / orthogonal_square_modulus));

    return BGC_SUCCESS;
}

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

static inline void _bgc_fp32_turn3_get_turning_quaternion(BGC_FP32_Quaternion* quaternion, const BGC_FP32_Vector3* unit_start, const BGC_FP32_Vector3* unit_end, const BGC_FP32_Vector3* unit_orthogonal)
{
    BGC_FP32_Vector3 axis;

    bgc_fp32_vector3_get_cross_product(&axis, unit_start, unit_end);

    const float dot_product = bgc_fp32_vector3_get_dot_product(unit_start, unit_end);

    const float axis_square_modulus = bgc_fp32_vector3_get_square_modulus(&axis);

    // unit_start and unit_end are parallel
    if (axis_square_modulus <= BGC_FP32_SQUARE_EPSILON) {
        // unit_start and unit_end are co-directional, angle = 180 degrees
        if (dot_product >= 0.0f) {
            quaternion->s0 = 1.0f;
            quaternion->x1 = 0.0f;
            quaternion->x2 = 0.0f;
            quaternion->x3 = 0.0f;
            return;
        }

        // unit_start and unit_end are opposite, angle = 180 degrees
        quaternion->s0 = 0.0f;
        quaternion->x1 = unit_orthogonal->x1;
        quaternion->x2 = unit_orthogonal->x2;
        quaternion->x3 = unit_orthogonal->x3;
        return;
    }

    const float axis_modulus = sqrtf(axis_square_modulus);

    const float angle = 0.5f * atan2f(axis_modulus, dot_product);

    const float multiplier = sinf(angle) / axis_modulus;

    quaternion->s0 = cosf(angle);
    quaternion->x1 = axis.x1 * multiplier;
    quaternion->x2 = axis.x2 * multiplier;
    quaternion->x3 = axis.x3 * multiplier;
}

static inline void _bgc_fp64_turn3_get_turning_quaternion(BGC_FP64_Quaternion* quaternion, const BGC_FP64_Vector3* unit_start, const BGC_FP64_Vector3* unit_end, const BGC_FP64_Vector3* unit_orthogonal)
{
    BGC_FP64_Vector3 axis;

    bgc_fp64_vector3_get_cross_product(&axis, unit_start, unit_end);

    const double dot_product = bgc_fp64_vector3_get_dot_product(unit_start, unit_end);

    const double axis_square_modulus = bgc_fp64_vector3_get_square_modulus(&axis);

    // unit_start and unit_end are parallel
    if (axis_square_modulus <= BGC_FP64_SQUARE_EPSILON) {
        // unit_start and unit_end are co-directional, angle = 180 degrees
        if (dot_product >= 0.0) {
            quaternion->s0 = 1.0;
            quaternion->x1 = 0.0;
            quaternion->x2 = 0.0;
            quaternion->x3 = 0.0;
            return;
        }

        // unit_start and unit_end are opposite, angle = 180 degrees
        quaternion->s0 = 0.0;
        quaternion->x1 = unit_orthogonal->x1;
        quaternion->x2 = unit_orthogonal->x2;
        quaternion->x3 = unit_orthogonal->x3;
        return;
    }

    const double axis_modulus = sqrt(axis_square_modulus);

    const double angle = 0.5 * atan2(axis_modulus, dot_product);

    const double multiplier = sin(angle) / axis_modulus;

    quaternion->s0 = cos(angle);
    quaternion->x1 = axis.x1 * multiplier;
    quaternion->x2 = axis.x2 * multiplier;
    quaternion->x3 = axis.x3 * multiplier;
}

// ============ Make Pair Difference ============ //

int bgc_fp32_turn3_find_pair_difference(
    BGC_FP32_Turn3* turn,
    const BGC_FP32_Vector3* first_pair_main,
    const BGC_FP32_Vector3* first_pair_branch,
    const BGC_FP32_Vector3* second_pair_main,
    const BGC_FP32_Vector3* second_pair_branch
) {
    BGC_FP32_Vector3 first_fixed_main, first_fixed_branch, first_turned_branch, second_fixed_main, second_fixed_branch;

    int status = _bgc_fp32_turn3_get_orthogonal_pair(&first_fixed_main, &first_fixed_branch, first_pair_main, first_pair_branch);

    if (status != BGC_SUCCESS) {
        bgc_fp32_turn3_reset(turn);
        return status + _BGC_ERROR_TURN3_FIRST_PAIR;
    }

    status = _bgc_fp32_turn3_get_orthogonal_pair(&second_fixed_main, &second_fixed_branch, second_pair_main, second_pair_branch);

    if (status != BGC_SUCCESS) {
        bgc_fp32_turn3_reset(turn);
        return status + _BGC_ERROR_TURN3_SECOND_PAIR;
    }

    BGC_FP32_Quaternion q1, q2;

    // Calculation of a turn (q1) which turns first_fixed_main into second_fixed_main
    _bgc_fp32_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);

    // Roughly turn first_fixed_branch with q1 turn
    _bgc_fp32_quaternion_turn_vector_roughly(&first_turned_branch, &q1, &first_fixed_branch);

    // Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
    _bgc_fp32_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);

    // Composing two turns with multiplication of quaterntions (q2 * q1)
    bgc_fp32_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);

    // Making a final versor (a normalized quaternion)
    const float square_modulus = bgc_fp32_quaternion_get_square_modulus(&turn->_versor);

    if (!bgc_fp32_is_square_unit(square_modulus)) {
        _bgc_fp32_turn3_normalize(turn, square_modulus);
    }

    return BGC_SUCCESS;
}

int bgc_fp64_turn3_find_pair_difference(
    BGC_FP64_Turn3* turn,
    const BGC_FP64_Vector3* first_pair_main,
    const BGC_FP64_Vector3* first_pair_branch,
    const BGC_FP64_Vector3* second_pair_main,
    const BGC_FP64_Vector3* second_pair_branch
) {
    BGC_FP64_Vector3 first_fixed_main, first_fixed_branch, first_turned_branch, second_fixed_main, second_fixed_branch;

    int status = _bgc_fp64_turn3_get_orthogonal_pair(&first_fixed_main, &first_fixed_branch, first_pair_main, first_pair_branch);

    if (status != BGC_SUCCESS) {
        bgc_fp64_turn3_reset(turn);
        return status + _BGC_ERROR_TURN3_FIRST_PAIR;
    }

    status = _bgc_fp64_turn3_get_orthogonal_pair(&second_fixed_main, &second_fixed_branch, second_pair_main, second_pair_branch);

    if (status != BGC_SUCCESS) {
        bgc_fp64_turn3_reset(turn);
        return status + _BGC_ERROR_TURN3_SECOND_PAIR;
    }

    BGC_FP64_Quaternion q1, q2;

    // Calculation of a turn (q1) which turns first_fixed_main into second_fixed_main
    _bgc_fp64_turn3_get_turning_quaternion(&q1, &first_fixed_main, &second_fixed_main, &first_fixed_branch);

    // Roughly turn first_fixed_branch with q1 turn
    _bgc_fp64_quaternion_turn_vector_roughly(&first_turned_branch, &q1, &first_fixed_branch);

    // Calculation of a turn (q2) which turns first_turned_branch into second_fixed_branch
    _bgc_fp64_turn3_get_turning_quaternion(&q2, &first_turned_branch, &second_fixed_branch, &second_fixed_main);

    // Composing two turns with multiplication of quaterntions (q2 * q1)
    bgc_fp64_quaternion_multiply_by_quaternion(&turn->_versor, &q2, &q1);

    // Making a final versor (a normalized quaternion)
    const double square_modulus = bgc_fp64_quaternion_get_square_modulus(&turn->_versor);

    if (!bgc_fp64_is_square_unit(square_modulus)) {
        _bgc_fp64_turn3_normalize(turn, square_modulus);
    }

    return BGC_SUCCESS;
}

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

void bgc_fp32_turn3_get_exponation(BGC_FP32_Turn3* power, const BGC_FP32_Turn3* base, const float exponent)
{
    const float square_vector = base->_versor.x1 * base->_versor.x1 + base->_versor.x2 * base->_versor.x2 + base->_versor.x3 * base->_versor.x3;

    if (square_vector <= BGC_FP32_SQUARE_EPSILON || square_vector != square_vector) {
        bgc_fp32_turn3_reset(power);
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

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

    const float multiplier = sinf(angle) / vector_modulus;

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

void bgc_fp64_turn3_get_exponation(BGC_FP64_Turn3* power, const BGC_FP64_Turn3* base, const double exponent)
{
    const double square_vector = base->_versor.x1 * base->_versor.x1 + base->_versor.x2 * base->_versor.x2 + base->_versor.x3 * base->_versor.x3;

    if (square_vector <= BGC_FP64_SQUARE_EPSILON || square_vector != square_vector) {
        bgc_fp64_turn3_reset(power);
        return;
    }

    const double vector_modulus = sqrt(square_vector);

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

    const double multiplier = sin(angle) / vector_modulus;

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

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

void bgc_fp32_turn3_spherically_interpolate(BGC_FP32_Turn3* interpolation, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end, const float phase)
{
    const float delta_s0 = (end->_versor.s0 * start->_versor.s0 + end->_versor.x1 * start->_versor.x1) + (end->_versor.x2 * start->_versor.x2 + end->_versor.x3 * start->_versor.x3);
    const float delta_x1 = (end->_versor.x1 * start->_versor.s0 + end->_versor.x3 * start->_versor.x2) - (end->_versor.s0 * start->_versor.x1 + end->_versor.x2 * start->_versor.x3);
    const float delta_x2 = (end->_versor.x2 * start->_versor.s0 + end->_versor.x1 * start->_versor.x3) - (end->_versor.s0 * start->_versor.x2 + end->_versor.x3 * start->_versor.x1);
    const float delta_x3 = (end->_versor.x3 * start->_versor.s0 + end->_versor.x2 * start->_versor.x1) - (end->_versor.s0 * start->_versor.x3 + end->_versor.x1 * start->_versor.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_FP32_SQUARE_EPSILON || isnan(square_vector)) {
        bgc_fp32_turn3_copy(interpolation, end);
        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_fp32_turn3_set_raw_values(
        interpolation,
        (turn_s0 * start->_versor.s0 - turn_x1 * start->_versor.x1) - (turn_x2 * start->_versor.x2 + turn_x3 * start->_versor.x3),
        (turn_x1 * start->_versor.s0 + turn_s0 * start->_versor.x1) - (turn_x3 * start->_versor.x2 - turn_x2 * start->_versor.x3),
        (turn_x2 * start->_versor.s0 + turn_s0 * start->_versor.x2) - (turn_x1 * start->_versor.x3 - turn_x3 * start->_versor.x1),
        (turn_x3 * start->_versor.s0 + turn_s0 * start->_versor.x3) - (turn_x2 * start->_versor.x1 - turn_x1 * start->_versor.x2)
    );
}

void bgc_fp64_turn3_spherically_interpolate(BGC_FP64_Turn3* interpolation, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end, const double phase)
{
    const double delta_s0 = (end->_versor.s0 * start->_versor.s0 + end->_versor.x1 * start->_versor.x1) + (end->_versor.x2 * start->_versor.x2 + end->_versor.x3 * start->_versor.x3);
    const double delta_x1 = (end->_versor.x1 * start->_versor.s0 + end->_versor.x3 * start->_versor.x2) - (end->_versor.s0 * start->_versor.x1 + end->_versor.x2 * start->_versor.x3);
    const double delta_x2 = (end->_versor.x2 * start->_versor.s0 + end->_versor.x1 * start->_versor.x3) - (end->_versor.s0 * start->_versor.x2 + end->_versor.x3 * start->_versor.x1);
    const double delta_x3 = (end->_versor.x3 * start->_versor.s0 + end->_versor.x2 * start->_versor.x1) - (end->_versor.s0 * start->_versor.x3 + end->_versor.x1 * start->_versor.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_FP64_SQUARE_EPSILON || isnan(square_vector)) {
        bgc_fp64_turn3_copy(interpolation, end);
        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_fp64_turn3_set_raw_values(
        interpolation,
        (turn_s0 * start->_versor.s0 - turn_x1 * start->_versor.x1) - (turn_x2 * start->_versor.x2 + turn_x3 * start->_versor.x3),
        (turn_x1 * start->_versor.s0 + turn_s0 * start->_versor.x1) - (turn_x3 * start->_versor.x2 - turn_x2 * start->_versor.x3),
        (turn_x2 * start->_versor.s0 + turn_s0 * start->_versor.x2) - (turn_x1 * start->_versor.x3 - turn_x3 * start->_versor.x1),
        (turn_x3 * start->_versor.s0 + turn_s0 * start->_versor.x3) - (turn_x2 * start->_versor.x1 - turn_x1 * start->_versor.x2)
    );
}