#include "./slerp.h"

extern inline void bgc_slerp_reset_fp32(BgcSlerpFP32* slerp);
extern inline void bgc_slerp_reset_fp64(BgcSlerpFP64* slerp);

extern inline bgc_slerp_get_turn_for_phase_fp32(const BgcSlerpFP32* slerp, const float phase, BgcVersorFP32* result);
extern inline bgc_slerp_get_turn_for_phase_fp64(const BgcSlerpFP64* slerp, const double phase, BgcVersorFP64* result);

void bgc_slerp_make_full_fp32(const BgcVersorFP32* start, const BgcVersorFP32* end, BgcSlerpFP32* slerp)
{
    BgcVersorFP32 delta;

    bgc_versor_exclude_fp32(end, start, &delta);

    const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
        bgc_slerp_reset_fp32(slerp);
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

    slerp->radians = atan2f(vector_modulus, delta.s0);

    const float mutliplier = 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 = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
    slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
    slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
    slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
}

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

    bgc_versor_exclude_fp64(end, start, &delta);

    const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
        bgc_slerp_reset_fp64(slerp);
        return;
    }

    const double vector_modulus = sqrt(square_vector);

    slerp->radians = atan2(vector_modulus, delta.s0);

    const double mutliplier = 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 = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
    slerp->x1_sin_weight = mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
    slerp->x2_sin_weight = mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
    slerp->x3_sin_weight = mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
}

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

    bgc_versor_exclude_fp32(end, start, &delta);
    bgc_versor_shorten_fp32(&delta, &delta);

    const float square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP32 || square_vector != square_vector) {
        bgc_slerp_reset_fp32(slerp);
        return;
    }

    const float vector_modulus = sqrtf(square_vector);

    slerp->radians = atan2f(vector_modulus, delta.s0);

    const float mutliplier = 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 = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
    slerp->x1_sin_weight =  mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
    slerp->x2_sin_weight =  mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
    slerp->x3_sin_weight =  mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
}

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

    bgc_versor_exclude_fp64(end, start, &delta);
    bgc_versor_shorten_fp64(&delta, &delta);

    const double square_vector = delta.x1 * delta.x1 + delta.x2 * delta.x2 + delta.x3 * delta.x3;

    if (square_vector <= BGC_SQUARE_EPSYLON_FP64 || square_vector != square_vector) {
        bgc_slerp_reset_fp64(slerp);
        return;
    }

    const double vector_modulus = sqrt(square_vector);

    slerp->radians = atan2(vector_modulus, delta.s0);

    const double mutliplier = 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 = -mutliplier * (delta.x1 * start->x1 + delta.x2 * start->x2 + delta.x3 * start->x3);
    slerp->x1_sin_weight =  mutliplier * (delta.x1 * start->s0 + delta.x2 * start->x3 - delta.x3 * start->x2);
    slerp->x2_sin_weight =  mutliplier * (delta.x2 * start->s0 - delta.x1 * start->x3 + delta.x3 * start->x1);
    slerp->x3_sin_weight =  mutliplier * (delta.x3 * start->s0 - delta.x2 * start->x1 + delta.x1 * start->x2);
}