#include "core/Vector.h"

#include <math.h>

#include "core/ToString.h"

typedef Vector2 V2;
typedef Vector3 V3;
typedef Vector4 V4;
typedef IntVector2 IV2;
typedef IntVector3 IV3;
typedef IntVector4 IV4;

V3* angles(V3* r, float lengthAngle, float widthAngle) {
    float sWidth = sinf(widthAngle);
    float cWidth = cosf(widthAngle);
    float sLength = sinf(lengthAngle);
    float cLength = cosf(lengthAngle);

    r->x = cWidth * cLength;
    r->y = sWidth;
    r->z = -sLength * cWidth;
    return r;
}

V3* cross(V3* r, const V3* a, const V3* b) {
    r->x = a->y * b->z - a->z * b->y;
    r->y = a->z * b->x - a->x * b->z;
    r->z = a->x * b->y - a->y * b->x;
    return r;
}

#define ADD_OP(i) r->data[i] = a->data[i] + b->data[i]
#define SUB_OP(i) r->data[i] = a->data[i] - b->data[i]
#define MUL_OP(i) r->data[i] = a->data[i] * b->data[i]
#define MUL_F_OP(i) r->data[i] = a->data[i] * f
#define DIV_OP(i) r->data[i] = a->data[i] / b->data[i]
#define DIV_F_OP(i) r->data[i] = a->data[i] / f
#define INVERT_OP(i) r->data[i] = -a->data[i];
#define DOT_OP(i) length += a->data[i] * b->data[i];
#define FLOAT_CAST_OP(i) r->data[i] = (float)a->data[i];
#define INT_CAST_OP(i) r->data[i] = (int)a->data[i];
#define DO2(OP) \
    OP(0);      \
    OP(1)
#define DO3(OP) \
    DO2(OP);    \
    OP(2)
#define DO4(OP) \
    DO3(OP);    \
    OP(3)

#define VECTOR_IMPL(T, N, FT)                 \
    T* addSet##T(T* r, const T* a) {          \
        return add##T(r, r, a);               \
    }                                         \
                                              \
    T* add##T(T* r, const T* a, const T* b) { \
        DO##N(ADD_OP);                        \
        return r;                             \
    }                                         \
                                              \
    T* subSet##T(T* r, const T* a) {          \
        return sub##T(r, r, a);               \
    }                                         \
                                              \
    T* sub##T(T* r, const T* a, const T* b) { \
        DO##N(SUB_OP);                        \
        return r;                             \
    }                                         \
                                              \
    T* mulSet##T(T* r, const T* a) {          \
        return mul##T(r, r, a);               \
    }                                         \
                                              \
    T* mul##T(T* r, const T* a, const T* b) { \
        DO##N(MUL_OP);                        \
        return r;                             \
    }                                         \
                                              \
    T* divSet##T(T* r, const T* a) {          \
        return div##T(r, r, a);               \
    }                                         \
                                              \
    T* div##T(T* r, const T* a, const T* b) { \
        DO##N(DIV_OP);                        \
        return r;                             \
    }                                         \
                                              \
    T* mulSet##T##F(T* r, FT f) {             \
        return mul##T##F(r, r, f);            \
    }                                         \
                                              \
    T* mul##T##F(T* r, const T* a, FT f) {    \
        DO##N(MUL_F_OP);                      \
        return r;                             \
    }                                         \
                                              \
    T* divSet##T##F(T* r, FT f) {             \
        return div##T##F(r, r, f);            \
    }                                         \
                                              \
    T* div##T##F(T* r, const T* a, FT f) {    \
        DO##N(DIV_F_OP);                      \
        return r;                             \
    }                                         \
                                              \
    T* invertSet##T(T* r) {                   \
        return invert##T(r, r);               \
    }                                         \
                                              \
    T* invert##T(T* r, const T* a) {          \
        DO##N(INVERT_OP);                     \
        return r;                             \
    }

#define VECTOR_IMPL_FLOAT(T, N, CN)                  \
    float dot##T(const T* a, const T* b) {           \
        float length = 0;                            \
        DO##N(DOT_OP);                               \
        return length;                               \
    }                                                \
                                                     \
    float squareLength##T(const T* a) {              \
        return dot##T(a, a);                         \
    }                                                \
                                                     \
    float length##T(const T* a) {                    \
        return sqrtf(squareLength##T(a));            \
    }                                                \
                                                     \
    T* normalize##T(T* r) {                          \
        return mulSet##T##F(r, 1.0f / length##T(r)); \
    }                                                \
                                                     \
    T* convertI##T(T* r, const CN* a) {              \
        DO##N(FLOAT_CAST_OP);                        \
        return r;                                    \
    }                                                \
                                                     \
    CN* convert##T(CN* r, const T* a) {              \
        DO##N(INT_CAST_OP);                          \
        return r;                                    \
    }

VECTOR_IMPL(V2, 2, float)
VECTOR_IMPL(V3, 3, float)
VECTOR_IMPL(V4, 4, float)
VECTOR_IMPL(IV2, 2, int)
VECTOR_IMPL(IV3, 3, int)
VECTOR_IMPL(IV4, 4, int)
VECTOR_IMPL_FLOAT(V2, 2, IV2)
VECTOR_IMPL_FLOAT(V3, 3, IV3)
VECTOR_IMPL_FLOAT(V4, 4, IV4)

size_t toStringV2(const V2* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%.3f, %.3f]", (double)a->x, (double)a->y);
}

size_t toStringV3(const V3* a, char* buffer, size_t n) {
    return toString(
        buffer, n, "[%.3f, %.3f, %.3f]", (double)a->x, (double)a->y,
        (double)a->z);
}

size_t toStringV4(const V4* a, char* buffer, size_t n) {
    return toString(
        buffer, n, "[%.3f, %.3f, %.3f, %.3f]", (double)a->x, (double)a->y,
        (double)a->z, (double)a->w);
}

size_t toStringIV2(const IV2* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%d, %d]", a->x, a->y);
}

size_t toStringIV3(const IV3* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%d, %d, %d]", a->x, a->y, a->z);
}

size_t toStringIV4(const IV4* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%d, %d, %d, %d]", a->x, a->y, a->z, a->w);
}