#include "core/Vector.h"

#include <math.h>
#include <stdio.h>

#include "core/ToString.h"
#include "core/Utility.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) {
    lengthAngle = degreeToRadian(lengthAngle);
    widthAngle = degreeToRadian(widthAngle);

    float sWidth = sinf(widthAngle);
    float cWidth = cosf(widthAngle);
    float sLength = sinf(lengthAngle);
    float cLength = cosf(lengthAngle);

    r->data[0] = cWidth * cLength;
    r->data[1] = sWidth;
    r->data[2] = -sLength * cWidth;
    return r;
}

V3* cross(V3* r, const V3* a, const V3* b) {
    r->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1];
    r->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2];
    r->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0];
    return r;
}

#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) {                                  \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] + b->data[i];                              \
        }                                                                      \
        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) {                                  \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] - b->data[i];                              \
        }                                                                      \
        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) {                                  \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] * b->data[i];                              \
        }                                                                      \
        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) {                                  \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] / b->data[i];                              \
        }                                                                      \
        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) {                                     \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] * f;                                       \
        }                                                                      \
        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) {                                     \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = a->data[i] / f;                                       \
        }                                                                      \
        return r;                                                              \
    }                                                                          \
                                                                               \
    T* invertSet##T(T* r) {                                                    \
        return invert##T(r, r);                                                \
    }                                                                          \
                                                                               \
    T* invert##T(T* r, const T* a) {                                           \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = -a->data[i];                                          \
        }                                                                      \
        return r;                                                              \
    }

#define VECTOR_IMPL_FLOAT(T, N, CN)                                            \
    float dot##T(const T* a, const T* b) {                                     \
        float length = 0;                                                      \
        for(int i = 0; i < N; i++) {                                           \
            length += a->data[i] * b->data[i];                                 \
        }                                                                      \
        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) {                                        \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = (float)a->data[i];                                    \
        }                                                                      \
        return r;                                                              \
    }                                                                          \
                                                                               \
    CN* convert##T(CN* r, const T* a) {                                        \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = (int)a->data[i];                                      \
        }                                                                      \
        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->data[0],
                    (double)a->data[1]);
}

size_t toStringV3(const V3* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%.3f, %.3f, %.3f]", (double)a->data[0],
                    (double)a->data[1], (double)a->data[2]);
}

size_t toStringV4(const V4* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%.3f, %.3f, %.3f, %.3f]", (double)a->data[0],
                    (double)a->data[1], (double)a->data[2], (double)a->data[3]);
}

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

size_t toStringIV3(const IV3* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%d, %d, %d]", a->data[0], a->data[1],
                    a->data[2]);
}

size_t toStringIV4(const IV4* a, char* buffer, size_t n) {
    return toString(buffer, n, "[%d, %d, %d, %d]", a->data[0], a->data[1],
                    a->data[2], a->data[3]);
}