#include <math.h>

#include "../Tests.h"
#include "core/Vector.h"

static const float eps = 0.0001f;

#define V2 CoreVector2
#define CV2(a, b, c, d) (&(V2){{a, b}})
#define FV2(a, b) CV2(a, b, 0, 0)
#define CV20 CV2(0, 0, 0, 0)

#define V3 CoreVector3
#define CV3(a, b, c, d) (&(V3){{a, b, c}})
#define FV3(a, b, c) CV3(a, b, c, 0)
#define CV30 CV3(0, 0, 0, 0)

#define V4 CoreVector4
#define CV4(a, b, c, d) (&(V4){{a, b, c, d}})
#define CV40 CV4(0, 0, 0, 0)

#define IV2 CoreIntVector2
#define CIV2(a, b, c, d) (&(IV2){{a, b}})
#define FIV2(a, b) CIV2(a, b, 0, 0)
#define CIV20 CIV2(0, 0, 0, 0)

#define IV3 CoreIntVector3
#define CIV3(a, b, c, d) (&(IV3){{a, b, c}})
#define FIV3(a, b, c) CIV3(a, b, c, 0)
#define CIV30 CIV3(0, 0, 0, 0)

#define IV4 CoreIntVector4
#define CIV4(a, b, c, d) (&(IV4){{a, b, c, d}})
#define CIV40 CIV4(0, 0, 0, 0)

#define TESTS X(V2) X(V3) X(V4) X(IV2) X(IV3) X(IV4)

static void testSetAngles() {
    float root = sqrtf(2) * 0.5f;
    CORE_TEST_V3(FV3(1, 0, 0), coreAngles(CV30, 0, 0));
    CORE_TEST_V3(FV3(root, 0, -root), coreAngles(CV30, 45, 0));
    CORE_TEST_V3(FV3(0, 0, -1), coreAngles(CV30, 90, 0));
    CORE_TEST_V3(FV3(-root, 0, -root), coreAngles(CV30, 135, 0));
    CORE_TEST_V3(FV3(-1, 0, 0), coreAngles(CV30, 180, 0));
    CORE_TEST_V3(FV3(-root, 0, root), coreAngles(CV30, 225, 0));
    CORE_TEST_V3(FV3(0, 0, 1), coreAngles(CV30, 270, 0));
    CORE_TEST_V3(FV3(root, 0, root), coreAngles(CV30, 315, 0));

    CORE_TEST_V3(FV3(0, 1, 0), coreAngles(CV30, 0, 90));
    CORE_TEST_V3(FV3(0, 1, 0), coreAngles(CV30, 90, 90));
    CORE_TEST_V3(FV3(0, 1, 0), coreAngles(CV30, 180, 90));
    CORE_TEST_V3(FV3(0, 1, 0), coreAngles(CV30, 270, 90));

    CORE_TEST_V3(FV3(0, -1, 0), coreAngles(CV30, 0, -90));
    CORE_TEST_V3(FV3(0, -1, 0), coreAngles(CV30, 90, -90));
    CORE_TEST_V3(FV3(0, -1, 0), coreAngles(CV30, 180, -90));
    CORE_TEST_V3(FV3(0, -1, 0), coreAngles(CV30, 270, -90));

    CORE_TEST_V3(FV3(root, root, 0), coreAngles(CV30, 0, 45));
    CORE_TEST_V3(FV3(0, root, -root), coreAngles(CV30, 90, 45));
    CORE_TEST_V3(FV3(-root, root, 0), coreAngles(CV30, 180, 45));
    CORE_TEST_V3(FV3(0, root, root), coreAngles(CV30, 270, 45));

    CORE_TEST_V3(FV3(root, -root, 0), coreAngles(CV30, 0, -45));
    CORE_TEST_V3(FV3(0, -root, -root), coreAngles(CV30, 90, -45));
    CORE_TEST_V3(FV3(-root, -root, 0), coreAngles(CV30, 180, -45));
    CORE_TEST_V3(FV3(0, -root, root), coreAngles(CV30, 270, -45));

    CORE_TEST_V3(FV3(0.5f, root, -0.5f), coreAngles(CV30, 45, 45));
}

