#include "./slerp3.h"

extern inline void bgc_fp32_slerp3_reset(BGC_FP32_Slerp3* slerp);
extern inline void bgc_fp64_slerp3_reset(BGC_FP64_Slerp3* slerp);

extern inline void bgc_fp32_slerp3_make_full(BGC_FP32_Slerp3* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end);
extern inline void bgc_fp64_slerp3_make_full(BGC_FP64_Slerp3* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end);

extern inline void bgc_fp32_slerp3_make_shortened(BGC_FP32_Slerp3* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end);
extern inline void bgc_fp64_slerp3_make_shortened(BGC_FP64_Slerp3* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end);

extern inline void bgc_fp32_slerp3_get_phase_turn(BGC_FP32_Turn3* versor, const BGC_FP32_Slerp3* slerp, const float phase);
extern inline void bgc_fp64_slerp3_get_phase_turn(BGC_FP64_Turn3* versor, const BGC_FP64_Slerp3* slerp, const double phase);

extern inline void bgc_fp32_slerp3_get_phase_rotation_matrix(BGC_FP32_Matrix3x3* rotation_matrix, const BGC_FP32_Slerp3* slerp, const float phase);
extern inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* rotation_matrix, const BGC_FP64_Slerp3* slerp, const double phase);

extern inline void bgc_fp32_slerp3_get_phase_reverse_matrix(BGC_FP32_Matrix3x3* reverse_matrix, const BGC_FP32_Slerp3* slerp, const float phase);
extern inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* reverse_matrix, const BGC_FP64_Slerp3* slerp, const double phase);

extern inline void bgc_fp32_slerp3_get_phase_both_matrices(BGC_FP32_Matrix3x3* rotation_matrix, BGC_FP32_Matrix3x3* reverse_matrix, const BGC_FP32_Slerp3* slerp, const float phase);
extern inline void bgc_fp64_slerp3_get_phase_both_matrices(BGC_FP64_Matrix3x3* rotation_matrix, BGC_FP64_Matrix3x3* reverse_matrix, const BGC_FP64_Slerp3* slerp, const double phase);

void bgc_fp32_slerp3_make(BGC_FP32_Slerp3* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* augment)
{
    const float square_vector = augment->_versor.x1 * augment->_versor.x1 + augment->_versor.x2 * augment->_versor.x2 + augment->_versor.x3 * augment->_versor.x3;

    if (isnan(square_vector)) {
        bgc_fp32_slerp3_reset(slerp);
        return;
    }

    if (square_vector <= BGC_FP32_SQUARE_EPSILON) {
        slerp->_cosine_weight.s0 = start->_versor.s0;
        slerp->_cosine_weight.x1 = start->_versor.x1;
        slerp->_cosine_weight.x2 = start->_versor.x2;
        slerp->_cosine_weight.x3 = start->_versor.x3;

        slerp->_sine_weight.s0 = 0.0f;
        slerp->_sine_weight.x1 = 0.0f;
        slerp->_sine_weight.x2 = 0.0f;
        slerp->_sine_weight.x3 = 0.0f;

        slerp->radians = 0.0f;
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

    slerp->radians = atan2f(vector_modulus, augment->_versor.s0);

    const float multiplier = 1.0f / vector_modulus;

    slerp->_cosine_weight.s0 = start->_versor.s0;
    slerp->_cosine_weight.x1 = start->_versor.x1;
    slerp->_cosine_weight.x2 = start->_versor.x2;
    slerp->_cosine_weight.x3 = start->_versor.x3;

    slerp->_sine_weight.s0 = -multiplier * (augment->_versor.x1 * start->_versor.x1 + augment->_versor.x2 * start->_versor.x2 + augment->_versor.x3 * start->_versor.x3);
    slerp->_sine_weight.x1 =  multiplier * (augment->_versor.x1 * start->_versor.s0 + augment->_versor.x2 * start->_versor.x3 - augment->_versor.x3 * start->_versor.x2);
    slerp->_sine_weight.x2 =  multiplier * (augment->_versor.x2 * start->_versor.s0 - augment->_versor.x1 * start->_versor.x3 + augment->_versor.x3 * start->_versor.x1);
    slerp->_sine_weight.x3 =  multiplier * (augment->_versor.x3 * start->_versor.s0 - augment->_versor.x2 * start->_versor.x1 + augment->_versor.x1 * start->_versor.x2);
}

void bgc_fp64_slerp3_make(BGC_FP64_Slerp3* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* augment)
{
    const double square_vector = augment->_versor.x1 * augment->_versor.x1 + augment->_versor.x2 * augment->_versor.x2 + augment->_versor.x3 * augment->_versor.x3;

    if (isnan(square_vector)) {
        bgc_fp64_slerp3_reset(slerp);
        return;
    }

    if (square_vector <= BGC_FP64_SQUARE_EPSILON) {
        slerp->_cosine_weight.s0 = start->_versor.s0;
        slerp->_cosine_weight.x1 = start->_versor.x1;
        slerp->_cosine_weight.x2 = start->_versor.x2;
        slerp->_cosine_weight.x3 = start->_versor.x3;

        slerp->_sine_weight.s0 = 0.0;
        slerp->_sine_weight.x1 = 0.0;
        slerp->_sine_weight.x2 = 0.0;
        slerp->_sine_weight.x3 = 0.0;

        slerp->radians = 0.0;
        return;
    }

    const double vector_modulus = sqrt(square_vector);

    slerp->radians = atan2(vector_modulus, augment->_versor.s0);

    const double multiplier = 1.0 / vector_modulus;

    slerp->_cosine_weight.s0 = start->_versor.s0;
    slerp->_cosine_weight.x1 = start->_versor.x1;
    slerp->_cosine_weight.x2 = start->_versor.x2;
    slerp->_cosine_weight.x3 = start->_versor.x3;

    slerp->_sine_weight.s0 = -multiplier * (augment->_versor.x1 * start->_versor.x1 + augment->_versor.x2 * start->_versor.x2 + augment->_versor.x3 * start->_versor.x3);
    slerp->_sine_weight.x1 =  multiplier * (augment->_versor.x1 * start->_versor.s0 + augment->_versor.x2 * start->_versor.x3 - augment->_versor.x3 * start->_versor.x2);
    slerp->_sine_weight.x2 =  multiplier * (augment->_versor.x2 * start->_versor.s0 - augment->_versor.x1 * start->_versor.x3 + augment->_versor.x3 * start->_versor.x1);
    slerp->_sine_weight.x3 =  multiplier * (augment->_versor.x3 * start->_versor.s0 - augment->_versor.x2 * start->_versor.x1 + augment->_versor.x1 * start->_versor.x2);
}