#ifndef _BGC_SLERP_H_INCLUDED_
#define _BGC_SLERP_H_INCLUDED_

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

// =================== Reset ==================== //

inline void bgc_fp32_slerp3_reset(BGC_FP32_Slerp3* slerp)
{
    slerp->_cosine_weight.s0 = 1.0f;
    slerp->_cosine_weight.x1 = 0.0f;
    slerp->_cosine_weight.x2 = 0.0f;
    slerp->_cosine_weight.x3 = 0.0f;

    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;
}

inline void bgc_fp64_slerp3_reset(BGC_FP64_Slerp3* slerp)
{
    slerp->_cosine_weight.s0 = 1.0;
    slerp->_cosine_weight.x1 = 0.0;
    slerp->_cosine_weight.x2 = 0.0;
    slerp->_cosine_weight.x3 = 0.0;

    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;
}

// ==================== Make ==================== //

void bgc_fp32_slerp3_make(BGC_FP32_Slerp3* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* augment);
void bgc_fp64_slerp3_make(BGC_FP64_Slerp3* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* augment);

// ================= Make Full ================== //

inline void bgc_fp32_slerp3_make_full(BGC_FP32_Slerp3* slerp, const BGC_FP32_Turn3* start, const BGC_FP32_Turn3* end)
{
    BGC_FP32_Turn3 augment;

    bgc_fp32_turn3_exclude(&augment, end, start);

    bgc_fp32_slerp3_make(slerp, start, &augment);
}

inline void bgc_fp64_slerp3_make_full(BGC_FP64_Slerp3* slerp, const BGC_FP64_Turn3* start, const BGC_FP64_Turn3* end)
{
    BGC_FP64_Turn3 augment;

    bgc_fp64_turn3_exclude(&augment, end, start);

    bgc_fp64_slerp3_make(slerp, start, &augment);
}

// =============== Make Shortened =============== //

inline void bgc_fp32_slerp3_make_shortened(BGC_FP32_Slerp3* 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_slerp3_make(slerp, start, &augment);
}

inline void bgc_fp64_slerp3_make_shortened(BGC_FP64_Slerp3* 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_slerp3_make(slerp, start, &augment);
}

// =============== Get Phase Turn =============== //

inline void bgc_fp32_slerp3_get_phase_turn(BGC_FP32_Turn3* turn, const BGC_FP32_Slerp3* slerp, const float phase)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    BGC_FP32_Quaternion q;

    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp32_turn3_set_quaternion(turn, &q);
}

inline void bgc_fp64_slerp3_get_phase_turn(BGC_FP64_Turn3* turn, const BGC_FP64_Slerp3* slerp, const double phase)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    BGC_FP64_Quaternion q;

    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp64_turn3_set_quaternion(turn, &q);
}

// ========= Get Phase Rotation Matrix ========== //

inline void bgc_fp32_slerp3_get_phase_rotation_matrix(BGC_FP32_Matrix3x3* rotation_matrix, const BGC_FP32_Slerp3* slerp, const float phase)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    BGC_FP32_Quaternion q;

    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp32_quaternion_get_rotation_matrix(rotation_matrix, &q);
}

inline void bgc_fp64_slerp3_get_phase_rotation_matrix(BGC_FP64_Matrix3x3* rotation_matrix, const BGC_FP64_Slerp3* slerp, const double phase)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    BGC_FP64_Quaternion q;

    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp64_quaternion_get_rotation_matrix(rotation_matrix, &q);
}

// ========== Get Phase Reverse Matrix ========== //

inline void bgc_fp32_slerp3_get_phase_reverse_matrix(BGC_FP32_Matrix3x3* reverse_matrix, const BGC_FP32_Slerp3* slerp, const float phase)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    BGC_FP32_Quaternion q;

    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp32_quaternion_get_reverse_matrix(reverse_matrix, &q);
}

inline void bgc_fp64_slerp3_get_phase_reverse_matrix(BGC_FP64_Matrix3x3* reverse_matrix, const BGC_FP64_Slerp3* slerp, const double phase)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    BGC_FP64_Quaternion q;

    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp64_quaternion_get_reverse_matrix(reverse_matrix, &q);
}

// ========== Get Phase Both Matrices =========== //

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)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    BGC_FP32_Quaternion q;

    bgc_fp32_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp32_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp32_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q);
}

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)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    BGC_FP64_Quaternion q;

    bgc_fp64_quaternion_multiply_by_real(&q, &slerp->_cosine_weight, cosine);
    bgc_fp64_quaternion_add_scaled(&q, &q, &slerp->_sine_weight, sine);

    bgc_fp64_quaternion_get_both_matrices(rotation_matrix, reverse_matrix, &q);
}

#endif