#include <math.h>

#include "../Tests.h"
#include "core/Generic.h"
#include "core/Utility.h"

static const float eps = 0.0001f;

#define V2 Vector2
#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 Vector3
#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 Vector4
#define CV4(a, b, c, d) (&(V4){{a, b, c, d}})
#define CV40 CV4(0, 0, 0, 0)

#define IV2 IntVector2
#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 IntVector3
#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 IntVector4
#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)

#define R45 degreeToRadian(45.0f)
#define R90 degreeToRadian(90.0f)
#define R135 degreeToRadian(135.0f)
#define R180 degreeToRadian(180.0f)
#define R225 degreeToRadian(225.0f)
#define R270 degreeToRadian(270.0f)
#define R315 degreeToRadian(315.0f)

static void testSetAngles() {
    float root = sqrtf(2) * 0.5f;
    TEST_V3(FV3(1, 0, 0), angles(CV30, 0, 0));
    TEST_V3(FV3(root, 0, -root), angles(CV30, R45, 0));
    TEST_V3(FV3(0, 0, -1), angles(CV30, R90, 0));
    TEST_V3(FV3(-root, 0, -root), angles(CV30, R135, 0));
    TEST_V3(FV3(-1, 0, 0), angles(CV30, R180, 0));
    TEST_V3(FV3(-root, 0, root), angles(CV30, R225, 0));
    TEST_V3(FV3(0, 0, 1), angles(CV30, R270, 0));
    TEST_V3(FV3(root, 0, root), angles(CV30, R315, 0));

    TEST_V3(FV3(0, 1, 0), angles(CV30, 0, R90));
    TEST_V3(FV3(0, 1, 0), angles(CV30, R90, R90));
    TEST_V3(FV3(0, 1, 0), angles(CV30, R180, R90));
    TEST_V3(FV3(0, 1, 0), angles(CV30, R270, R90));

    TEST_V3(FV3(0, -1, 0), angles(CV30, 0, -R90));
    TEST_V3(FV3(0, -1, 0), angles(CV30, R90, -R90));
    TEST_V3(FV3(0, -1, 0), angles(CV30, R180, -R90));
    TEST_V3(FV3(0, -1, 0), angles(CV30, R270, -R90));

    TEST_V3(FV3(root, root, 0), angles(CV30, 0, R45));
    TEST_V3(FV3(0, root, -root), angles(CV30, R90, R45));
    TEST_V3(FV3(-root, root, 0), angles(CV30, R180, R45));
    TEST_V3(FV3(0, root, root), angles(CV30, R270, R45));

    TEST_V3(FV3(root, -root, 0), angles(CV30, 0, -R45));
    TEST_V3(FV3(0, -root, -root), angles(CV30, R90, -R45));
    TEST_V3(FV3(-root, -root, 0), angles(CV30, R180, -R45));
    TEST_V3(FV3(0, -root, root), angles(CV30, R270, -R45));

    TEST_V3(FV3(0.5f, root, -0.5f), angles(CV30, R45, R45));
}

static void testCross() {
    TEST_V3(FV3(0, 0, 1), cross(FV3(1, 0, 0), FV3(0, 1, 0)));
    TEST_V3(FV3(0, -1, 0), cross(FV3(1, 0, 0), FV3(0, 0, 1)));
    TEST_V3(FV3(0, 0, -1), cross(FV3(0, 1, 0), FV3(1, 0, 0)));
    TEST_V3(FV3(1, 0, 0), cross(FV3(0, 1, 0), FV3(0, 0, 1)));
    TEST_V3(FV3(0, 1, 0), cross(FV3(0, 0, 1), FV3(1, 0, 0)));
    TEST_V3(FV3(-1, 0, 0), cross(FV3(0, 0, 1), FV3(0, 1, 0)));
}

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

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

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

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

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

static void testMul() {
    TEST_V3(FV3(3, 6, 9), mul(FV3(1, 2, 3), 3));
}

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

static void testMulVector() {
    TEST_V3(FV3(-2, -2, -9), mul(FV3(2, 1, 3), FV3(-1, -2, -3)));
    TEST_V3(FV3(2, 2, 9), mul(FV3(1, 2, 3), FV3(2, 1, 3)));
}

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

static void testDiv() {
    TEST_V3(FV3(-1, -2, -3), div(FV3(-3, -6, -9), 3));
}

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

static void testDivVector() {
    TEST_V3(FV3(-2, -0.5f, -1), div(FV3(2, 1, 3), FV3(-1, -2, -3)));
    TEST_V3(FV3(0.5f, 2, 1), div(FV3(1, 2, 3), FV3(2, 1, 3)));
}

