#include "core/Vector.h"

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

#include "core/Utility.h"

typedef CoreVector2 V2;
typedef CoreVector3 V3;
typedef CoreVector4 V4;
typedef CoreIntVector2 IV2;
typedef CoreIntVector3 IV3;
typedef CoreIntVector4 IV4;

V3* coreAngles(V3* r, float lengthAngle, float widthAngle) {
    lengthAngle = coreDegreeToRadian(lengthAngle);
    widthAngle = coreDegreeToRadian(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* coreCross(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* coreAddSet##T(T* r, const T* a) {                                       \
        return coreAdd##T(r, r, a);                                            \
    }                                                                          \
                                                                               \
    T* coreAdd##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* coreSubSet##T(T* r, const T* a) {                                       \
        return coreSub##T(r, r, a);                                            \
    }                                                                          \
                                                                               \
    T* coreSub##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* coreMulSet##T(T* r, const T* a) {                                       \
        return coreMul##T(r, r, a);                                            \
    }                                                                          \
                                                                               \
    T* coreMul##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* coreDivSet##T(T* r, const T* a) {                                       \
        return coreDiv##T(r, r, a);                                            \
    }                                                                          \
                                                                               \
    T* coreDiv##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* coreMulSet##T##F(T* r, FT f) {                                          \
        return coreMul##T##F(r, r, f);                                         \
    }                                                                          \
                                                                               \
    T* coreMul##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* coreDivSet##T##F(T* r, FT f) {                                          \
        return coreDiv##T##F(r, r, f);                                         \
    }                                                                          \
                                                                               \
    T* coreDiv##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* coreInvertSet##T(T* r) {                                                \
        return coreInvert##T(r, r);                                            \
    }                                                                          \
                                                                               \
    T* coreInvert##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 coreDot##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 coreSquareLength##T(const T* a) {                                    \
        return coreDot##T(a, a);                                               \
    }                                                                          \
                                                                               \
    float coreLength##T(const T* a) {                                          \
        return sqrtf(coreSquareLength##T(a));                                  \
    }                                                                          \
                                                                               \
    T* coreNormalize##T(T* r) {                                                \
        return coreMulSet##T##F(r, 1.0f / coreLength##T(r));                   \
    }                                                                          \
                                                                               \
    T* coreConvertI##T(T* r, const CN* a) {                                    \
        for(int i = 0; i < N; i++) {                                           \
            r->data[i] = (float)a->data[i];                                    \
        }                                                                      \
        return r;                                                              \
    }                                                                          \
                                                                               \
    CN* coreConvert##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)

static size_t safeCast(int s) {
    return s < 0 ? 0 : (size_t)s;
}

size_t coreToStringV2(const V2* a, char* buffer, size_t n) {
    return safeCast(snprintf(buffer, n, "[%.3f, %.3f]", (double)a->data[0],
                             (double)a->data[1]));
}

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

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

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

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

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