#ifndef _GEOMETRY_VECTOR3_H_
#define _GEOMETRY_VECTOR3_H_

#include "basis.h"
#include "angle.h"

#include <math.h>

// ================== Vector3 =================== //
//
typedef struct
{
    float x1, x2, x3;
} BgFP32Vector3;

typedef struct
{
    double x1, x2, x3;
} BgFP64Vector3;

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

static inline void bg_fp32_vector3_reset(BgFP32Vector3* vector)
{
    vector->x1 = 0.0f;
    vector->x2 = 0.0f;
    vector->x3 = 0.0f;
}

static inline void bg_fp64_vector3_reset(BgFP64Vector3* vector)
{
    vector->x1 = 0.0;
    vector->x2 = 0.0;
    vector->x3 = 0.0;
}

// ==================== Set ===================== //

static inline void bg_fp32_vector3_set_values(const float x1, const float x2, const float x3, BgFP32Vector3* to)
{
    to->x1 = x1;
    to->x2 = x2;
    to->x3 = x3;
}

static inline void bg_fp64_vector3_set_values(const double x1, const double x2, const double x3, BgFP64Vector3* to)
{
    to->x1 = x1;
    to->x2 = x2;
    to->x3 = x3;
}

// ==================== Copy ==================== //

static inline void bg_fp32_vector3_copy(const BgFP32Vector3* from, BgFP32Vector3* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
    to->x3 = from->x3;
}

static inline void bg_fp64_vector3_copy(const BgFP64Vector3* from, BgFP64Vector3* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
    to->x3 = from->x3;
}

// ============= Copy to twin type ============== //

static inline void bg_fp32_vector3_set_from_fp64(const BgFP64Vector3* from, BgFP32Vector3* to)
{
    to->x1 = (float) from->x1;
    to->x2 = (float) from->x2;
    to->x3 = (float) from->x3;
}

static inline void bg_fp64_vector3_set_from_fp32(const BgFP32Vector3* from, BgFP64Vector3* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
    to->x3 = from->x3;
}

// =================== Reverse ================== //

static inline void bg_fp32_vector3_set_reverse(const BgFP32Vector3* from, BgFP32Vector3* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
    to->x3 = -from->x3;
}

static inline void bg_fp64_vector3_set_reverse(const BgFP64Vector3* from, BgFP64Vector3* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
    to->x3 = -from->x3;
}

// ============= Reverse twin type ============== //

static inline void bg_fp32_vector3_set_reverse_fp64(const BgFP64Vector3* from, BgFP32Vector3* to)
{
    to->x1 = (float) -from->x1;
    to->x2 = (float) -from->x2;
    to->x3 = (float) -from->x3;
}

static inline void bg_fp64_vector3_set_reverse_fp32(const BgFP32Vector3* from, BgFP64Vector3* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
    to->x3 = -from->x3;
}

// =================== Module =================== //

static inline float bg_fp32_vector3_get_square_modulus(const BgFP32Vector3* vector)
{
    return vector->x1 * vector->x1 + vector->x2 * vector->x2 + vector->x3 * vector->x3;
}

static inline double bg_fp64_vector3_get_square_modulus(const BgFP64Vector3* vector)
{
    return vector->x1 * vector->x1 + vector->x2 * vector->x2 + vector->x3 * vector->x3;
}

static inline float bg_fp32_vector3_get_modulus(const BgFP32Vector3* vector)
{
    return sqrtf(bg_fp32_vector3_get_square_modulus(vector));
}

static inline double bg_fp64_vector3_get_modulus(const BgFP64Vector3* vector)
{
    return sqrt(bg_fp64_vector3_get_square_modulus(vector));
}

// ================= Comparison ================= //

static inline int bg_fp32_vector3_is_zero(const BgFP32Vector3* vector)
{
    return bg_fp32_vector3_get_square_modulus(vector) <= BG_FP32_SQUARE_EPSYLON;
}

static inline int bg_fp64_vector3_is_zero(const BgFP64Vector3* vector)
{
    return bg_fp64_vector3_get_square_modulus(vector) <= BG_FP64_SQUARE_EPSYLON;
}

static inline int bg_fp32_vector3_is_unit(const BgFP32Vector3* vector)
{
    const float square_modulus = bg_fp32_vector3_get_square_modulus(vector);

    return 1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON;
}

static inline int bg_fp64_vector3_is_unit(const BgFP64Vector3* vector)
{
    const double square_modulus = bg_fp64_vector3_get_square_modulus(vector);

    return 1.0f - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP64_TWO_EPSYLON;
}

// ==================== Add ===================== //

static inline void bg_fp32_vector3_add(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* sum)
{
    sum->x1 = vector1->x1 + vector2->x1;
    sum->x2 = vector1->x2 + vector2->x2;
    sum->x3 = vector1->x3 + vector2->x3;
}

static inline void bg_fp64_vector3_add(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* sum)
{
    sum->x1 = vector1->x1 + vector2->x1;
    sum->x2 = vector1->x2 + vector2->x2;
    sum->x3 = vector1->x3 + vector2->x3;
}

// ================ Subtraction ================= //

