#include <cmath>

#include "math/Vector.h"
#include "tests/Test.h"
#include "tests/VectorTests.h"
#include "utils/StringBuffer.h"

const float eps = 0.0001f;

template<int N>
static void compareVectors(Test& test, const Vector<N>& wanted,
                           const Vector<N>& actual, const char* text) {
    for(int i = 0; i < N; i++) {
        test.checkFloat(
            wanted[i], actual[i], eps,
            StringBuffer<50>(text).append(" (").append(i).append(")"));
    }
}

static void testInitAndRead(Test& test) {
    Vector3 v1;
    Vector2 v2(1.0f, 2.0f);
    Vector3 v3(3.0f, 4.0f, 5.0f);
    Vector4 v4(6.0f, 7.0f, 8.0f, 9.0f);
    test.checkEqual(0.0f, v1[0], "init 1");
    test.checkEqual(0.0f, v1[0], "init 2");
    test.checkEqual(0.0f, v1[0], "init 3");
    test.checkEqual(1.0f, v2[0], "init 4");
    test.checkEqual(2.0f, v2[1], "init 5");
    test.checkEqual(3.0f, v3[0], "init 6");
    test.checkEqual(4.0f, v3[1], "init 7");
    test.checkEqual(5.0f, v3[2], "init 8");
    test.checkEqual(6.0f, v4[0], "init 9");
    test.checkEqual(7.0f, v4[1], "init 10");
    test.checkEqual(8.0f, v4[2], "init 11");
    test.checkEqual(9.0f, v4[3], "init 12");
}

static void testSetAngles(Test& test) {
    float root = sqrtf(2.0f) * 0.5f;
    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f),
                   Vector3().setAngles(0.0f, 0.0f), "angle 0 | 0");
    compareVectors(test, Vector3(root, 0.0f, -root),
                   Vector3().setAngles(45.0f, 0.0f), "angle 45 | 0");
    compareVectors(test, Vector3(0.0f, 0.0f, -1.0f),
                   Vector3().setAngles(90.0f, 0.0f), "angle 90 | 0");
    compareVectors(test, Vector3(-root, 0.0f, -root),
                   Vector3().setAngles(135.0f, 0.0f), "angle 135 | 0");
    compareVectors(test, Vector3(-1.0f, 0.0f, 0.0f),
                   Vector3().setAngles(180.0f, 0.0f), "angle 180 | 0");
    compareVectors(test, Vector3(-root, 0.0f, root),
                   Vector3().setAngles(225.0f, 0.0f), "angle 225 | 0");
    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f),
                   Vector3().setAngles(270.0f, 0.0f), "angle 270 | 0");
    compareVectors(test, Vector3(root, 0.0f, root),
                   Vector3().setAngles(315.0f, 0.0f), "angle 315 | 0");

    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f),
                   Vector3().setAngles(0.0f, 90.0f), "angle 0 | 90");
    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f),
                   Vector3().setAngles(90.0f, 90.0f), "angle 90 | 90");
    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f),
                   Vector3().setAngles(180.0f, 90.0f), "angle 180 | 90");
    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f),
                   Vector3().setAngles(270.0f, 90.0f), "angle 270 | 90");

    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f),
                   Vector3().setAngles(0.0f, -90.0f), "angle 0 | -90");
    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f),
                   Vector3().setAngles(90.0f, -90.0f), "angle 90 | -90");
    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f),
                   Vector3().setAngles(180.0f, -90.0f), "angle 180 | -90");
    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f),
                   Vector3().setAngles(270.0f, -90.0f), "angle 270 | -90");

    compareVectors(test, Vector3(root, root, 0.0f),
                   Vector3().setAngles(0.0f, 45.0f), "angle 0 | 45");
    compareVectors(test, Vector3(0.0f, root, -root),
                   Vector3().setAngles(90.0f, 45.0f), "angle 90 | 45");
    compareVectors(test, Vector3(-root, root, 0.0f),
                   Vector3().setAngles(180.0f, 45.0f), "angle 180 | 45");
    compareVectors(test, Vector3(0.0f, root, root),
                   Vector3().setAngles(270.0f, 45.0f), "angle 270 | 45");

    compareVectors(test, Vector3(root, -root, 0.0f),
                   Vector3().setAngles(0.0f, -45.0f), "angle 0 | -45");
    compareVectors(test, Vector3(0.0f, -root, -root),
                   Vector3().setAngles(90.0f, -45.0f), "angle 90 | -45");
    compareVectors(test, Vector3(-root, -root, 0.0f),
                   Vector3().setAngles(180.0f, -45.0f), "angle 180 | -45");
    compareVectors(test, Vector3(0.0f, -root, root),
                   Vector3().setAngles(270.0f, -45.0f), "angle 270 | -45");

    compareVectors(test, Vector3(0.5f, root, -0.5f),
                   Vector3().setAngles(45.0f, 45.0f), "angle 45 | 45");
}

