#ifndef _BGC_VERSOR_SLERP_H_
#define _BGC_VERSOR_SLERP_H_

#include "./versor.h"

typedef struct {
    float s0_cos_weight, s0_sin_weight;
    float x1_cos_weight, x1_sin_weight;
    float x2_cos_weight, x2_sin_weight;
    float x3_cos_weight, x3_sin_weight;
    float radians;
} BGC_FP32_Slerp;

typedef struct {
    double s0_cos_weight, s0_sin_weight;
    double x1_cos_weight, x1_sin_weight;
    double x2_cos_weight, x2_sin_weight;
    double x3_cos_weight, x3_sin_weight;
    double radians;
} BGC_FP64_Slerp;

inline void bgc_fp32_slerp_reset(BGC_FP32_Slerp* 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_Slerp* 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(const BGC_FP32_Versor* start, const BGC_FP32_Versor* augment, BGC_FP32_Slerp* slerp);

void bgc_fp64_slerp_make(const BGC_FP64_Versor* start, const BGC_FP64_Versor* augment, BGC_FP64_Slerp* slerp);

inline void bgc_fp32_slerp_make_full(const BGC_FP32_Versor* start, const BGC_FP32_Versor* end, BGC_FP32_Slerp* slerp)
{
    BGC_FP32_Versor augment;

    bgc_fp32_versor_exclude(end, start, &augment);

    bgc_fp32_slerp_make(start, &augment, slerp);
}

inline void bgc_fp64_slerp_make_full(const BGC_FP64_Versor* start, const BGC_FP64_Versor* end, BGC_FP64_Slerp* slerp)
{
    BGC_FP64_Versor augment;

    bgc_fp64_versor_exclude(end, start, &augment);

    bgc_fp64_slerp_make(start, &augment, slerp);
}

inline void bgc_fp32_slerp_make_shortened(const BGC_FP32_Versor* start, const BGC_FP32_Versor* end, BGC_FP32_Slerp* slerp)
{
    BGC_FP32_Versor augment;

    bgc_fp32_versor_exclude(end, start, &augment);
    bgc_fp32_versor_shorten(&augment);

    bgc_fp32_slerp_make(start, &augment, slerp);
}

inline void bgc_fp64_slerp_make_shortened(const BGC_FP64_Versor* start, const BGC_FP64_Versor* end, BGC_FP64_Slerp* slerp)
{
    BGC_FP64_Versor augment;

    bgc_fp64_versor_exclude(end, start, &augment);
    bgc_fp64_versor_shorten(&augment);

    bgc_fp64_slerp_make(start, &augment, slerp);
}

inline void bgc_fp32_slerp_get_phase_versor(const BGC_FP32_Slerp* slerp, const float phase, BGC_FP32_Versor* result)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

    bgc_fp32_versor_make(
        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,
        result
    );
}

inline void bgc_fp64_slerp_get_phase_versor(const BGC_FP64_Slerp* slerp, const double phase, BGC_FP64_Versor* result)
{
    const double angle = slerp->radians * phase;
    const double cosine = cos(angle);
    const double sine = sin(angle);

    bgc_fp64_versor_make(
        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,
        result
    );
}

#endif