#include "../Tests.hpp"
#include "core/Test.hpp"
#include "core/Vector.hpp"

const float eps = 0.0001f;

using V3 = Core::Vector3;
using I3 = Core::IntVector3;

template class Core::Vector<4, float>;
template class Core::Vector<4, int>;
template class Core::Vector<3, float>;
template class Core::Vector<3, int>;
template class Core::Vector<2, float>;
template class Core::Vector<2, int>;

static void testInitAndRead() {
    V3 v1;
    Core::Vector2 v2(1.0f, 2.0f);
    V3 v3(3.0f, 4.0f, 5.0f);
    Core::Vector4 v4(6.0f, 7.0f, 8.0f, 9.0f);
    TEST_FLOAT(0.0f, v1[0], 0.0f);
    TEST_FLOAT(0.0f, v1[0], 0.0f);
    TEST_FLOAT(0.0f, v1[0], 0.0f);
    TEST_FLOAT(1.0f, v2[0], 0.0f);
    TEST_FLOAT(2.0f, v2[1], 0.0f);
    TEST_FLOAT(3.0f, v3[0], 0.0f);
    TEST_FLOAT(4.0f, v3[1], 0.0f);
    TEST_FLOAT(5.0f, v3[2], 0.0f);
    TEST_FLOAT(6.0f, v4[0], 0.0f);
    TEST_FLOAT(7.0f, v4[1], 0.0f);
    TEST_FLOAT(8.0f, v4[2], 0.0f);
    TEST_FLOAT(9.0f, v4[3], 0.0f);
}

static V3 angles(float lengthAngle, float widthAngle) {
    V3 v;
    Core::setAngles(v, lengthAngle, widthAngle);
    return v;
}

static void testSetAngles() {
    float root = sqrtf(2.0f) * 0.5f;
    TEST(V3(1.0f, 0.0f, 0.0f), angles(0.0f, 0.0f));
    TEST(V3(root, 0.0f, -root), angles(45.0f, 0.0f));
    TEST(V3(0.0f, 0.0f, -1.0f), angles(90.0f, 0.0f));
    TEST(V3(-root, 0.0f, -root), angles(135.0f, 0.0f));
    TEST(V3(-1.0f, 0.0f, 0.0f), angles(180.0f, 0.0f));
    TEST(V3(-root, 0.0f, root), angles(225.0f, 0.0f));
    TEST(V3(0.0f, 0.0f, 1.0f), angles(270.0f, 0.0f));
    TEST(V3(root, 0.0f, root), angles(315.0f, 0.0f));

    TEST(V3(0.0f, 1.0f, 0.0f), angles(0.0f, 90.0f));
    TEST(V3(0.0f, 1.0f, 0.0f), angles(90.0f, 90.0f));
    TEST(V3(0.0f, 1.0f, 0.0f), angles(180.0f, 90.0f));
    TEST(V3(0.0f, 1.0f, 0.0f), angles(270.0f, 90.0f));

    TEST(V3(0.0f, -1.0f, 0.0f), angles(0.0f, -90.0f));
    TEST(V3(0.0f, -1.0f, 0.0f), angles(90.0f, -90.0f));
    TEST(V3(0.0f, -1.0f, 0.0f), angles(180.0f, -90.0f));
    TEST(V3(0.0f, -1.0f, 0.0f), angles(270.0f, -90.0f));

    TEST(V3(root, root, 0.0f), angles(0.0f, 45.0f));
    TEST(V3(0.0f, root, -root), angles(90.0f, 45.0f));
    TEST(V3(-root, root, 0.0f), angles(180.0f, 45.0f));
    TEST(V3(0.0f, root, root), angles(270.0f, 45.0f));

    TEST(V3(root, -root, 0.0f), angles(0.0f, -45.0f));
    TEST(V3(0.0f, -root, -root), angles(90.0f, -45.0f));
    TEST(V3(-root, -root, 0.0f), angles(180.0f, -45.0f));
    TEST(V3(0.0f, -root, root), angles(270.0f, -45.0f));

    TEST(V3(0.5f, root, -0.5f), angles(45.0f, 45.0f));
}

static void testCross() {
    TEST(V3(0, 0, 1), Core::cross(V3(1, 0, 0), V3(0, 1, 0)));
    TEST(V3(0, -1, 0), Core::cross(V3(1, 0, 0), V3(0, 0, 1)));
    TEST(V3(0, 0, -1), Core::cross(V3(0, 1, 0), V3(1, 0, 0)));
    TEST(V3(1, 0, 0), Core::cross(V3(0, 1, 0), V3(0, 0, 1)));
    TEST(V3(0, 1, 0), Core::cross(V3(0, 0, 1), V3(1, 0, 0)));
    TEST(V3(-1, 0, 0), Core::cross(V3(0, 0, 1), V3(0, 1, 0)));
}

static void testSetAdd() {
    V3 v;
    v += V3(1.0f, 2.0f, 3.0f);
    TEST(V3(1.0f, 2.0f, 3.0f), v);
    v += V3(2.0f, 3.0f, 4.0f);
    TEST(V3(3.0f, 5.0f, 7.0f), v);
}

static void testAdd() {
    TEST(V3(1.0f, 2.0f, 3.0f), V3() + V3(1.0f, 2.0f, 3.0f));
    TEST(V3(3.0f, 5.0f, 7.0f), V3(1.0f, 2.0f, 3.0f) + V3(2.0f, 3.0f, 4.0f));
}