static void testCross(Test& test) {
    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f),
                   Vector3(1.0f, 0.0f, 0.0f).cross(Vector3(0.0f, 1.0f, 0.0f)),
                   "cross 1");
    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f),
                   Vector3(1.0f, 0.0f, 0.0f).cross(Vector3(0.0f, 0.0f, 1.0f)),
                   "cross 2");
    compareVectors(test, Vector3(0.0f, 0.0f, -1.0f),
                   Vector3(0.0f, 1.0f, 0.0f).cross(Vector3(1.0f, 0.0f, 0.0f)),
                   "cross 3");
    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f),
                   Vector3(0.0f, 1.0f, 0.0f).cross(Vector3(0.0f, 0.0f, 1.0f)),
                   "cross 4");
    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f),
                   Vector3(0.0f, 0.0f, 1.0f).cross(Vector3(1.0f, 0.0f, 0.0f)),
                   "cross 5");
    compareVectors(test, Vector3(-1.0f, 0.0f, 0.0f),
                   Vector3(0.0f, 0.0f, 1.0f).cross(Vector3(0.0f, 1.0f, 0.0f)),
                   "cross 6");
}

static void testSetAdd(Test& test) {
    Vector3 v;
    v += Vector3(1.0f, 2.0f, 3.0f);
    compareVectors(test, Vector3(1.0f, 2.0f, 3.0f), v, "set add 1");
    v += Vector3(2.0f, 3.0f, 4.0f);
    compareVectors(test, Vector3(3.0f, 5.0f, 7.0f), v, "set add 2");
}

static void testAdd(Test& test) {
    compareVectors(test, Vector3(1.0f, 2.0f, 3.0f),
                   Vector3() + Vector3(1.0f, 2.0f, 3.0f), "add 1");
    compareVectors(test, Vector3(3.0f, 5.0f, 7.0f),
                   Vector3(1.0f, 2.0f, 3.0f) + Vector3(2.0f, 3.0f, 4.0f),
                   "add 2");
}

static void testSetSub(Test& test) {
    Vector3 v;
    v -= Vector3(1.0f, 2.0f, 3.0f);
    compareVectors(test, Vector3(-1.0f, -2.0f, -3.0f), v, "set sub 1");
    v -= Vector3(2.0f, 3.0f, 4.0f);
    compareVectors(test, Vector3(-3.0f, -5.0f, -7.0f), v, "set sub 2");
}

static void testSub(Test& test) {
    compareVectors(test, Vector3(1.0f, 2.0f, 3.0f),
                   Vector3() - Vector3(-1.0f, -2.0f, -3.0f), "sub 1");
    compareVectors(test, Vector3(-1.0f, -1.0f, -1.0f),
                   Vector3(1.0f, 2.0f, 3.0f) - Vector3(2.0f, 3.0f, 4.0f),
                   "sub 2");
}