static void testCross() {
    CORE_TEST_V3(FV3(0, 0, 1), coreCross(CV30, FV3(1, 0, 0), FV3(0, 1, 0)));
    CORE_TEST_V3(FV3(0, -1, 0), coreCross(CV30, FV3(1, 0, 0), FV3(0, 0, 1)));
    CORE_TEST_V3(FV3(0, 0, -1), coreCross(CV30, FV3(0, 1, 0), FV3(1, 0, 0)));
    CORE_TEST_V3(FV3(1, 0, 0), coreCross(CV30, FV3(0, 1, 0), FV3(0, 0, 1)));
    CORE_TEST_V3(FV3(0, 1, 0), coreCross(CV30, FV3(0, 0, 1), FV3(1, 0, 0)));
    CORE_TEST_V3(FV3(-1, 0, 0), coreCross(CV30, FV3(0, 0, 1), FV3(0, 1, 0)));
}

static void testSetAdd() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(0, 0, 0, 0);                                               \
        coreAddSet##T(&v, C##T(1, 2, 3, 4));                                   \
        CORE_TEST_##T(C##T(1, 2, 3, 4), &v);                                   \
        coreAddSet##T(&v, C##T(2, 3, 4, 5));                                   \
        CORE_TEST_##T(C##T(3, 5, 7, 9), &v);                                   \
    }
    TESTS
#undef X
}

static void testAdd() {
    CORE_TEST_V3(FV3(1, 2, 3), coreAddV3(CV30, CV30, FV3(1, 2, 3)));
    CORE_TEST_V3(FV3(3, 5, 7), coreAddV3(CV30, FV3(1, 2, 3), FV3(2, 3, 4)));
}

static void testSetSub() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(0, 0, 0, 0);                                               \
        coreSubSet##T(&v, C##T(1, 2, 3, 4));                                   \
        CORE_TEST_##T(C##T(-1, -2, -3, -4), &v);                               \
        coreSubSet##T(&v, C##T(2, 3, 4, 5));                                   \
        CORE_TEST_##T(C##T(-3, -5, -7, -9), &v);                               \
    }
    TESTS
#undef X
}

static void testSub() {
    CORE_TEST_V3(FV3(1, 2, 3), coreSubV3(CV30, CV30, FV3(-1, -2, -3)));
    CORE_TEST_V3(FV3(-1, -1, -1), coreSubV3(CV30, FV3(1, 2, 3), FV3(2, 3, 4)));
}

static void testSetMul() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(1, 2, 3, 4);                                               \
        coreMulSet##T##F(&v, 3);                                               \
        CORE_TEST_##T(C##T(3, 6, 9, 12), &v);                                  \
        coreMulSet##T##F(&v, -2);                                              \
        CORE_TEST_##T(C##T(-6, -12, -18, -24), &v);                            \
    }
    TESTS
#undef X
}

static void testMul() {
    CORE_TEST_V3(FV3(3, 6, 9), coreMulV3F(CV30, FV3(1, 2, 3), 3));
}

static void testSetMulVector() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(1, 2, 3, 4);                                               \
        coreMulSet##T(&v, C##T(2, 1, 3, 4));                                   \
        CORE_TEST_##T(C##T(2, 2, 9, 16), &v);                                  \
        coreMulSet##T(&v, C##T(-3, 4, -2, -2));                                \
        CORE_TEST_##T(C##T(-6, 8, -18, -32), &v);                              \
    }
    TESTS
#undef X
}

