#include "../Tests.hpp"
#include "core/math/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);
    CORE_TEST_FLOAT(0.0f, v1[0], 0.0f);
    CORE_TEST_FLOAT(0.0f, v1[0], 0.0f);
    CORE_TEST_FLOAT(0.0f, v1[0], 0.0f);
    CORE_TEST_FLOAT(1.0f, v2[0], 0.0f);
    CORE_TEST_FLOAT(2.0f, v2[1], 0.0f);
    CORE_TEST_FLOAT(3.0f, v3[0], 0.0f);
    CORE_TEST_FLOAT(4.0f, v3[1], 0.0f);
    CORE_TEST_FLOAT(5.0f, v3[2], 0.0f);
    CORE_TEST_FLOAT(6.0f, v4[0], 0.0f);
    CORE_TEST_FLOAT(7.0f, v4[1], 0.0f);
    CORE_TEST_FLOAT(8.0f, v4[2], 0.0f);
    CORE_TEST_FLOAT(9.0f, v4[3], 0.0f);
}

static void testSetAngles() {
    float root = Core::Math::squareRoot(2.0f) * 0.5f;
    CORE_TEST_VECTOR(V3(1.0f, 0.0f, 0.0f), V3().setAngles(0.0f, 0.0f));
    CORE_TEST_VECTOR(V3(root, 0.0f, -root), V3().setAngles(45.0f, 0.0f));
    CORE_TEST_VECTOR(V3(0.0f, 0.0f, -1.0f), V3().setAngles(90.0f, 0.0f));
    CORE_TEST_VECTOR(V3(-root, 0.0f, -root), V3().setAngles(135.0f, 0.0f));
    CORE_TEST_VECTOR(V3(-1.0f, 0.0f, 0.0f), V3().setAngles(180.0f, 0.0f));
    CORE_TEST_VECTOR(V3(-root, 0.0f, root), V3().setAngles(225.0f, 0.0f));
    CORE_TEST_VECTOR(V3(0.0f, 0.0f, 1.0f), V3().setAngles(270.0f, 0.0f));
    CORE_TEST_VECTOR(V3(root, 0.0f, root), V3().setAngles(315.0f, 0.0f));

    CORE_TEST_VECTOR(V3(0.0f, 1.0f, 0.0f), V3().setAngles(0.0f, 90.0f));
    CORE_TEST_VECTOR(V3(0.0f, 1.0f, 0.0f), V3().setAngles(90.0f, 90.0f));
    CORE_TEST_VECTOR(V3(0.0f, 1.0f, 0.0f), V3().setAngles(180.0f, 90.0f));
    CORE_TEST_VECTOR(V3(0.0f, 1.0f, 0.0f), V3().setAngles(270.0f, 90.0f));

    CORE_TEST_VECTOR(V3(0.0f, -1.0f, 0.0f), V3().setAngles(0.0f, -90.0f));
    CORE_TEST_VECTOR(V3(0.0f, -1.0f, 0.0f), V3().setAngles(90.0f, -90.0f));
    CORE_TEST_VECTOR(V3(0.0f, -1.0f, 0.0f), V3().setAngles(180.0f, -90.0f));
    CORE_TEST_VECTOR(V3(0.0f, -1.0f, 0.0f), V3().setAngles(270.0f, -90.0f));

    CORE_TEST_VECTOR(V3(root, root, 0.0f), V3().setAngles(0.0f, 45.0f));
    CORE_TEST_VECTOR(V3(0.0f, root, -root), V3().setAngles(90.0f, 45.0f));
    CORE_TEST_VECTOR(V3(-root, root, 0.0f), V3().setAngles(180.0f, 45.0f));
    CORE_TEST_VECTOR(V3(0.0f, root, root), V3().setAngles(270.0f, 45.0f));

    CORE_TEST_VECTOR(V3(root, -root, 0.0f), V3().setAngles(0.0f, -45.0f));
    CORE_TEST_VECTOR(V3(0.0f, -root, -root), V3().setAngles(90.0f, -45.0f));
    CORE_TEST_VECTOR(V3(-root, -root, 0.0f), V3().setAngles(180.0f, -45.0f));
    CORE_TEST_VECTOR(V3(0.0f, -root, root), V3().setAngles(270.0f, -45.0f));

    CORE_TEST_VECTOR(V3(0.5f, root, -0.5f), V3().setAngles(45.0f, 45.0f));
}