static inline void bg_fp32_vector3_subtract(const BgFP32Vector3* minuend, const BgFP32Vector3* subtrahend, BgFP32Vector3* difference)
{
    difference->x1 = minuend->x1 - subtrahend->x1;
    difference->x2 = minuend->x2 - subtrahend->x2;
    difference->x3 = minuend->x3 - subtrahend->x3;
}

static inline void bg_fp64_vector3_subtract(const BgFP64Vector3* minuend, const BgFP64Vector3* subtrahend, BgFP64Vector3* difference)
{
    difference->x1 = minuend->x1 - subtrahend->x1;
    difference->x2 = minuend->x2 - subtrahend->x2;
    difference->x3 = minuend->x3 - subtrahend->x3;
}

// =============== Multiplication =============== //

static inline void bg_fp32_vector3_multiply(const BgFP32Vector3* multiplicand, const float multiplier, BgFP32Vector3* product)
{
    product->x1 = multiplicand->x1 * multiplier;
    product->x2 = multiplicand->x2 * multiplier;
    product->x3 = multiplicand->x3 * multiplier;
}

static inline void bg_fp64_vector3_multiply(const BgFP64Vector3* multiplicand, const double multiplier, BgFP64Vector3* product)
{
    product->x1 = multiplicand->x1 * multiplier;
    product->x2 = multiplicand->x2 * multiplier;
    product->x3 = multiplicand->x3 * multiplier;
}

// ================== Division ================== //

static inline void bg_fp32_vector3_divide(const BgFP32Vector3* dividend, const float divisor, BgFP32Vector3* quotient)
{
    bg_fp32_vector3_multiply(dividend, 1.0f / divisor, quotient);
}

static inline void bg_fp64_vector3_divide(const BgFP64Vector3* dividend, const double divisor, BgFP64Vector3* quotient)
{
    bg_fp64_vector3_multiply(dividend, 1.0 / divisor, quotient);
}

// ================ Append scaled =============== //

static inline void bg_fp32_vector3_append_scaled(BgFP32Vector3* basic_vector, const BgFP32Vector3* scalable_vector, const float scale)
{
    basic_vector->x1 += scalable_vector->x1 * scale;
    basic_vector->x2 += scalable_vector->x2 * scale;
    basic_vector->x3 += scalable_vector->x3 * scale;
}

static inline void bg_fp64_vector3_append_scaled(BgFP64Vector3* basic_vector, const BgFP64Vector3* scalable_vector, const double scale)
{
    basic_vector->x1 += scalable_vector->x1 * scale;
    basic_vector->x2 += scalable_vector->x2 * scale;
    basic_vector->x3 += scalable_vector->x3 * scale;
}

// ================== Average2 ================== //

static inline void bg_fp32_vector3_get_mean2(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result)
{
    result->x1 = (vector1->x1 + vector2->x1) * 0.5f;
    result->x2 = (vector1->x2 + vector2->x2) * 0.5f;
    result->x3 = (vector1->x3 + vector2->x3) * 0.5f;
}

static inline void bg_fp64_vector3_get_mean2(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result)
{
    result->x1 = (vector1->x1 + vector2->x1) * 0.5;
    result->x2 = (vector1->x2 + vector2->x2) * 0.5;
    result->x3 = (vector1->x3 + vector2->x3) * 0.5;
}

// ================== Average3 ================== //

static inline void bg_fp32_vector3_get_mean3(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3, BgFP32Vector3* result)
{
    result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BG_FP32_ONE_THIRD;
    result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BG_FP32_ONE_THIRD;
    result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BG_FP32_ONE_THIRD;
}

static inline void bg_fp64_vector3_get_mean3(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3, BgFP64Vector3* result)
{
    result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * BG_FP64_ONE_THIRD;
    result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * BG_FP64_ONE_THIRD;
    result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * BG_FP64_ONE_THIRD;
}

// =============== Scalar Product =============== //

static inline float bg_fp32_vector3_scalar_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2)
{
    return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3;
}

static inline double bg_fp64_vector3_scalar_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2)
{
    return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3;
}

// =============== Triple Product =============== //

static inline float bg_fp32_vector3_triple_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3)
{
    return vector1->x1 * (vector2->x2 * vector3->x3 - vector2->x3 * vector3->x2)
         + vector1->x2 * (vector2->x3 * vector3->x1 - vector2->x1 * vector3->x3)
         + vector1->x3 * (vector2->x1 * vector3->x2 - vector2->x2 * vector3->x1);
}

static inline double bg_fp64_vector3_triple_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3)
{
    return vector1->x1 * (vector2->x2 * vector3->x3 - vector2->x3 * vector3->x2)
         + vector1->x2 * (vector2->x3 * vector3->x1 - vector2->x1 * vector3->x3)
         + vector1->x3 * (vector2->x1 * vector3->x2 - vector2->x2 * vector3->x1);
}

// =============== Cross Product ================ //

static inline void bg_fp32_vector3_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, BgFP32Vector3* result)
{
    const float x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2;
    const float x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3;
    const float x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;

    result->x1 = x1;
    result->x2 = x2;
    result->x3 = x3;
}