static void testMulVector() {
    CORE_TEST_V3(FV3(-2, -2, -9),
                 coreMulV3(CV30, FV3(2, 1, 3), FV3(-1, -2, -3)));
    CORE_TEST_V3(FV3(2, 2, 9), coreMulV3(CV30, FV3(1, 2, 3), FV3(2, 1, 3)));
}

static void testSetDiv() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(18, 36, 9, 27);                                            \
        coreDivSet##T##F(&v, 3);                                               \
        CORE_TEST_##T(C##T(6, 12, 3, 9), &v);                                  \
        coreDivSet##T##F(&v, -3);                                              \
        CORE_TEST_##T(C##T(-2, -4, -1, -3), &v);                               \
    }
    TESTS
#undef X
}

static void testDiv() {
    CORE_TEST_V3(FV3(-1, -2, -3), coreDivV3F(CV30, FV3(-3, -6, -9), 3));
}

static void testSetDivVector() {
#define X(T)                                                                   \
    {                                                                          \
        T v = *C##T(12, 4, 6, 8);                                              \
        coreDivSet##T(&v, C##T(2, 1, 3, 4));                                   \
        CORE_TEST_##T(C##T(6, 4, 2, 2), &v);                                   \
        coreDivSet##T(&v, C##T(-3, 4, -2, -1));                                \
        CORE_TEST_##T(C##T(-2, 1, -1, -2), &v);                                \
    }
    TESTS
#undef X
}

static void testDivVector() {
    CORE_TEST_V3(FV3(-2, -0.5f, -1),
                 coreDivV3(CV30, FV3(2, 1, 3), FV3(-1, -2, -3)));
    CORE_TEST_V3(FV3(0.5f, 2, 1), coreDivV3(CV30, FV3(1, 2, 3), FV3(2, 1, 3)));
}

static void testSetInvert() {
    CORE_TEST_V2(FV2(-1, 2), coreInvertSetV2(FV2(1, -2)));
    CORE_TEST_V3(FV3(-1, 2, 3), coreInvertSetV3(FV3(1, -2, -3)));
    CORE_TEST_V4(CV4(-1, 2, 3, 4), coreInvertSetV4(CV4(1, -2, -3, -4)));
    CORE_TEST_IV2(FIV2(-1, 2), coreInvertSetIV2(FIV2(1, -2)));
    CORE_TEST_IV3(FIV3(-1, 2, 3), coreInvertSetIV3(FIV3(1, -2, -3)));
    CORE_TEST_IV4(CIV4(-1, 2, 3, 4), coreInvertSetIV4(CIV4(1, -2, -3, -4)));
}

static void testInvert() {
    CORE_TEST_V3(FV3(-1, 2, 3), coreInvertV3(CV30, FV3(1, -2, -3)));
}

static void testDot() {
    CORE_TEST_FLOAT(0, coreDotV2(FV2(-4, 2), FV2(-1, -2)), eps);
    CORE_TEST_FLOAT(9, coreDotV3(FV3(-4, 2, -3), FV3(-1, -2, -3)), eps);
    CORE_TEST_FLOAT(16, coreDotV4(CV4(-4, 2, -3, 1), CV4(-1, -2, -3, 7)), eps);
}

static void testSquareLength() {
    CORE_TEST_FLOAT(20, coreSquareLengthV2(FV2(-4, 2)), eps);
    CORE_TEST_FLOAT(29, coreSquareLengthV3(FV3(-4, 2, -3)), eps);
    CORE_TEST_FLOAT(54, coreSquareLengthV4(CV4(-4, 2, -3, 5)), eps);
}

static void testLength() {
    CORE_TEST_FLOAT(5, coreLengthV2(FV2(-3, 4)), eps);
    CORE_TEST_FLOAT(13, coreLengthV2(FV2(5, 12)), eps);
    CORE_TEST_FLOAT(3, coreLengthV3(FV3(-2, 2, -1)), eps);
    CORE_TEST_FLOAT(7, coreLengthV3(FV3(6, 2, -3)), eps);
    CORE_TEST_FLOAT(3, coreLengthV4(CV4(-2, 2, 0, -1)), eps);
    CORE_TEST_FLOAT(9, coreLengthV4(CV4(6, 0, -6, 3)), eps);
}

