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

using Core::Matrix;
using V3 = Core::Vector3;

static void testInit() {
    Matrix m;
    const float* data = m.getValues();
    for(int i = 0; i < 16; i++) {
        int x = i % 4;
        int y = i / 4;
        TEST_FLOAT(x == y, data[i], 0.0f);
    }
}

static void testTranspose() {
    Matrix m;
    m.set(0, Core::Vector4(1.0f, 2.0f, 3.0f, 4.0f));
    m.set(1, Core::Vector4(5.0f, 6.0f, 7.0f, 8.0f));
    m.set(2, Core::Vector4(9.0f, 10.0f, 11.0f, 12.0f));
    m.set(3, Core::Vector4(13.0f, 14.0f, 15.0f, 16.0f));
    Matrix t = m.transpose();
    Matrix m2 = t.transpose();

    const float* mp = m.getValues();
    const float* tp = t.getValues();
    for(int x = 0; x < 4; x++) {
        for(int y = 0; y < 4; y++) {
            TEST_FLOAT(mp[y * 4 + x], tp[x * 4 + y], 0.0f);
        }
    }
    const float* mp2 = m2.getValues();
    for(int i = 0; i < 16; i++) {
        TEST_FLOAT(mp[i], mp2[i], 0.0f);
    }
}

static void testScale() {
    Core::Matrix m;
    m.scale(V3(2.0f, 3.0f, 4.0f));
    TEST(V3(-8.0f, 18.0f, 28.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testUniformScale() {
    Core::Matrix m;
    m.scale(2.0f);
    TEST(V3(-8.0f, 12.0f, 14.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testTranslateX() {
    Core::Matrix m;
    m.translateX(5.0f);
    TEST(V3(1.0f, 6.0f, 7.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testTranslateY() {
    Core::Matrix m;
    m.translateY(6.0f);
    TEST(V3(-4.0f, 12.0f, 7.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testTranslateZ() {
    Core::Matrix m;
    m.translateZ(7.0f);
    TEST(V3(-4.0f, 6.0f, 14.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testTranslate() {
    Core::Matrix m;
    m.translate(V3(1.0f, 2.0f, 3.0f));
    TEST(V3(-3.0f, 8.0f, 10.0f), m * V3(-4.0f, 6.0f, 7.0f));
}

static void testTranslateTo() {
    Matrix m;
    m.translateTo(V3(6.0f, 8.0f, 9.0f));
    TEST_STRING(
        "[[1.00, 0.00, 0.00, 6.00], [0.00, 1.00, 0.00, 8.00], "
        "[0.00, 0.00, 1.00, 9.00], [0.00, 0.00, 0.00, 1.00]]",
        m);
}

static void testCombination() {
    Matrix m;
    m.scale(2.0f);
    m.translateX(1.0f);
    m.translateY(2.0f);
    m.translateZ(3.0f);
    m.translate(V3(-4.0f, 2.0f, 3.0f));
    m.scale(V3(2.0f, 3.0f, 4.0f));
    m.scale(0.5f);
    TEST(V3(-1.0f, 9.0f, 16.0f), m * V3(1.0f, 1.0f, 1.0f));
}

static void testMatrixCombination() {
    Matrix a;
    a.scale(2.0f);
    a.translate(V3(1.0f, 2.0f, 3.0f));

    Matrix b;
    b.scale(3.0f);
    b.translate(V3(1.0f, 1.0f, 1.0f));

    Matrix c;
    c.translate(V3(-1.0f, -2.0f, -3.0f));

    c *= b * a;
    TEST(V3(9.0f, 11.0f, 13.0f), c * V3(1.0f, 1.0f, 1.0f));
}

static void testRotateX() {
    Matrix m;
    m.rotateX(Core::degreeToRadian(90.0f));
    TEST(V3(1.0f, 0.0f, 0.0f), m * V3(1.0f, 0.0f, 0.0f));
    TEST(V3(0.0f, 0.0f, 1.0f), m * V3(0.0f, 1.0f, 0.0f));
    TEST(V3(0.0f, -1.0f, 0.0f), m * V3(0.0f, 0.0f, 1.0f));
}

static void testRotateY() {
    Matrix m;
    m.rotateY(Core::degreeToRadian(90.0f));
    TEST(V3(0.0f, 0.0f, -1.0f), m * V3(1.0f, 0.0f, 0.0f));
    TEST(V3(0.0f, 1.0f, 0.0f), m * V3(0.0f, 1.0f, 0.0f));
    TEST(V3(1.0f, 0.0f, 0.0f), m * V3(0.0f, 0.0f, 1.0f));
}

static void testRotateZ() {
    Matrix m;
    m.rotateZ(Core::degreeToRadian(90.0f));
    TEST(V3(0.0f, 1.0f, 0.0f), m * V3(1.0f, 0.0f, 0.0f));
    TEST(V3(-1.0f, 0.0f, 0.0f), m * V3(0.0f, 1.0f, 0.0f));
    TEST(V3(0.0f, 0.0f, 1.0f), m * V3(0.0f, 0.0f, 1.0f));
}

static void testQuaternionMatrix() {
    Core::Quaternion q1(V3(1.0f, 0.0f, 0.0f), Core::degreeToRadian(48.0f));
    Core::Quaternion q2(V3(0.0f, 1.0f, 0.0f), Core::degreeToRadian(52.0f));
    Core::Quaternion q3(V3(0.0f, 0.0f, 1.0f), Core::degreeToRadian(60.0f));

    Matrix m;
    m.translate(V3(1.0f, 2.0f, 3.0f));
    m.rotate(q1);
    m.rotate(q2);
    m.rotate(q3);
    m.translate(V3(1.0f, 2.0f, 3.0f));

    Matrix check;
    check.translate(V3(1.0f, 2.0f, 3.0f));
    check.rotateX(Core::degreeToRadian(48.0f));
    check.rotateY(Core::degreeToRadian(52.0f));
    check.rotateZ(Core::degreeToRadian(60.0f));
    check.translate(V3(1.0f, 2.0f, 3.0f));

    for(int i = 0; i < 16; i++) {
        TEST_FLOAT(check.getValues()[i], m.getValues()[i], 0.0001f);
    }
}

static void testToString() {
    Matrix m;
    m.set(0, Core::Vector4(1.0f, 2.0f, 3.0f, 4.0f));
    m.set(1, Core::Vector4(5.0f, 6.0f, 7.0f, 8.0f));
    m.set(2, Core::Vector4(9.0f, 10.0f, 11.0f, 12.0f));
    m.set(3, Core::Vector4(13.0f, 14.0f, 15.0f, 16.0f));
    char buffer[1024];
    size_t n = toString(m, buffer, sizeof(buffer));
    TEST(111, n);
    TEST_STRING(
        "[[1.00, 2.00, 3.00, 4.00], [5.00, 6.00, 7.00, 8.00], "
        "[9.00, 10.00, 11.00, 12.00], [13.00, 14.00, 15.00, 16.00]]",
        buffer);
    n = toString(m, buffer, 20);
    TEST(111, n);
    TEST_STRING("[[1.00, 2.00, 3.00,", buffer);
}

void testMatrix() {
    testInit();
    testTranspose();
    testScale();
    testUniformScale();
    testTranslateX();
    testTranslateY();
    testTranslateZ();
    testTranslate();
    testTranslateTo();
    testCombination();
    testMatrixCombination();
    testRotateX();
    testRotateY();
    testRotateZ();
    testQuaternionMatrix();
    testToString();
}