#include "./vector3.h"

extern inline void bgc_fp32_vector3_reset(BGC_FP32_Vector3* const vector);
extern inline void bgc_fp64_vector3_reset(BGC_FP64_Vector3* const vector);

extern inline void bgc_fp32_vector3_set_values(BGC_FP32_Vector3* const destination, const float x1, const float x2, const float x3);
extern inline void bgc_fp64_vector3_set_values(BGC_FP64_Vector3* const destination, const double x1, const double x2, const double x3);

extern inline float bgc_fp32_vector3_get_squared_length(const BGC_FP32_Vector3* const vector);
extern inline double bgc_fp64_vector3_get_squared_length(const BGC_FP64_Vector3* const vector);

extern inline float bgc_fp32_vector3_get_length(const BGC_FP32_Vector3* const vector);
extern inline double bgc_fp64_vector3_get_length(const BGC_FP64_Vector3* const vector);

extern inline int bgc_fp32_vector3_is_zero(const BGC_FP32_Vector3* const vector);
extern inline int bgc_fp64_vector3_is_zero(const BGC_FP64_Vector3* const vector);

extern inline int bgc_fp32_vector3_is_unit(const BGC_FP32_Vector3* const vector);
extern inline int bgc_fp64_vector3_is_unit(const BGC_FP64_Vector3* const vector);

extern inline void bgc_fp32_vector3_copy(BGC_FP32_Vector3* const destination, const BGC_FP32_Vector3* const source);
extern inline void bgc_fp64_vector3_copy(BGC_FP64_Vector3* const destination, const BGC_FP64_Vector3* const source);

extern inline void bgc_fp32_vector3_convert_to_fp64(BGC_FP64_Vector3* const destination, const BGC_FP32_Vector3* const source);
extern inline void bgc_fp64_vector3_convert_to_fp32(BGC_FP32_Vector3* const destination, const BGC_FP64_Vector3* const source);

extern inline void bgc_fp32_vector3_swap(BGC_FP32_Vector3* const vector1, BGC_FP32_Vector3* const vector2);
extern inline void bgc_fp64_vector3_swap(BGC_FP64_Vector3* const vector1, BGC_FP64_Vector3* const vector2);