static void testNormalize() {
    {
        V2 v1 = {{-15, 20}};
        V2 v2;
        coreMulV2F(&v2, &v1, 1.0f / 25.0f);
        coreNormalizeV2(&v1);
        CORE_TEST_V2(&v2, &v1);

        V2 v3 = {{15, 36}};
        V2 v4;
        coreMulV2F(&v4, &v3, 1.0f / 39.0f);
        coreNormalizeV2(&v3);
        CORE_TEST_V2(&v4, &v3);
    }
    {
        V3 v1 = {{-2, 2, -1}};
        V3 v2;
        coreMulV3F(&v2, &v1, 1.0f / 3.0f);
        coreNormalizeV3(&v1);
        CORE_TEST_V3(&v2, &v1);

        V3 v3 = {{6, 2, -3}};
        V3 v4;
        coreMulV3F(&v4, &v3, 1.0f / 7.0f);
        coreNormalizeV3(&v3);
        CORE_TEST_V3(&v4, &v3);
    }
    {
        V4 v1 = {{-2, 2, 0, -1}};
        V4 v2;
        coreMulV4F(&v2, &v1, 1.0f / 3.0f);
        coreNormalizeV4(&v1);
        CORE_TEST_V4(&v2, &v1);

        V4 v3 = {{6, 0, -6, 3}};
        V4 v4;
        coreMulV4F(&v4, &v3, 1.0f / 9.0f);
        coreNormalizeV4(&v3);
        CORE_TEST_V4(&v4, &v3);
    }
}

static void testCast() {
    CORE_TEST_V2(FV2(-2.0f, 2.0f), coreConvertIV2(CV20, FIV2(-2, 2)));
    CORE_TEST_IV2(FIV2(-2, 2), coreConvertV2(CIV20, FV2(-2.5f, 2.6f)));
    CORE_TEST_V3(FV3(-2.0f, 2.0f, 9.0f), coreConvertIV3(CV30, FIV3(-2, 2, 9)));
    CORE_TEST_IV3(FIV3(-2, 2, 9), coreConvertV3(CIV30, FV3(-2.5f, 2.6f, 9.0f)));
    CORE_TEST_V4(CV4(-2.0f, 2.0f, 9.0f, 6.0f),
                 coreConvertIV4(CV40, CIV4(-2, 2, 9, 6)));
    CORE_TEST_IV4(CIV4(-2, 2, 9, 3),
                  coreConvertV4(CIV40, CV4(-2.5f, 2.6f, 9.0f, 3.2f)));
}

static void testToString() {
    char buffer[64];
    coreToStringV2(FV2(4, 5), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4.000, 5.000]", buffer);
    coreToStringV3(FV3(4, 5, 6), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4.000, 5.000, 6.000]", buffer);
    coreToStringV4(CV4(4, 5, 6, 7), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4.000, 5.000, 6.000, 7.000]", buffer);
    coreToStringIV2(FIV2(4, 5), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4, 5]", buffer);
    coreToStringIV3(FIV3(4, 5, 6), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4, 5, 6]", buffer);
    coreToStringIV4(CIV4(4, 5, 6, 7), buffer, sizeof(buffer));
    CORE_TEST_STRING("[4, 5, 6, 7]", buffer);
}

void coreTestVector() {
    testSetAngles();
    testCross();
    testSetAdd();
    testAdd();
    testSetSub();
    testSub();
    testSetMul();
    testMul();
    testSetMulVector();
    testMulVector();
    testSetDiv();
    testDiv();
    testSetDivVector();
    testDivVector();
    testSetInvert();
    testInvert();
    testDot();
    testSquareLength();
    testLength();
    testNormalize();
    testCast();
    testToString();
}