#ifndef _GEOMETRY_VECTOR2_H_
#define _GEOMETRY_VECTOR2_H_

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

#include <math.h>

typedef struct
{
    float x1, x2;
} BgFP32Vector2;

typedef struct
{
    double x1, x2;
} BgFP64Vector2;

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

static inline void bg_fp32_vector2_reset(BgFP32Vector2* vector)
{
    vector->x1 = 0.0f;
    vector->x2 = 0.0f;
}

static inline void bg_fp64_vector2_reset(BgFP64Vector2* vector)
{
    vector->x1 = 0.0;
    vector->x2 = 0.0;
}

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

static inline void bg_fp32_vector2_set_values(const float x1, const float x2, BgFP32Vector2* to)
{
    to->x1 = x1;
    to->x2 = x2;
}

static inline void bg_fp64_vector2_set_values(const double x1, const double x2, BgFP64Vector2* to)
{
    to->x1 = x1;
    to->x2 = x2;
}

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

static inline void bg_fp32_vector2_copy(const BgFP32Vector2* from, BgFP32Vector2* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
}

static inline void bg_fp64_vector2_copy(const BgFP64Vector2* from, BgFP64Vector2* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
}

// ==================== Swap ==================== //

static inline void bg_fp32_vector2_swap(BgFP32Vector2* vector1, BgFP32Vector2* vector2)
{
    const float x1 = vector2->x1;
    const float x2 = vector2->x2;

    vector2->x1 = vector1->x1;
    vector2->x2 = vector1->x2;

    vector1->x1 = x1;
    vector1->x2 = x2;
}

static inline void bg_fp64_vector2_swap(BgFP64Vector2* vector1, BgFP64Vector2* vector2)
{
    const double x1 = vector2->x1;
    const double x2 = vector2->x2;

    vector2->x1 = vector1->x1;
    vector2->x2 = vector1->x2;

    vector1->x1 = x1;
    vector1->x2 = x2;
}

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

static inline void bg_fp32_vector2_set_from_fp64(const BgFP64Vector2* from, BgFP32Vector2* to)
{
    to->x1 = (float)from->x1;
    to->x2 = (float)from->x2;
}

static inline void bg_fp64_vector2_set_from_fp32(const BgFP32Vector2* from, BgFP64Vector2* to)
{
    to->x1 = from->x1;
    to->x2 = from->x2;
}

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

static inline void bg_fp32_vector2_set_reverse(const BgFP32Vector2* from, BgFP32Vector2* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
}

static inline void bg_fp64_vector2_set_reverse(const BgFP64Vector2* from, BgFP64Vector2* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
}

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

static inline void bg_fp32_vector2_set_reverse_fp64(const BgFP64Vector2* from, BgFP32Vector2* to)
{
    to->x1 = (float) -from->x1;
    to->x2 = (float) -from->x2;
}

static inline void bg_fp64_vector2_set_reverse_fp32(const BgFP32Vector2* from, BgFP64Vector2* to)
{
    to->x1 = -from->x1;
    to->x2 = -from->x2;
}

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

static inline float bg_fp32_vector2_get_square_modulus(const BgFP32Vector2* vector)
{
    return vector->x1 * vector->x1 + vector->x2 * vector->x2;
}

static inline double bg_fp64_vector2_get_square_modulus(const BgFP64Vector2* vector)
{
    return vector->x1 * vector->x1 + vector->x2 * vector->x2;
}

static inline float bg_fp32_vector2_get_modulus(const BgFP32Vector2* vector)
{
    return sqrtf(bg_fp32_vector2_get_square_modulus(vector));
}

static inline double bg_fp64_vector2_get_modulus(const BgFP64Vector2* vector)
{
    return sqrt(bg_fp64_vector2_get_square_modulus(vector));
}

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

static inline int bg_fp32_vector2_is_zero(const BgFP32Vector2* vector)
{
    return bg_fp32_vector2_get_square_modulus(vector) <= BG_FP32_SQUARE_EPSYLON;
}

static inline int bg_fp64_vector2_is_zero(const BgFP64Vector2* vector)
{
    return bg_fp64_vector2_get_square_modulus(vector) <= BG_FP64_SQUARE_EPSYLON;
}

