#ifndef _BGC_TURN2_H_INCLUDED_
#define _BGC_TURN2_H_INCLUDED_

#include <math.h>

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

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

typedef struct
{
    float _cos, _sin;
} BGC_FP32_Turn2;

typedef struct
{
    double _cos, _sin;
} BGC_FP64_Turn2;

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

extern const BGC_FP32_Turn2 BGC_FP32_IDLE_TURN2;
extern const BGC_FP64_Turn2 BGC_FP64_IDLE_TURN2;

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

inline void bgc_fp32_turn2_reset(BGC_FP32_Turn2* turn)
{
    turn->_cos = 1.0f;
    turn->_sin = 0.0f;
}

inline void bgc_fp64_turn2_reset(BGC_FP64_Turn2* turn)
{
    turn->_cos = 1.0;
    turn->_sin = 0.0;
}

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

inline void bgc_fp32_turn2_make_for_angle(BGC_FP32_Turn2* turn, const float angle, const int angle_unit)
{
    const float radians = bgc_fp32_angle_to_radians(angle, angle_unit);

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

inline void bgc_fp64_turn2_make_for_angle(BGC_FP64_Turn2* turn, const double angle, const int angle_unit)
{
    const double radians = bgc_fp64_angle_to_radians(angle, angle_unit);

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


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

inline int bgc_fp32_turn2_is_idle(const BGC_FP32_Turn2* turn)
{
    return bgc_fp32_is_unit(turn->_cos) && bgc_fp32_is_zero(turn->_sin);
}

inline int bgc_fp64_turn2_is_idle(const BGC_FP64_Turn2* turn)
{
    return bgc_fp64_is_unit(turn->_cos) && bgc_fp64_is_zero(turn->_sin);
}

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

void _bgc_fp32_turn2_normalize(BGC_FP32_Turn2* twin);

void _bgc_fp64_turn2_normalize(BGC_FP64_Turn2* twin);

inline void bgc_fp32_turn2_make(BGC_FP32_Turn2* turn, const float x1, const float x2)
{
    const float square_modulus = x1 * x1 + x2 * x2;

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

    if (!bgc_fp32_is_square_unit(square_modulus)) {
        _bgc_fp32_turn2_normalize(turn);
    }
}

inline void bgc_fp64_turn2_make(BGC_FP64_Turn2* turn, const double x1, const double x2)
{
    const double square_modulus = x1 * x1 + x2 * x2;

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

    if (!bgc_fp64_is_square_unit(square_modulus)) {
        _bgc_fp64_turn2_normalize(turn);
    }
}

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

inline float bgc_fp32_turn2_get_angle(const BGC_FP32_Turn2* turn, const int angle_unit)
{
    return bgc_fp32_radians_to_units(atan2f(turn->_sin, turn->_cos), angle_unit);
}

inline double bgc_fp64_turn2_get_angle(const BGC_FP64_Turn2* turn, const int angle_unit)
{
    return bgc_fp64_radians_to_units(atan2(turn->_sin, turn->_cos), angle_unit);
}

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

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

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

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

inline void bgc_fp32_turn2_swap(BGC_FP32_Turn2* turn1, BGC_FP32_Turn2* turn2)
{
    const float cos = turn1->_cos;
    const float sin = turn1->_sin;

    turn1->_cos = turn2->_cos;
    turn1->_sin = turn2->_sin;

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

inline void bgc_fp64_turn2_swap(BGC_FP64_Turn2* turn1, BGC_FP64_Turn2* turn2)
{
    const double cos = turn1->_cos;
    const double sin = turn1->_sin;

    turn1->_cos = turn2->_cos;
    turn1->_sin = turn2->_sin;

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

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

inline void bgc_fp64_turn2_convert_to_fp32(BGC_FP32_Turn2* destination, const BGC_FP64_Turn2* source)
{
    bgc_fp32_turn2_make(destination, (float)source->_cos, (float)source->_sin);
}

inline void bgc_fp32_turn2_convert_to_fp64(BGC_FP64_Turn2* destination, const BGC_FP32_Turn2* source)
{
    bgc_fp64_turn2_make(destination, (double)source->_cos, (double)source->_sin);
}

// =================== Revert =================== //

inline void bgc_fp32_turn2_revert(BGC_FP32_Turn2* turn)
{
    turn->_sin = -turn->_sin;
}

inline void bgc_fp64_turn2_revert(BGC_FP64_Turn2* turn)
{
    turn->_sin = -turn->_sin;
}

inline void bgc_fp32_turn2_get_reverse(BGC_FP32_Turn2* reverse, const BGC_FP32_Turn2* turn)
{
    reverse->_cos = turn->_cos;
    reverse->_sin = -turn->_sin;
}

inline void bgc_fp64_turn2_get_reverse(BGC_FP64_Turn2* reverse, const BGC_FP64_Turn2* turn)
{
    reverse->_cos = turn->_cos;
    reverse->_sin = -turn->_sin;
}

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

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

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

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

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

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

inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* combination, const BGC_FP32_Turn2* turn1, const BGC_FP32_Turn2* turn2)
{
    bgc_fp32_turn2_make(
        combination,
        turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin,
        turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos
    );
}

inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* combination, const BGC_FP64_Turn2* turn1, const BGC_FP64_Turn2* turn2)
{
    bgc_fp64_turn2_make(
        combination,
        turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin,
        turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos
    );
}

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

inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* difference, const BGC_FP32_Turn2* base, const BGC_FP32_Turn2* excludant)
{
    bgc_fp32_turn2_make(
        difference,
        base->_cos * excludant->_cos + base->_sin * excludant->_sin,
        base->_sin * excludant->_cos - base->_cos * excludant->_sin
    );
}

inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* difference, const BGC_FP64_Turn2* base, const BGC_FP64_Turn2* excludant)
{
    bgc_fp64_turn2_make(
        difference,
        base->_cos * excludant->_cos + base->_sin * excludant->_sin,
        base->_sin * excludant->_cos - base->_cos * excludant->_sin
    );
}

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