static inline void bg_fp64_vector3_cross_product(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, BgFP64Vector3* result)
{
    const double x1 = vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2;
    const double x2 = vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3;
    const double x3 = vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;

    result->x1 = x1;
    result->x2 = x2;
    result->x3 = x3;
}

// ============ Double Cross Product ============ //

static inline void bg_fp32_vector3_double_cross_product(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const BgFP32Vector3* vector3, BgFP32Vector3* result)
{
    const float ac = bg_fp32_vector3_scalar_product(vector1, vector3);
    const float ab = bg_fp32_vector3_scalar_product(vector1, vector2);

    result->x1 = vector2->x1 * ac - vector3->x1 * ab;
    result->x2 = vector2->x2 * ac - vector3->x2 * ab;
    result->x3 = vector2->x3 * ac - vector3->x3 * ab;
}

static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const BgFP64Vector3* vector3, BgFP64Vector3* result)
{
    const double ac = bg_fp64_vector3_scalar_product(vector1, vector3);
    const double ab = bg_fp64_vector3_scalar_product(vector1, vector2);

    result->x1 = vector2->x1 * ac - vector3->x1 * ab;
    result->x2 = vector2->x2 * ac - vector3->x2 * ab;
    result->x3 = vector2->x3 * ac - vector3->x3 * ab;
}

// =============== Normalization ================ //

int bg_fp32_vector3_normalize(BgFP32Vector3* vector);

int bg_fp64_vector3_normalize(BgFP64Vector3* vector);

// =============== Get Normalized =============== //

static inline int bg_fp32_vector3_set_normalized(const BgFP32Vector3* vector, BgFP32Vector3* result)
{
    bg_fp32_vector3_copy(vector, result);
    return bg_fp32_vector3_normalize(result);
}

static inline int bg_fp64_vector3_set_normalized(const BgFP64Vector3* vector, BgFP64Vector3* result)
{
    bg_fp64_vector3_copy(vector, result);
    return bg_fp64_vector3_normalize(result);
}

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

float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit);

double bg_fp64_vector3_get_angle(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2, const angle_unit_t unit);

// =============== Square Distance ============== //

static inline float bg_fp32_vector3_get_square_distance(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2)
{
    const float dx1 = (vector1->x1 - vector2->x1);
    const float dx2 = (vector1->x2 - vector2->x2);
    const float dx3 = (vector1->x3 - vector2->x3);

    return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
}

static inline double bg_fp64_vector3_get_square_distance(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2)
{
    const double dx1 = (vector1->x1 - vector2->x1);
    const double dx2 = (vector1->x2 - vector2->x2);
    const double dx3 = (vector1->x3 - vector2->x3);

    return dx1 * dx1 + dx2 * dx2 + dx3 * dx3;
}

// ================== Distance ================== //

static inline float bg_fp32_vector3_get_distance(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2)
{
    return sqrtf(bg_fp32_vector3_get_square_distance(vector1, vector2));
}

static inline double bg_fp64_vector3_get_distance(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2)
{
    return sqrt(bg_fp64_vector3_get_square_distance(vector1, vector2));
}

// ================== Are Equal ================= //

static inline int bg_fp32_vector3_are_equal(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2)
{
    const float square_modulus1 = bg_fp32_vector3_get_square_modulus(vector1);
    const float square_modulus2 = bg_fp32_vector3_get_square_modulus(vector2);
    const float square_modulus3 = bg_fp32_vector3_get_square_distance(vector1, vector2);

    // 3.0f means dimension amount
    if (square_modulus1 < BG_FP32_EPSYLON_EFFECTIVENESS_LIMIT || square_modulus2 < BG_FP32_EPSYLON_EFFECTIVENESS_LIMIT) {
        return square_modulus3 < (3.0f * BG_FP32_SQUARE_EPSYLON);
    }

    if (square_modulus1 <= square_modulus2) {
        return square_modulus3 <= (3.0f * BG_FP32_SQUARE_EPSYLON) * square_modulus2;
    }

    return square_modulus3 <= (3.0f * BG_FP32_SQUARE_EPSYLON) * square_modulus1;
}

static inline int bg_fp64_vector3_are_equal(const BgFP64Vector3* vector1, const BgFP64Vector3* vector2)
{
    const double square_modulus1 = bg_fp64_vector3_get_square_modulus(vector1);
    const double square_modulus2 = bg_fp64_vector3_get_square_modulus(vector2);
    const double square_modulus3 = bg_fp64_vector3_get_square_distance(vector1, vector2);

    // 3.0 means dimension amount
    if (square_modulus1 < BG_FP64_EPSYLON_EFFECTIVENESS_LIMIT || square_modulus2 < BG_FP64_EPSYLON_EFFECTIVENESS_LIMIT) {
        return square_modulus3 < (3.0 * BG_FP64_SQUARE_EPSYLON);
    }

    if (square_modulus1 <= square_modulus2) {
        return square_modulus3 <= (3.0 * BG_FP64_SQUARE_EPSYLON) * square_modulus2;
    }

    return square_modulus3 <= (3.0 * BG_FP64_SQUARE_EPSYLON) * square_modulus1;
}

#endif