#include "./slerp.h"

extern inline void bgc_fp32_slerp_reset(BGC_FP32_Slerp* slerp);
extern inline void bgc_fp64_slerp_reset(BGC_FP64_Slerp* slerp);

extern inline void bgc_fp32_slerp_make_full(BGC_FP32_Slerp* slerp, const BGC_FP32_Versor* start, const BGC_FP32_Versor* end);
extern inline void bgc_fp64_slerp_make_full(BGC_FP64_Slerp* slerp, const BGC_FP64_Versor* start, const BGC_FP64_Versor* end);

extern inline void bgc_fp32_slerp_make_shortened(BGC_FP32_Slerp* slerp, const BGC_FP32_Versor* start, const BGC_FP32_Versor* end);
extern inline void bgc_fp64_slerp_make_shortened(BGC_FP64_Slerp* slerp, const BGC_FP64_Versor* start, const BGC_FP64_Versor* end);

extern inline void bgc_fp32_slerp_get_phase_versor(BGC_FP32_Versor* versor, const BGC_FP32_Slerp* slerp, const float phase);
extern inline void bgc_fp64_slerp_get_phase_versor(BGC_FP64_Versor* versor, const BGC_FP64_Slerp* slerp, const double phase);

void bgc_fp32_slerp_make(BGC_FP32_Slerp* slerp, const BGC_FP32_Versor* start, const BGC_FP32_Versor* augment)
{
    const float square_vector = augment->_x1 * augment->_x1 + augment->_x2 * augment->_x2 + augment->_x3 * augment->_x3;

    if (square_vector != square_vector) {
        bgc_fp32_slerp_reset(slerp);
        return;
    }

    if (square_vector <= BGC_FP32_SQUARE_EPSYLON) {
        slerp->s0_cos_weight = start->_s0;
        slerp->x1_cos_weight = start->_x1;
        slerp->x2_cos_weight = start->_x2;
        slerp->x3_cos_weight = start->_x3;

        slerp->s0_sin_weight = 0.0f;
        slerp->x1_sin_weight = 0.0f;
        slerp->x2_sin_weight = 0.0f;
        slerp->x3_sin_weight = 0.0f;

        slerp->radians = 0.0f;
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

    slerp->radians = atan2f(vector_modulus, augment->_s0);

    const float multiplier = 1.0f / vector_modulus;

    slerp->s0_cos_weight = start->_s0;
    slerp->x1_cos_weight = start->_x1;
    slerp->x2_cos_weight = start->_x2;
    slerp->x3_cos_weight = start->_x3;

    slerp->s0_sin_weight = -multiplier * (augment->_x1 * start->_x1 + augment->_x2 * start->_x2 + augment->_x3 * start->_x3);
    slerp->x1_sin_weight =  multiplier * (augment->_x1 * start->_s0 + augment->_x2 * start->_x3 - augment->_x3 * start->_x2);
    slerp->x2_sin_weight =  multiplier * (augment->_x2 * start->_s0 - augment->_x1 * start->_x3 + augment->_x3 * start->_x1);
    slerp->x3_sin_weight =  multiplier * (augment->_x3 * start->_s0 - augment->_x2 * start->_x1 + augment->_x1 * start->_x2);
}

void bgc_fp64_slerp_make(BGC_FP64_Slerp* slerp, const BGC_FP64_Versor* start, const BGC_FP64_Versor* augment)
{
    const double square_vector = augment->_x1 * augment->_x1 + augment->_x2 * augment->_x2 + augment->_x3 * augment->_x3;

    if (square_vector != square_vector) {
        bgc_fp64_slerp_reset(slerp);
        return;
    }

    if (square_vector <= BGC_FP64_SQUARE_EPSYLON) {
        slerp->s0_cos_weight = start->_s0;
        slerp->x1_cos_weight = start->_x1;
        slerp->x2_cos_weight = start->_x2;
        slerp->x3_cos_weight = start->_x3;

        slerp->s0_sin_weight = 0.0;
        slerp->x1_sin_weight = 0.0;
        slerp->x2_sin_weight = 0.0;
        slerp->x3_sin_weight = 0.0;

        slerp->radians = 0.0;
        return;
    }

    const double vector_modulus = sqrt(square_vector);

    slerp->radians = atan2(vector_modulus, augment->_s0);

    const double multiplier = 1.0 / vector_modulus;

    slerp->s0_cos_weight = start->_s0;
    slerp->x1_cos_weight = start->_x1;
    slerp->x2_cos_weight = start->_x2;
    slerp->x3_cos_weight = start->_x3;

    slerp->s0_sin_weight = -multiplier * (augment->_x1 * start->_x1 + augment->_x2 * start->_x2 + augment->_x3 * start->_x3);
    slerp->x1_sin_weight =  multiplier * (augment->_x1 * start->_s0 + augment->_x2 * start->_x3 - augment->_x3 * start->_x2);
    slerp->x2_sin_weight =  multiplier * (augment->_x2 * start->_s0 - augment->_x1 * start->_x3 + augment->_x3 * start->_x1);
    slerp->x3_sin_weight =  multiplier * (augment->_x3 * start->_s0 - augment->_x2 * start->_x1 + augment->_x1 * start->_x2);
}