#ifndef _BGC_COTES_NUMBER_H_
#define _BGC_COTES_NUMBER_H_

#include <math.h>

#include "utilities.h"
#include "angle.h"
#include "vector2.h"
#include "matrix2x2.h"

// =================== Types ==================== //

typedef struct
{
    float _cos, _sin;
} BgcCotesNumberFP32;

typedef struct
{
    double _cos, _sin;
} BgcCotesNumberFP64;

// ================= Constants ================== //

extern const BgcCotesNumberFP32 BGC_IDLE_COTES_NUMBER_FP32;
extern const BgcCotesNumberFP64 BGC_IDLE_COTES_NUMBER_FP64;

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

inline void bgc_cotes_number_reset_fp32(BgcCotesNumberFP32* number)
{
    number->_cos = 1.0f;
    number->_sin = 0.0f;
}

inline void bgc_cotes_number_reset_fp64(BgcCotesNumberFP64* number)
{
    number->_cos = 1.0;
    number->_sin = 0.0;
}

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

void _bgc_cotes_number_normalize_fp32(const float square_modulus, BgcCotesNumberFP32* twin);

void _bgc_cotes_number_normalize_fp64(const double square_modulus, BgcCotesNumberFP64* twin);

inline void bgc_cotes_number_set_values_fp32(const float x1, const float x2, BgcCotesNumberFP32* number)
{
    const float square_modulus = x1 * x1 + x2 * x2;

    number->_cos = x1;
    number->_sin = x2;

    if (!bgc_is_sqare_unit_fp32(square_modulus)) {
        _bgc_cotes_number_normalize_fp32(square_modulus, number);
    }
}

inline void bgc_cotes_number_set_values_fp64(const double x1, const double x2, BgcCotesNumberFP64* number)
{
    const double square_modulus = x1 * x1 + x2 * x2;

    number->_cos = x1;
    number->_sin = x2;

    if (!bgc_is_sqare_unit_fp64(square_modulus)) {
        _bgc_cotes_number_normalize_fp64(square_modulus, number);
    }
}

// ================== Set Turn ================== //

inline void bgc_cotes_number_set_turn_fp32(const float angle, const BgcAngleUnitEnum unit, BgcCotesNumberFP32* number)
{
    const float radians = bgc_angle_to_radians_fp32(angle, unit);

    number->_cos = cosf(radians);
    number->_sin = sinf(radians);
}

inline void bgc_cotes_number_set_turn_fp64(const double angle, const BgcAngleUnitEnum unit, BgcCotesNumberFP64* number)
{
    const double radians = bgc_angle_to_radians_fp64(angle, unit);

    number->_cos = cos(radians);
    number->_sin = sin(radians);
}

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

inline float bgc_cotes_number_get_angle_fp32(const BgcCotesNumberFP32* number, const BgcAngleUnitEnum unit)
{
    return bgc_radians_to_units_fp32(atan2f(number->_sin, number->_cos), unit);
}

inline double bgc_cotes_number_get_angle_fp64(const BgcCotesNumberFP64* number, const BgcAngleUnitEnum unit)
{
    return bgc_radians_to_units_fp64(atan2(number->_sin, number->_cos), unit);
}

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

inline void bgc_cotes_number_copy_fp32(const BgcCotesNumberFP32* source, BgcCotesNumberFP32* destination)
{
    destination->_cos = source->_cos;
    destination->_sin = source->_sin;
}

inline void bgc_cotes_number_copy_fp64(const BgcCotesNumberFP64* source, BgcCotesNumberFP64* destination)
{
    destination->_cos = source->_cos;
    destination->_sin = source->_sin;
}

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

inline void bgc_cotes_number_swap_fp32(BgcCotesNumberFP32* number1, BgcCotesNumberFP32* number2)
{
    const float cos = number1->_cos;
    const float sin = number1->_sin;

    number1->_cos = number2->_cos;
    number1->_sin = number2->_sin;

    number2->_cos = cos;
    number2->_sin = sin;
}

inline void bgc_cotes_number_swap_fp64(BgcCotesNumberFP64* number1, BgcCotesNumberFP64* number2)
{
    const double cos = number1->_cos;
    const double sin = number1->_sin;

    number1->_cos = number2->_cos;
    number1->_sin = number2->_sin;

    number2->_cos = cos;
    number2->_sin = sin;
}