extern inline void bgc_fp32_vector3_add(BGC_FP32_Vector3* const sum, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline void bgc_fp64_vector3_add(BGC_FP64_Vector3* const sum, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline void bgc_fp32_vector3_add_scaled(BGC_FP32_Vector3* const sum, const BGC_FP32_Vector3* const basic_vector, const BGC_FP32_Vector3* const scalable_vector, const float scale);
extern inline void bgc_fp64_vector3_add_scaled(BGC_FP64_Vector3* const sum, const BGC_FP64_Vector3* const basic_vector, const BGC_FP64_Vector3* const scalable_vector, const double scale);

extern inline void bgc_fp32_vector3_subtract(BGC_FP32_Vector3* const difference, const BGC_FP32_Vector3* const minuend, const BGC_FP32_Vector3* const subtrahend);
extern inline void bgc_fp64_vector3_subtract(BGC_FP64_Vector3* const difference, const BGC_FP64_Vector3* const minuend, const BGC_FP64_Vector3* const subtrahend);

extern inline void bgc_fp32_vector3_subtract_scaled(BGC_FP32_Vector3* const difference, const BGC_FP32_Vector3* const basic_vector, const BGC_FP32_Vector3* const scalable_vector, const float scale);
extern inline void bgc_fp64_vector3_subtract_scaled(BGC_FP64_Vector3* const difference, const BGC_FP64_Vector3* const basic_vector, const BGC_FP64_Vector3* const scalable_vector, const double scale);

extern inline void bgc_fp32_vector3_multiply_by_real_number(BGC_FP32_Vector3* const product, const BGC_FP32_Vector3* const multiplicand, const float multiplier);
extern inline void bgc_fp64_vector3_multiply_by_real_number(BGC_FP64_Vector3* const product, const BGC_FP64_Vector3* const multiplicand, const double multiplier);

extern inline void bgc_fp32_vector3_multiply_by_dual_number(BGC_FP32_DualVector3* const product, const BGC_FP32_Vector3* const multiplicand, const BGC_FP32_DualNumber* const multiplier);
extern inline void bgc_fp64_vector3_multiply_by_dual_number(BGC_FP64_DualVector3* const product, const BGC_FP64_Vector3* const multiplicand, const BGC_FP64_DualNumber* const multiplier);

extern inline void _bgc_fp32_restrict_vector3_multiply_by_dual_number(BGC_FP32_DualVector3* restrict const product, const BGC_FP32_Vector3* const multiplicand, const BGC_FP32_DualNumber* const multiplier);
extern inline void _bgc_fp64_restrict_vector3_multiply_by_dual_number(BGC_FP64_DualVector3* restrict const product, const BGC_FP64_Vector3* const multiplicand, const BGC_FP64_DualNumber* const multiplier);

extern inline void bgc_fp32_vector3_multiply_by_matrix2x3(BGC_FP32_Vector2* const product, const BGC_FP32_Vector3* const vector, const BGC_FP32_Matrix2x3* const matrix);
extern inline void bgc_fp64_vector3_multiply_by_matrix2x3(BGC_FP64_Vector2* const product, const BGC_FP64_Vector3* const vector, const BGC_FP64_Matrix2x3* const matrix);

extern inline void bgc_fp32_vector3_multiply_by_matrix3x3(BGC_FP32_Vector3* const product, const BGC_FP32_Vector3* const vector, const BGC_FP32_Matrix3x3* const matrix);
extern inline void bgc_fp64_vector3_multiply_by_matrix3x3(BGC_FP64_Vector3* const product, const BGC_FP64_Vector3* const vector, const BGC_FP64_Matrix3x3* const matrix);

extern inline int bgc_fp32_vector3_divide_by_real(BGC_FP32_Vector3* const quotient, const BGC_FP32_Vector3* const dividend, const float divisor);
extern inline int bgc_fp64_vector3_divide_by_real(BGC_FP64_Vector3* const quotient, const BGC_FP64_Vector3* const dividend, const double divisor);

extern inline void bgc_fp32_vector3_get_mean2(BGC_FP32_Vector3* const mean, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline void bgc_fp64_vector3_get_mean2(BGC_FP64_Vector3* const mean, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline void bgc_fp32_vector3_get_mean3(BGC_FP32_Vector3* const mean, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const BGC_FP32_Vector3* const vector3);
extern inline void bgc_fp64_vector3_get_mean3(BGC_FP64_Vector3* const mean, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const BGC_FP64_Vector3* const vector3);

extern inline void bgc_fp32_vector3_interpolate(BGC_FP32_Vector3* const interpolation, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const float phase);
extern inline void bgc_fp64_vector3_interpolate(BGC_FP64_Vector3* const interpolation, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const double phase);

extern inline void bgc_fp32_vector3_revert(BGC_FP32_Vector3* const vector);
extern inline void bgc_fp64_vector3_revert(BGC_FP64_Vector3* const vector);

extern inline void bgc_fp32_vector3_get_reverse(BGC_FP32_Vector3* const reverse, const BGC_FP32_Vector3* const vector);
extern inline void bgc_fp64_vector3_get_reverse(BGC_FP64_Vector3* const reverse, const BGC_FP64_Vector3* const vector);

extern inline int bgc_fp32_vector3_normalize(BGC_FP32_Vector3* const vector);
extern inline int bgc_fp64_vector3_normalize(BGC_FP64_Vector3* const vector);

extern inline int bgc_fp32_vector3_get_normalized(BGC_FP32_Vector3* const normalized, const BGC_FP32_Vector3* const vector);
extern inline int bgc_fp64_vector3_get_normalized(BGC_FP64_Vector3* const normalized, const BGC_FP64_Vector3* const vector);

extern inline float bgc_fp32_vector3_get_dot_product(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline double bgc_fp64_vector3_get_dot_product(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline float bgc_fp32_vector3_get_triple_product(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const BGC_FP32_Vector3* const vector3);
extern inline double bgc_fp64_vector3_get_triple_product(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const BGC_FP64_Vector3* const vector3);

extern inline void _bgc_fp32_restrict_vector3_get_cross_product(BGC_FP32_Vector3* restrict const product, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline void _bgc_fp64_restrict_vector3_get_cross_product(BGC_FP64_Vector3* restrict const product, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline void bgc_fp32_vector3_get_cross_product(BGC_FP32_Vector3* const product, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline void bgc_fp64_vector3_get_cross_product(BGC_FP64_Vector3* const product, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline void bgc_fp32_vector3_get_double_cross(BGC_FP32_Vector3* const product, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const BGC_FP32_Vector3* const vector3);
extern inline void bgc_fp64_vector3_get_double_cross(BGC_FP64_Vector3* const product, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const BGC_FP64_Vector3* const vector3);

extern inline void bgc_fp32_vector3_get_alternative_double_cross(BGC_FP32_Vector3* const product, const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const BGC_FP32_Vector3* const vector3);
extern inline void bgc_fp64_vector3_get_alternative_double_cross(BGC_FP64_Vector3* const product, const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const BGC_FP64_Vector3* const vector3);

extern inline float bgc_fp32_vector3_get_square_distance(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline double bgc_fp64_vector3_get_square_distance(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline float bgc_fp32_vector3_get_distance(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline double bgc_fp64_vector3_get_distance(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline int bgc_fp32_vector3_are_close_enough(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const float distance);
extern inline int bgc_fp64_vector3_are_close_enough(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const double distance);

extern inline int bgc_fp32_vector3_are_close(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline int bgc_fp64_vector3_are_close(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline int bgc_fp32_vector3_are_parallel(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline int bgc_fp64_vector3_are_parallel(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline int bgc_fp32_vector3_are_orthogonal(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline int bgc_fp64_vector3_are_orthogonal(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

extern inline int bgc_fp32_vector3_get_attitude(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2);
extern inline int bgc_fp64_vector3_get_attitude(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2);

// =================== Angle ==================== //

float bgc_fp32_vector3_get_angle(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2, const int angle_unit)
{
    const float square_modulus1 = bgc_fp32_vector3_get_squared_length(vector1);
    const float square_modulus2 = bgc_fp32_vector3_get_squared_length(vector2);

    if (square_modulus1 <= BGC_FP32_SQUARE_EPSILON || square_modulus2 <= BGC_FP32_SQUARE_EPSILON || isnan(square_modulus1) || isnan(square_modulus2)) {
        return 0.0f;
    }

    BGC_FP32_Vector3 cross_product;

    bgc_fp32_vector3_get_cross_product(&cross_product, vector1, vector2);

    const float scalar = bgc_fp32_vector3_get_dot_product(vector1, vector2);

    const float cross = bgc_fp32_vector3_get_length(&cross_product);

    return bgc_fp32_radians_to_units(atan2f(cross, scalar), angle_unit);
}

double bgc_fp64_vector3_get_angle(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2, const int angle_unit)
{
    const double square_modulus1 = bgc_fp64_vector3_get_squared_length(vector1);
    const double square_modulus2 = bgc_fp64_vector3_get_squared_length(vector2);


    if (square_modulus1 <= BGC_FP64_SQUARE_EPSILON || square_modulus2 <= BGC_FP64_SQUARE_EPSILON || isnan(square_modulus1) || isnan(square_modulus2)) {
        return 0.0;
    }

    BGC_FP64_Vector3 cross_product;

    bgc_fp64_vector3_get_cross_product(&cross_product, vector1, vector2);

    const double scalar = bgc_fp64_vector3_get_dot_product(vector1, vector2);

    const double cross = bgc_fp64_vector3_get_length(&cross_product);

    return bgc_fp64_radians_to_units(atan2(cross, scalar), angle_unit);
}

// ================== Attitude ================== //

int bgc_fp32_vector3_get_attitude(const BGC_FP32_Vector3* const vector1, const BGC_FP32_Vector3* const vector2)
{
    const float square_modulus1 = bgc_fp32_vector3_get_squared_length(vector1);
    const float square_modulus2 = bgc_fp32_vector3_get_squared_length(vector2);

    if (square_modulus1 <= BGC_FP32_SQUARE_EPSILON || square_modulus2 <= BGC_FP32_SQUARE_EPSILON) {
        return BGC_ATTITUDE_ZERO;
    }

    const float square_limit = BGC_FP32_SQUARE_EPSILON * square_modulus1 * square_modulus2;

    const float scalar_product = bgc_fp32_vector3_get_dot_product(vector1, vector2);

    if (scalar_product * scalar_product <= square_limit) {
        return BGC_ATTITUDE_ORTHOGONAL;
    }

    BGC_FP32_Vector3 product;

    _bgc_fp32_restrict_vector3_get_cross_product(&product, vector1, vector2);

    if (bgc_fp32_vector3_get_squared_length(&product) > square_limit) {
        return BGC_ATTITUDE_ANY;
    }

    return scalar_product > 0.0f ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL;
}

int bgc_fp64_vector3_get_attitude(const BGC_FP64_Vector3* const vector1, const BGC_FP64_Vector3* const vector2)
{
    const double square_modulus1 = bgc_fp64_vector3_get_squared_length(vector1);
    const double square_modulus2 = bgc_fp64_vector3_get_squared_length(vector2);

    if (square_modulus1 <= BGC_FP64_SQUARE_EPSILON || square_modulus2 <= BGC_FP64_SQUARE_EPSILON) {
        return BGC_ATTITUDE_ZERO;
    }

    const double square_limit = BGC_FP64_SQUARE_EPSILON * square_modulus1 * square_modulus2;

    const double scalar_product = bgc_fp64_vector3_get_dot_product(vector1, vector2);

    if (scalar_product * scalar_product <= square_limit) {
        return BGC_ATTITUDE_ORTHOGONAL;
    }

    BGC_FP64_Vector3 product;

    _bgc_fp64_restrict_vector3_get_cross_product(&product, vector1, vector2);

    if (bgc_fp64_vector3_get_squared_length(&product) > square_limit) {
        return BGC_ATTITUDE_ANY;
    }

    return scalar_product > 0.0 ? BGC_ATTITUDE_CO_DIRECTIONAL : BGC_ATTITUDE_COUNTER_DIRECTIONAL;
}