static inline int bg_fp32_vector2_is_unit(const BgFP32Vector2* vector)
{
    const float square_modulus = bg_fp32_vector2_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_vector2_is_unit(const BgFP64Vector2* vector)
{
    const double square_modulus = bg_fp64_vector2_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_vector2_add(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* sum)
{
    sum->x1 = vector1->x1 + vector2->x1;
    sum->x2 = vector1->x2 + vector2->x2;
}

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

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

static inline void bg_fp32_vector2_subtract(const BgFP32Vector2* minuend, const BgFP32Vector2* subtrahend, BgFP32Vector2* difference)
{
    difference->x1 = minuend->x1 - subtrahend->x1;
    difference->x2 = minuend->x2 - subtrahend->x2;
}

static inline void bg_fp64_vector2_subtract(const BgFP64Vector2* minuend, const BgFP64Vector2* subtrahend, BgFP64Vector2* difference)
{
    difference->x1 = minuend->x1 - subtrahend->x1;
    difference->x2 = minuend->x2 - subtrahend->x2;
}

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

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

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

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

static inline void bg_fp32_vector2_divide(const BgFP32Vector2* dividend, const float divisor, BgFP32Vector2* quotient)
{
    bg_fp32_vector2_multiply(dividend, 1.0f / divisor, quotient);
}

static inline void bg_fp64_vector2_divide(const BgFP64Vector2* dividend, const double divisor, BgFP64Vector2* quotient)
{
    bg_fp64_vector2_multiply(dividend, 1.0 / divisor, quotient);
}

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

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

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

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

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

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

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

static inline void bg_fp32_vector2_get_mean3(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const BgFP32Vector2* vector3, BgFP32Vector2* 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;
}

static inline void bg_fp64_vector2_get_mean3(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, const BgFP64Vector2* vector3, BgFP64Vector2* 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;
}

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

static inline float bg_fp32_vector2_scalar_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{
    return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2;
}

static inline double bg_fp64_vector2_scalar_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{
    return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2;
}

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

static inline float bg_fp32_vector2_cross_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{
    return vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;
}

static inline double bg_fp64_vector2_cross_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{
    return vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1;
}

// ============== Complex Product =============== //

static inline void bg_fp32_vector2_complex_product(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, BgFP32Vector2* result)
{
    const float x1 = vector1->x1 * vector2->x1 - vector1->x2 * vector2->x2;
    const float x2 = vector1->x1 * vector2->x2 + vector1->x2 * vector2->x1;

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

static inline void bg_fp64_vector2_complex_product(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, BgFP64Vector2* result)
{
    const double x1 = vector1->x1 * vector2->x1 - vector1->x2 * vector2->x2;
    const double x2 = vector1->x1 * vector2->x2 + vector1->x2 * vector2->x1;

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

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

static inline int bg_fp32_vector2_normalize(BgFP32Vector2* vector)
{
    const float square_modulus = bg_fp32_vector2_get_square_modulus(vector);

    if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
        return 1;
    }

    if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
        bg_fp32_vector2_reset(vector);
        return 0;
    }

    bg_fp32_vector2_multiply(vector, sqrtf(1.0f / square_modulus), vector);
    return 1;
}

static inline int bg_fp64_vector2_normalize(BgFP64Vector2* vector)
{
    const double square_modulus = bg_fp64_vector2_get_square_modulus(vector);

    if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
        return 1;
    }

    if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
        bg_fp64_vector2_reset(vector);
        return 0;
    }

    bg_fp64_vector2_multiply(vector, sqrt(1.0 / square_modulus), vector);
    return 1;
}

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

static inline int bg_fp32_vector2_set_normalized(const BgFP32Vector2* vector, BgFP32Vector2* result)
{
    bg_fp32_vector2_copy(vector, result);
    return bg_fp32_vector2_normalize(result);
}

static inline int bg_fp64_vector2_set_normalized(const BgFP64Vector2* vector, BgFP64Vector2* result)
{
    bg_fp64_vector2_copy(vector, result);
    return bg_fp64_vector2_normalize(result);
}

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

float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit);

double bg_fp64_vector2_get_angle(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2, const angle_unit_t unit);

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

static inline float bg_fp32_vector2_get_square_distance(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{
    const float dx1 = (vector1->x1 - vector2->x1);
    const float dx2 = (vector1->x2 - vector2->x2);

    return dx1 * dx1 + dx2 * dx2;
}

static inline double bg_fp64_vector2_get_square_distance(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{
    const double dx1 = (vector1->x1 - vector2->x1);
    const double dx2 = (vector1->x2 - vector2->x2);

    return dx1 * dx1 + dx2 * dx2;
}

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

static inline float bg_fp32_vector2_get_distance(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{
    return sqrtf(bg_fp32_vector2_get_square_distance(vector1, vector2));
}

static inline double bg_fp64_vector2_get_distance(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{
    return sqrt(bg_fp64_vector2_get_square_distance(vector1, vector2));
}

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

static inline int bg_fp32_vector2_are_equal(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2)
{
    const float square_modulus1 = bg_fp32_vector2_get_square_modulus(vector1);
    const float square_modulus2 = bg_fp32_vector2_get_square_modulus(vector2);
    const float square_modulus3 = bg_fp32_vector2_get_square_distance(vector1, vector2);

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

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

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

static inline int bg_fp64_vector2_are_equal(const BgFP64Vector2* vector1, const BgFP64Vector2* vector2)
{
    const double square_modulus1 = bg_fp64_vector2_get_square_modulus(vector1);
    const double square_modulus2 = bg_fp64_vector2_get_square_modulus(vector2);
    const double square_modulus3 = bg_fp64_vector2_get_square_distance(vector1, vector2);

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

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

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

#endif