static void testSetInvert() {
    TEST_V2(FV2(-1, 2), invertSet(FV2(1, -2)));
    TEST_V3(FV3(-1, 2, 3), invertSet(FV3(1, -2, -3)));
    TEST_V4(CV4(-1, 2, 3, 4), invertSet(CV4(1, -2, -3, -4)));
    TEST_IV2(FIV2(-1, 2), invertSet(FIV2(1, -2)));
    TEST_IV3(FIV3(-1, 2, 3), invertSet(FIV3(1, -2, -3)));
    TEST_IV4(CIV4(-1, 2, 3, 4), invertSet(CIV4(1, -2, -3, -4)));
}

static void testInvert() {
    TEST_V3(FV3(-1, 2, 3), invert(FV3(1, -2, -3)));
}

static void testDot() {
    TEST_FLOAT(0, dot(FV2(-4, 2), FV2(-1, -2)), eps);
    TEST_FLOAT(9, dot(FV3(-4, 2, -3), FV3(-1, -2, -3)), eps);
    TEST_FLOAT(16, dot(CV4(-4, 2, -3, 1), CV4(-1, -2, -3, 7)), eps);
}

static void testSquareLength() {
    TEST_FLOAT(20, squareLength(FV2(-4, 2)), eps);
    TEST_FLOAT(29, squareLength(FV3(-4, 2, -3)), eps);
    TEST_FLOAT(54, squareLength(CV4(-4, 2, -3, 5)), eps);
}

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

static void testNormalize() {
    {
        V2 v1 = {{-15, 20}};
        V2 v2;
        mul(&v2, &v1, 1.0f / 25.0f);
        normalize(&v1);
        TEST_V2(&v2, &v1);

        V2 v3 = {{15, 36}};
        V2 v4;
        mul(&v4, &v3, 1.0f / 39.0f);
        normalize(&v3);
        TEST_V2(&v4, &v3);
    }
    {
        V3 v1 = {{-2, 2, -1}};
        V3 v2;
        mul(&v2, &v1, 1.0f / 3.0f);
        normalize(&v1);
        TEST_V3(&v2, &v1);

        V3 v3 = {{6, 2, -3}};
        V3 v4;
        mul(&v4, &v3, 1.0f / 7.0f);
        normalize(&v3);
        TEST_V3(&v4, &v3);
    }
    {
        V4 v1 = {{-2, 2, 0, -1}};
        V4 v2;
        mul(&v2, &v1, 1.0f / 3.0f);
        normalize(&v1);
        TEST_V4(&v2, &v1);

        V4 v3 = {{6, 0, -6, 3}};
        V4 v4;
        mul(&v4, &v3, 1.0f / 9.0f);
        normalize(&v3);
        TEST_V4(&v4, &v3);
    }
}

static void testCast() {
    TEST_V2(FV2(-2.0f, 2.0f), convert(CV20, FIV2(-2, 2)));
    TEST_IV2(FIV2(-2, 2), convert(CIV20, FV2(-2.5f, 2.6f)));
    TEST_V3(FV3(-2.0f, 2.0f, 9.0f), convert(CV30, FIV3(-2, 2, 9)));
    TEST_IV3(FIV3(-2, 2, 9), convert(CIV30, FV3(-2.5f, 2.6f, 9.0f)));
    TEST_V4(CV4(-2.0f, 2.0f, 9.0f, 6.0f), convert(CV40, CIV4(-2, 2, 9, 6)));
    TEST_IV4(CIV4(-2, 2, 9, 3), convert(CIV40, CV4(-2.5f, 2.6f, 9.0f, 3.2f)));
}

static void testToString() {
    char buffer[64];
    toStringV2(FV2(4, 5), buffer, sizeof(buffer));
    TEST_STRING("[4.000, 5.000]", buffer);
    toStringV3(FV3(4, 5, 6), buffer, sizeof(buffer));
    TEST_STRING("[4.000, 5.000, 6.000]", buffer);
    toStringV4(CV4(4, 5, 6, 7), buffer, sizeof(buffer));
    TEST_STRING("[4.000, 5.000, 6.000, 7.000]", buffer);
    toStringIV2(FIV2(4, 5), buffer, sizeof(buffer));
    TEST_STRING("[4, 5]", buffer);
    toStringIV3(FIV3(4, 5, 6), buffer, sizeof(buffer));
    TEST_STRING("[4, 5, 6]", buffer);
    toStringIV4(CIV4(4, 5, 6, 7), buffer, sizeof(buffer));
    TEST_STRING("[4, 5, 6, 7]", buffer);
}

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