#include "tests/QuaternionTests.h"
#include "math/Quaternion.h"
#include "tests/Test.h"
#include "utils/StringBuffer.h"

typedef StringBuffer<100> String;

const float eps = 0.0001f;

template<int N, typename T>
static void compareVectors(Test& test, const Vector<N, T>& wanted,
                           const Vector<N, T>& 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 testInit(Test& test) {
    Quaternion q;
    test.checkEqual(String("(0.00 i + 0.00 j + 0.00 k + 1.00)"), String(q),
                    "init");
}

static void testAxisAndDegreesInit(Test& test) {
    Quaternion q(Vector3(1.0f, 2.0f, 3.0f), 142.0f);
    test.checkEqual(String("(0.25 i + 0.51 j + 0.76 k + 0.33)"), String(q),
                    "init with axis and degrees");
}

static void testLerp(Test& test) {
    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 130.0f);
    Quaternion q2(Vector3(1.0f, 2.0f, 4.0f), 260.0f);
    Quaternion q3 = q1.lerp(0.3f, q2);
    test.checkEqual(String("(0.22 i + 0.53 j + 0.81 k + 0.12)"), String(q3),
                    "lerp");
}

static void testMulSet(Test& test) {
    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 50.0f);
    Quaternion q2(Vector3(2.0f, 5.0f, 7.0f), 60.0f);
    Quaternion q3;
    q3 *= q1;
    test.checkEqual(String(q1), String(q3), "mul set 1");
    q3 *= q2;
    test.checkEqual(String(Quaternion(Vector3(2.0f, 5.0f, 7.0f), 110.0f)),
                    String(q3), "mul set 2");
}

static void testMul(Test& test) {
    Quaternion q1(Vector3(2.0f, 5.0f, 7.0f), 50.0f);
    Quaternion q2(Vector3(2.0f, 5.0f, 7.0f), 60.0f);
    Quaternion q3 = q1 * q2;
    test.checkEqual(String(Quaternion(Vector3(2.0f, 5.0f, 7.0f), 110.0f)),
                    String(q3), "mul");
}

static void testMulVector(Test& test) {
    Quaternion q1(Vector3(1.0f, 0.0f, 0.0f), 90.0f);
    Quaternion q2(Vector3(0.0f, 1.0f, 0.0f), 90.0f);
    Quaternion q3(Vector3(0.0f, 0.0f, 1.0f), 90.0f);

    Vector3 v1(1.0f, 0.0f, 0.0f);
    Vector3 v2(0.0f, 1.0f, 0.0f);
    Vector3 v3(0.0f, 0.0f, 1.0f);

    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f), q1 * v1,
                   "mul with vector 1");
    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f), q1 * v2,
                   "mul with vector 2");
    compareVectors(test, Vector3(0.0f, -1.0f, 0.0f), q1 * v3,
                   "mul with vector 3");

    compareVectors(test, Vector3(0.0f, 0.0f, -1.0f), q2 * v1,
                   "mul with vector 4");
    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f), q2 * v2,
                   "mul with vector 5");
    compareVectors(test, Vector3(1.0f, 0.0f, 0.0f), q2 * v3,
                   "mul with vector 6");

    compareVectors(test, Vector3(0.0f, 1.0f, 0.0f), q3 * v1,
                   "mul with vector 7");
    compareVectors(test, Vector3(-1.0f, 0.0f, 0.0f), q3 * v2,
                   "mul with vector 8");
    compareVectors(test, Vector3(0.0f, 0.0f, 1.0f), q3 * v3,
                   "mul with vector 9");
}

void QuaternionTests::test() {
    Test test("Quaternion");
    testInit(test);
    testAxisAndDegreesInit(test);
    testLerp(test);
    testMulSet(test);
    testMul(test);
    testMulVector(test);
    test.finalize();
}