static void testInvert(Test& test) {
    compareVectors(test, Vector3(-1.0f, 2.0f, 3.0f),
                   -Vector3(1.0f, -2.0f, -3.0f), "invert");
}

static void testSetMul(Test& test) {
    Vector3 v(1.0f, 2.0f, 3.0f);
    v *= 3.0f;
    compareVectors(test, Vector3(3.0f, 6.0f, 9.0f), v, "set mul 1");
    v *= -2.0f;
    compareVectors(test, Vector3(-6.0f, -12.0f, -18.0f), v, "set mul 2");
}

static void testMul(Test& test) {
    compareVectors(test, Vector3(-3.0f, -6.0f, -9.0f),
                   3.0f * Vector3(-1.0f, -2.0f, -3.0f), "mul 1");
    compareVectors(test, Vector3(3.0f, 6.0f, 9.0f),
                   Vector3(1.0f, 2.0f, 3.0f) * 3.0, "mul 2");
}

static void testSetDiv(Test& test) {
    Vector3 v(12.0f, 24.0f, 9.0f);
    v /= 3.0f;
    compareVectors(test, Vector3(4.0f, 8.0f, 3.0f), v, "set div 1");
    v /= -2.0f;
    compareVectors(test, Vector3(-2.0f, -4.0f, -1.5f), v, "set div 2");
}

static void testDiv(Test& test) {
    compareVectors(test, Vector3(-1.0f, -2.0f, -3.0f),
                   Vector3(-3.0f, -6.0f, -9.0f) / 3.0f, "div");
}

static void testDot(Test& test) {
    test.checkFloat(
        9.0f, Vector3(-4.0f, 2.0f, -3.0f).dot(Vector3(-1.0f, -2.0f, -3.0f)),
        eps, "dot 1");
    test.checkFloat(-22.0f,
                    Vector3(2.0f, 2.0f, -4.0f).dot(Vector3(1.0f, -2.0f, 5.0f)),
                    eps, "dot 2");
}

static void testSquareLength(Test& test) {
    test.checkFloat(29.0f, Vector3(-4.0f, 2.0f, -3.0f).squareLength(), eps,
                    "square length 1");
    test.checkFloat(24.0f, Vector3(2.0f, 2.0f, -4.0f).squareLength(), eps,
                    "square length 2");
}

static void testLength(Test& test) {
    test.checkFloat(3.0f, Vector3(-2.0f, 2.0f, -1.0f).length(), eps,
                    "length 1");
    test.checkFloat(7.0f, Vector3(6.0f, 2.0f, -3.0f).length(), eps, "length 2");
}

static void testNormalize(Test& test) {
    Vector3 v1(-2.0f, 2.0f, -1.0f);
    Vector3 v2 = v1 * (1.0f / 3.0f);
    v1.normalize();
    compareVectors(test, v2, v1, "normalize 1");

    Vector3 v3(6.0f, 2.0f, -3.0f);
    Vector3 v4 = v3 * (1.0f / 7.0f);
    v3.normalize();
    compareVectors(test, v4, v3, "normalize 2");
}

static void testToString(Test& test) {
    StringBuffer<50> s;
    s.append(Vector<1>())
        .append(Vector<2>(2.0f, 3.0f))
        .append(Vector<3>(4.0f, 5.0f, 6.0f));
    test.checkEqual(StringBuffer<50>("[0.00][2.00, 3.00][4.00, 5.00, 6.00]"), s,
                    "to string");
}

void VectorTests::test() {
    Test test("Vector");
    testInitAndRead(test);
    testSetAngles(test);
    testCross(test);
    testSetAdd(test);
    testAdd(test);
    testSetSub(test);
    testSub(test);
    testInvert(test);
    testSetMul(test);
    testMul(test);
    testSetDiv(test);
    testDiv(test);
    testDot(test);
    testSquareLength(test);
    testLength(test);
    testNormalize(test);
    testToString(test);
    test.finalize();
}