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

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

inline void bgc_slerp_reset_fp32(BgcSlerpFP32* 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_slerp_reset_fp64(BgcSlerpFP64* 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_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);

void bgc_slerp_make_full_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);

void bgc_slerp_make_shortened_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp);

void bgc_slerp_make_shortened_fp64(const BgcVersorFP64* start, const BgcVersorFP64* end, BgcSlerpFP64* slerp);

inline void bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result)
{
    const float angle = slerp->radians * phase;
    const float cosine = cosf(angle);
    const float sine = sinf(angle);

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

    bgc_versor_set_values_fp64(
        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