// ================== Convert =================== //

inline void bgc_cotes_number_convert_fp64_to_fp32(const BgcCotesNumberFP64* source, BgcCotesNumberFP32* destination)
{
    bgc_cotes_number_set_values_fp32((float)source->_cos, (float)source->_sin, destination);
}

inline void bgc_cotes_number_convert_fp32_to_fp64(const BgcCotesNumberFP32* source, BgcCotesNumberFP64* destination)
{
    bgc_cotes_number_set_values_fp64((double)source->_cos, (double)source->_sin, destination);
}

// ================== Negative ================== //

inline void bgc_cotes_number_make_opposite_fp32(BgcCotesNumberFP32* number)
{
    number->_cos = -number->_cos;
    number->_sin = -number->_sin;
}

inline void bgc_cotes_number_make_opposite_fp64(BgcCotesNumberFP64* number)
{
    number->_cos = -number->_cos;
    number->_sin = -number->_sin;
}

inline void bgc_cotes_number_get_opposite_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* opposite)
{
    opposite->_cos = -number->_cos;
    opposite->_sin = -number->_sin;
}

inline void bgc_cotes_number_get_opposite_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* opposite)
{
    opposite->_cos = -number->_cos;
    opposite->_sin = -number->_sin;
}

// =================== Invert =================== //

inline void bgc_cotes_number_invert_fp32(BgcCotesNumberFP32* number)
{
    number->_sin = -number->_sin;
}

inline void bgc_cotes_number_invert_fp64(BgcCotesNumberFP64* number)
{
    number->_sin = -number->_sin;
}

inline void bgc_cotes_number_get_inverse_fp32(const BgcCotesNumberFP32* number, BgcCotesNumberFP32* inverse)
{
    inverse->_cos = number->_cos;
    inverse->_sin = -number->_sin;
}

inline void bgc_cotes_number_get_inverse_fp64(const BgcCotesNumberFP64* number, BgcCotesNumberFP64* inverse)
{
    inverse->_cos = number->_cos;
    inverse->_sin = -number->_sin;
}

// ================= Exponation ================= //

inline void bgc_cotes_number_get_exponation_fp32(const BgcCotesNumberFP32* base, const float exponent, BgcCotesNumberFP32* power)
{
    const float power_angle = exponent * atan2f(base->_sin, base->_cos);

    power->_cos = cosf(power_angle);
    power->_sin = sinf(power_angle);
}

inline void bgc_cotes_number_get_exponation_fp64(const BgcCotesNumberFP64* base, const double exponent, BgcCotesNumberFP64* power)
{
    const double power_angle = exponent * atan2(base->_sin, base->_cos);

    power->_cos = cos(power_angle);
    power->_sin = sin(power_angle);
}

// ================ Combination ================= //

inline void bgc_cotes_number_combine_fp32(const BgcCotesNumberFP32* number1, const BgcCotesNumberFP32* number2, BgcCotesNumberFP32* result)
{
    bgc_cotes_number_set_values_fp32(
        number1->_cos * number2->_cos - number1->_sin * number2->_sin,
        number1->_cos * number2->_sin + number1->_sin * number2->_cos,
        result
    );
}

inline void bgc_cotes_number_combine_fp64(const BgcCotesNumberFP64* number1, const BgcCotesNumberFP64* number2, BgcCotesNumberFP64* result)
{
    bgc_cotes_number_set_values_fp64(
        number1->_cos * number2->_cos - number1->_sin * number2->_sin,
        number1->_cos * number2->_sin + number1->_sin * number2->_cos,
        result
    );
}

// ================= Exclusion ================== //

inline void bgc_cotes_number_exclude_fp32(const BgcCotesNumberFP32* base, const BgcCotesNumberFP32* excludant, BgcCotesNumberFP32* difference)
{
    bgc_cotes_number_set_values_fp32(
        base->_cos * excludant->_cos + base->_sin * excludant->_sin,
        base->_sin * excludant->_cos - base->_cos * excludant->_sin,
        difference
    );
}