static void testSetSub() {
    V3 v;
    v -= V3(1.0f, 2.0f, 3.0f);
    TEST(V3(-1.0f, -2.0f, -3.0f), v);
    v -= V3(2.0f, 3.0f, 4.0f);
    TEST(V3(-3.0f, -5.0f, -7.0f), v);
}

static void testSub() {
    TEST(V3(1.0f, 2.0f, 3.0f), V3() - V3(-1.0f, -2.0f, -3.0f));
    TEST(V3(-1.0f, -1.0f, -1.0f), V3(1.0f, 2.0f, 3.0f) - V3(2.0f, 3.0f, 4.0f));
}

static void testInvert() {
    TEST(V3(-1.0f, 2.0f, 3.0f), -V3(1.0f, -2.0f, -3.0f));
}

static void testSetMul() {
    V3 v(1.0f, 2.0f, 3.0f);
    v *= 3.0f;
    TEST(V3(3.0f, 6.0f, 9.0f), v);
    v *= -2.0f;
    TEST(V3(-6.0f, -12.0f, -18.0f), v);
}

static void testMul() {
    TEST(V3(-3.0f, -6.0f, -9.0f), 3.0f * V3(-1.0f, -2.0f, -3.0f));
    TEST(V3(3.0f, 6.0f, 9.0f), V3(1.0f, 2.0f, 3.0f) * 3.0);
}

static void testSetMulVector() {
    V3 v(1.0f, 2.0f, 3.0f);
    v *= V3(2.0f, 1.0f, 3.0f);
    TEST(V3(2.0f, 2.0f, 9.0f), v);
    v *= V3(-3.0f, 4.0f, -2.0f);
    TEST(V3(-6.0f, 8.0f, -18.0f), v);
}

static void testMulVector() {
    TEST(
        V3(-2.0f, -2.0f, -9.0f),
        V3(2.0f, 1.0f, 3.0f) * V3(-1.0f, -2.0f, -3.0f));
    TEST(V3(2.0f, 2.0f, 9.0f), V3(1.0f, 2.0f, 3.0f) * V3(2.0f, 1.0f, 3.0f));
}

static void testSetDiv() {
    V3 v(12.0f, 24.0f, 9.0f);
    v /= 3.0f;
    TEST(V3(4.0f, 8.0f, 3.0f), v);
    v /= -2.0f;
    TEST(V3(-2.0f, -4.0f, -1.5f), v);
}

static void testDiv() {
    TEST(V3(-1.0f, -2.0f, -3.0f), V3(-3.0f, -6.0f, -9.0f) / 3.0f);
}

static void testSetDivVector() {
    TEST(
        V3(-6.0f, -4.0f, -2.0f),
        V3(-12.0f, -4.0f, -6.0f) / V3(2.0f, 1.0f, 3.0f));
    TEST(
        V3(2.0f, -1.0f, 1.0f),
        V3(-6.0f, -4.0f, -2.0f) / V3(-3.0f, 4.0f, -2.0f));
}

static void testDivVector() {
    TEST(V3(-2, -0.5f, -1), V3(2, 1, 3) / V3(-1, -2, -3));
    TEST(V3(0.5f, 2, 1), V3(1, 2, 3) / V3(2, 1, 3));
}

static void testDot() {
    TEST_FLOAT(9.0f, V3(-4.0f, 2.0f, -3.0f).dot(V3(-1.0f, -2.0f, -3.0f)), eps);
    TEST_FLOAT(-22.0f, V3(2.0f, 2.0f, -4.0f).dot(V3(1.0f, -2.0f, 5.0f)), eps);
}

static void testSquareLength() {
    TEST_FLOAT(29.0f, V3(-4.0f, 2.0f, -3.0f).squareLength(), eps);
    TEST_FLOAT(24.0f, V3(2.0f, 2.0f, -4.0f).squareLength(), eps);
}

static void testLength() {
    TEST_FLOAT(3.0f, V3(-2.0f, 2.0f, -1.0f).length(), eps);
    TEST_FLOAT(7.0f, V3(6.0f, 2.0f, -3.0f).length(), eps);
}

static void testNormalize() {
    V3 v1(-2.0f, 2.0f, -1.0f);
    V3 v2 = v1 * (1.0f / 3.0f);
    v1.normalize();
    TEST(v2, v1);

    V3 v3(6.0f, 2.0f, -3.0f);
    V3 v4 = v3 * (1.0f / 7.0f);
    v3.normalize();
    TEST(v4, v3);
}

static void testCast() {
    TEST(V3(-2.5f, 2.6f, 9.0f).toInt(), I3(-2, 2, 9));
    TEST(I3(-2.5f, 2.6f, 9.0f).toFloat(), V3(-2.0f, 2.0f, 9.0f));
}

static void testToString() {
    char buffer[200];
    formatBuffer(
        buffer, sizeof(buffer), "# # #", Core::Vector<1, float>(),
        Core::Vector2(2.0f, 3.0f), V3(4.0f, 5.0f, 6.0f));
    TEST_STRING("[0.00] [2.00, 3.00] [4.00, 5.00, 6.00]", buffer);
}

static void testNormalizeIntVector() {
    I3 i(1, 2, 3);
    i.normalize();
    TEST(I3(0, 0, 1), i);
}

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