inline void bgc_fp32_turn2_get_rotation_matrix(BGC_FP32_Matrix2x2* matrix, const BGC_FP32_Turn2* turn)
{
    matrix->r1c1 = turn->_cos;
    matrix->r1c2 = -turn->_sin;
    matrix->r2c1 = turn->_sin;
    matrix->r2c2 = turn->_cos;
}

inline void bgc_fp64_turn2_get_rotation_matrix(BGC_FP64_Matrix2x2* matrix, const BGC_FP64_Turn2* turn)
{
    matrix->r1c1 = turn->_cos;
    matrix->r1c2 = -turn->_sin;
    matrix->r2c1 = turn->_sin;
    matrix->r2c2 = turn->_cos;
}

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

inline void bgc_fp32_turn2_get_reverse_matrix(BGC_FP32_Matrix2x2* matrix, const BGC_FP32_Turn2* turn)
{
    matrix->r1c1 = turn->_cos;
    matrix->r1c2 = turn->_sin;
    matrix->r2c1 = -turn->_sin;
    matrix->r2c2 = turn->_cos;
}

inline void bgc_fp64_turn2_get_reverse_matrix(BGC_FP64_Matrix2x2* matrix, const BGC_FP64_Turn2* turn)
{
    matrix->r1c1 = turn->_cos;
    matrix->r1c2 = turn->_sin;
    matrix->r2c1 = -turn->_sin;
    matrix->r2c2 = turn->_cos;
}

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

inline void bgc_fp32_turn2_vector(BGC_FP32_Vector2* turned_vector, const BGC_FP32_Turn2* turn, const BGC_FP32_Vector2* vector)
{
    const float x1 = turn->_cos * vector->x1 - turn->_sin * vector->x2;
    const float x2 = turn->_sin * vector->x1 + turn->_cos * vector->x2;

    turned_vector->x1 = x1;
    turned_vector->x2 = x2;
}

inline void bgc_fp64_turn2_vector(BGC_FP64_Vector2* turned_vector, const BGC_FP64_Turn2* turn, const BGC_FP64_Vector2* vector)
{
    const double x1 = turn->_cos * vector->x1 - turn->_sin * vector->x2;
    const double x2 = turn->_sin * vector->x1 + turn->_cos * vector->x2;

    turned_vector->x1 = x1;
    turned_vector->x2 = x2;
}

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

inline void bgc_fp32_turn2_vector_back(BGC_FP32_Vector2* turned_vector, const BGC_FP32_Turn2* turn, const BGC_FP32_Vector2* vector)
{
    const float x1 = turn->_sin * vector->x2 + turn->_cos * vector->x1;
    const float x2 = turn->_cos * vector->x2 - turn->_sin * vector->x1;

    turned_vector->x1 = x1;
    turned_vector->x2 = x2;
}

inline void bgc_fp64_turn2_vector_back(BGC_FP64_Vector2* turned_vector, const BGC_FP64_Turn2* turn, const BGC_FP64_Vector2* vector)
{
    const double x1 = turn->_sin * vector->x2 + turn->_cos * vector->x1;
    const double x2 = turn->_cos * vector->x2 - turn->_sin * vector->x1;

    turned_vector->x1 = x1;
    turned_vector->x2 = x2;
}

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

inline int bgc_fp32_turn2_are_close(const BGC_FP32_Turn2* turn1, const BGC_FP32_Turn2* turn2)
{
    const float d_cos = turn1->_cos - turn2->_cos;
    const float d_sin = turn1->_sin - turn2->_sin;

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

inline int bgc_fp64_turn2_are_close(const BGC_FP64_Turn2* turn1, const BGC_FP64_Turn2* turn2)
{
    const double d_cos = turn1->_cos - turn2->_cos;
    const double d_sin = turn1->_sin - turn2->_sin;

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

#endif