#ifndef _BGC_SLERP_H_INCLUDED_
#define _BGC_SLERP_H_INCLUDED_

#include "./types.h"
#include "./turn3.h"

inline void bgc_fp32_slerp_reset(BGC_FP32_Turn3Slerp* slerp)
{
    slerp->_s0_cos_weight = 1.0f;
    slerp->_s0_sin_weight = 0.0f;

    slerp->_x1_cos_weight = 0.0f;
    slerp->_x1_sin_weight = 0.0f;

    slerp->_x2_cos_weight = 0.0f;
    slerp->_x2_sin_weight = 0.0f;

    slerp->_x3_cos_weight = 0.0f;
    slerp->_x3_sin_weight = 0.0f;

    slerp->radians = 0.0f;
}

inline void bgc_fp64_slerp_reset(BGC_FP64_Turn3Slerp* slerp)
{
    slerp->_s0_cos_weight = 1.0;
    slerp->_s0_sin_weight = 0.0;

    slerp->_x1_cos_weight = 0.0;
    slerp->_x1_sin_weight = 0.0;

    slerp->_x2_cos_weight = 0.0;
    slerp->_x2_sin_weight = 0.0;

    slerp->_x3_cos_weight = 0.0;
    slerp->_x3_sin_weight = 0.0;

    slerp->radians = 0.0;
}

void bgc_fp32_slerp_make(BGC_FP32_Turn3Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* augment);
void bgc_fp64_slerp_make(BGC_FP64_Turn3Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* augment);

inline void bgc_fp32_slerp_make_full(BGC_FP32_Turn3Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end)
{
    BGC_FP32_Turn3 augment;

    bgc_fp32_turn3_exclude(&augment, end, start);

    bgc_fp32_slerp_make(slerp, start, &augment);
}

inline void bgc_fp64_slerp_make_full(BGC_FP64_Turn3Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end)
{
    BGC_FP64_Turn3 augment;

    bgc_fp64_turn3_exclude(&augment, end, start);

    bgc_fp64_slerp_make(slerp, start, &augment);
}

inline void bgc_fp32_slerp_make_shortened(BGC_FP32_Turn3Slerp* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end)
{
    BGC_FP32_Turn3 augment;

    bgc_fp32_turn3_exclude(&augment, end, start);
    bgc_fp32_turn3_shorten(&augment);

    bgc_fp32_slerp_make(slerp, start, &augment);
}

inline void bgc_fp64_slerp_make_shortened(BGC_FP64_Turn3Slerp* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end)
{
    BGC_FP64_Turn3 augment;

    bgc_fp64_turn3_exclude(&augment, end, start);
    bgc_fp64_turn3_shorten(&augment);

    bgc_fp64_slerp_make(slerp, start, &augment);
}

inline void bgc_fp32_slerp_get_phase_versor(BGC_FP32_Turn3* versor, const BGC_FP32_Turn3Slerp* slerp, const float phase)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    bgc_fp32_turn3_set_raw_values(
        versor,
        slerp->_s0_cos_weight * cosine + slerp->_s0_sin_weight * sine,
        slerp->_x1_cos_weight * cosine + slerp->_x1_sin_weight * sine,
        slerp->_x2_cos_weight * cosine + slerp->_x2_sin_weight * sine,
        slerp->_x3_cos_weight * cosine + slerp->_x3_sin_weight * sine
    );
}

inline void bgc_fp64_slerp_get_phase_versor(BGC_FP64_Turn3* versor, const BGC_FP64_Turn3Slerp* slerp, const double phase)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    bgc_fp64_turn3_set_raw_values(
        versor,
        slerp->_s0_cos_weight * cosine + slerp->_s0_sin_weight * sine,
        slerp->_x1_cos_weight * cosine + slerp->_x1_sin_weight * sine,
        slerp->_x2_cos_weight * cosine + slerp->_x2_sin_weight * sine,
        slerp->_x3_cos_weight * cosine + slerp->_x3_sin_weight * sine
    );
}

#endif