inline void bgc_cotes_number_exclude_fp64(const BgcCotesNumberFP64* base, const BgcCotesNumberFP64* excludant, BgcCotesNumberFP64* difference)
{
    bgc_cotes_number_set_values_fp64(
        base->_cos * excludant->_cos + base->_sin * excludant->_sin,
        base->_sin * excludant->_cos - base->_cos * excludant->_sin,
        difference
    );
}

// ============== Rotation Matrix =============== //

inline void bgc_cotes_number_get_rotation_matrix_fp32(const BgcCotesNumberFP32* number, BgcMatrix2x2FP32* matrix)
{
    matrix->r1c1 = number->_cos;
    matrix->r1c2 = -number->_sin;
    matrix->r2c1 = number->_sin;
    matrix->r2c2 = number->_cos;
}

inline void bgc_cotes_number_get_rotation_matrix_fp64(const BgcCotesNumberFP64* number, BgcMatrix2x2FP64* matrix)
{
    matrix->r1c1 = number->_cos;
    matrix->r1c2 = -number->_sin;
    matrix->r2c1 = number->_sin;
    matrix->r2c2 = number->_cos;
}

// ============== Reverse Matrix ================ //

inline void bgc_cotes_number_get_reverse_matrix_fp32(const BgcCotesNumberFP32* number, BgcMatrix2x2FP32* matrix)
{
    matrix->r1c1 = number->_cos;
    matrix->r1c2 = number->_sin;
    matrix->r2c1 = -number->_sin;
    matrix->r2c2 = number->_cos;
}

inline void bgc_cotes_number_get_reverse_matrix_fp64(const BgcCotesNumberFP64* number, BgcMatrix2x2FP64* matrix)
{
    matrix->r1c1 = number->_cos;
    matrix->r1c2 = number->_sin;
    matrix->r2c1 = -number->_sin;
    matrix->r2c2 = number->_cos;
}

// ================ Turn Vector ================= //

inline void bgc_cotes_number_turn_vector_fp32(const BgcCotesNumberFP32* number, const BgcVector2FP32* vector, BgcVector2FP32* result)
{
    const float x1 = number->_cos * vector->x1 - number->_sin * vector->x2;
    const float x2 = number->_sin * vector->x1 + number->_cos * vector->x2;

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

inline void bgc_cotes_number_turn_vector_fp64(const BgcCotesNumberFP64* number, const BgcVector2FP64* vector, BgcVector2FP64* result)
{
    const double x1 = number->_cos * vector->x1 - number->_sin * vector->x2;
    const double x2 = number->_sin * vector->x1 + number->_cos * vector->x2;

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

// ============ Turn Vector Backward ============ //

inline void bgc_cotes_number_turn_vector_back_fp32(const BgcCotesNumberFP32* number, const BgcVector2FP32* vector, BgcVector2FP32* result)
{
    const float x1 = number->_sin * vector->x2 + number->_cos * vector->x1;
    const float x2 = number->_cos * vector->x2 - number->_sin * vector->x1;

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

inline void bgc_cotes_number_turn_vector_back_fp64(const BgcCotesNumberFP64* number, const BgcVector2FP64* vector, BgcVector2FP64* result)
{
    const double x1 = number->_sin * vector->x2 + number->_cos * vector->x1;
    const double x2 = number->_cos * vector->x2 - number->_sin * vector->x1;

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

// ================== Are Close ================= //

inline int bgc_cotes_number_are_close_fp32(const BgcCotesNumberFP32* number1, const BgcCotesNumberFP32* number2)
{
    const float d_cos = number1->_cos - number2->_cos;
    const float d_sin = number1->_sin - number2->_sin;

    return d_cos * d_cos + d_sin * d_sin <= BGC_SQUARE_EPSYLON_FP32;
}

inline int bgc_cotes_number_are_close_fp64(const BgcCotesNumberFP64* number1, const BgcCotesNumberFP64* number2)
{
    const double d_cos = number1->_cos - number2->_cos;
    const double d_sin = number1->_sin - number2->_sin;

    return d_cos * d_cos + d_sin * d_sin <= BGC_SQUARE_EPSYLON_FP64;
}

#endif