static void testCross() {
    CORE_TEST_VECTOR(V3(0.0f, 0.0f, 1.0f),
                     V3(1.0f, 0.0f, 0.0f).cross(V3(0.0f, 1.0f, 0.0f)));
    CORE_TEST_VECTOR(V3(0.0f, -1.0f, 0.0f),
                     V3(1.0f, 0.0f, 0.0f).cross(V3(0.0f, 0.0f, 1.0f)));
    CORE_TEST_VECTOR(V3(0.0f, 0.0f, -1.0f),
                     V3(0.0f, 1.0f, 0.0f).cross(V3(1.0f, 0.0f, 0.0f)));
    CORE_TEST_VECTOR(V3(1.0f, 0.0f, 0.0f),
                     V3(0.0f, 1.0f, 0.0f).cross(V3(0.0f, 0.0f, 1.0f)));
    CORE_TEST_VECTOR(V3(0.0f, 1.0f, 0.0f),
                     V3(0.0f, 0.0f, 1.0f).cross(V3(1.0f, 0.0f, 0.0f)));
    CORE_TEST_VECTOR(V3(-1.0f, 0.0f, 0.0f),
                     V3(0.0f, 0.0f, 1.0f).cross(V3(0.0f, 1.0f, 0.0f)));
}

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

static void testAdd() {
    CORE_TEST_VECTOR(V3(1.0f, 2.0f, 3.0f), V3() + V3(1.0f, 2.0f, 3.0f));
    CORE_TEST_VECTOR(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);
    CORE_TEST_VECTOR(V3(-1.0f, -2.0f, -3.0f), v);
    v -= V3(2.0f, 3.0f, 4.0f);
    CORE_TEST_VECTOR(V3(-3.0f, -5.0f, -7.0f), v);
}

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

static void testInvert() {
    CORE_TEST_VECTOR(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;
    CORE_TEST_VECTOR(V3(3.0f, 6.0f, 9.0f), v);
    v *= -2.0f;
    CORE_TEST_VECTOR(V3(-6.0f, -12.0f, -18.0f), v);
}

static void testMul() {
    CORE_TEST_VECTOR(V3(-3.0f, -6.0f, -9.0f), 3.0f * V3(-1.0f, -2.0f, -3.0f));
    CORE_TEST_VECTOR(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);
    CORE_TEST_VECTOR(V3(2.0f, 2.0f, 9.0f), v);
    v *= V3(-3.0f, 4.0f, -2.0f);
    CORE_TEST_VECTOR(V3(-6.0f, 8.0f, -18.0f), v);
}

static void testMulVector() {
    CORE_TEST_VECTOR(V3(-2.0f, -2.0f, -9.0f),
                     V3(2.0f, 1.0f, 3.0f) * V3(-1.0f, -2.0f, -3.0f));
    CORE_TEST_VECTOR(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;
    CORE_TEST_VECTOR(V3(4.0f, 8.0f, 3.0f), v);
    v /= -2.0f;
    CORE_TEST_VECTOR(V3(-2.0f, -4.0f, -1.5f), v);
}

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

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

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

static void testLength() {
    CORE_TEST_FLOAT(3.0f, V3(-2.0f, 2.0f, -1.0f).length(), eps);
    CORE_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();
    CORE_TEST_VECTOR(v2, v1);

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

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

static void testToString() {
    Core::String32<200> s;
    CORE_TEST_ERROR(s.append(Core::Vector<1, float>()));
    CORE_TEST_ERROR(s.append(Core::Vector2(2.0f, 3.0f)));
    CORE_TEST_ERROR(s.append(V3(4.0f, 5.0f, 6.0f)));
    Core::String32<200> s2;
    CORE_TEST_ERROR(s2.append("[0.00][2.00, 3.00][4.00, 5.00, 6.00]"));
    CORE_TEST_EQUAL(s2, s);
}

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

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