#include "../Tests.h"
#include "core/Matrix.h"

typedef CoreVector3 V3;
#define CV3(a, b, c) (&(V3){{a, b, c}})
#define CV30 CV3(0.0f, 0.0f, 0.0f)

static void testInit() {
    Matrix m = UNIT_MATRIX;
    const float* data = (float*)&m;
    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;
    float* data = (float*)&m;
    for(int i = 0; i < 16; i++) {
        data[i] = (float)(i + 1);
    }
    Matrix t = m;
    transposeMatrix(&t);
    Matrix m2 = t;
    transposeMatrix(&m2);

    const float* mp = (float*)&m;
    const float* tp = (float*)&t;
    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 = (float*)&m2;
    for(int i = 0; i < 16; i++) {
        TEST_FLOAT(mp[i], mp2[i], 0.0f);
    }
}

static void testScale() {
    Matrix m = UNIT_MATRIX;
    scaleMatrix(&m, CV3(2.0f, 3.0f, 4.0f));
    TEST_V3(CV3(-8.0f, 18.0f, 28.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testUniformScale() {
    Matrix m = UNIT_MATRIX;
    scaleMatrixF(&m, 2.0f);
    TEST_V3(CV3(-8.0f, 12.0f, 14.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testTranslateX() {
    Matrix m = UNIT_MATRIX;
    translateMatrixX(&m, 5.0f);
    TEST_V3(CV3(1.0f, 6.0f, 7.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testTranslateY() {
    Matrix m = UNIT_MATRIX;
    translateMatrixY(&m, 6.0f);
    TEST_V3(CV3(-4.0f, 12.0f, 7.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testTranslateZ() {
    Matrix m = UNIT_MATRIX;
    translateMatrixZ(&m, 7.0f);
    TEST_V3(CV3(-4.0f, 6.0f, 14.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testTranslate() {
    Matrix m = UNIT_MATRIX;
    translateMatrix(&m, CV3(1.0f, 2.0f, 3.0f));
    TEST_V3(CV3(-3.0f, 8.0f, 10.0f),
            mulMatrixV3(CV30, &m, CV3(-4.0f, 6.0f, 7.0f)));
}

static void testTranslateTo() {
    char buffer[1024];
    Matrix m;
    for(int i = 0; i < 16; i++) {
        ((float*)&m)[i] = (float)i + 1.0f;
    }
    translateMatrixTo(&m, CV3(6.0f, 8.0f, 9.0f));
    toStringMatrix(&m, buffer, sizeof(buffer));
    TEST_STRING("[[1.000, 0.000, 0.000, 6.000], [0.000, 1.000, 0.000, 8.000], "
                "[0.000, 0.000, 1.000, 9.000], [0.000, 0.000, 0.000, 1.000]]",
                buffer);
}

static void testCombination() {
    Matrix m = UNIT_MATRIX;
    scaleMatrixF(&m, 2.0f);
    translateMatrixX(&m, 1.0f);
    translateMatrixY(&m, 2.0f);
    translateMatrixZ(&m, 3.0f);
    translateMatrix(&m, CV3(-4.0f, 2.0f, 3.0f));
    scaleMatrix(&m, CV3(2.0f, 3.0f, 4.0f));
    scaleMatrixF(&m, 0.5f);
    TEST_V3(CV3(-1.0f, 9.0f, 16.0f),
            mulMatrixV3(CV30, &m, CV3(1.0f, 1.0f, 1.0f)));
}

static void testMatrixCombination() {
    Matrix a = UNIT_MATRIX;
    scaleMatrixF(&a, 2.0f);
    translateMatrix(&a, CV3(1.0f, 2.0f, 3.0f));

    Matrix b = UNIT_MATRIX;
    scaleMatrixF(&b, 3.0f);
    translateMatrix(&b, CV3(1.0f, 1.0f, 1.0f));

    Matrix c = UNIT_MATRIX;
    translateMatrix(&c, CV3(-1.0f, -2.0f, -3.0f));

    mulSetMatrix(&c, mulMatrix(&ZERO_MATRIX, &b, &a));

    TEST_V3(CV3(9.0f, 11.0f, 13.0f),
            mulMatrixV3(CV30, &c, CV3(1.0f, 1.0f, 1.0f)));
}

static void testRotateX() {
    Matrix m = UNIT_MATRIX;
    rotateMatrixX(&m, 90.0f);
    TEST_V3(CV3(1.0f, 0.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(1.0f, 0.0f, 0.0f)));
    TEST_V3(CV3(0.0f, 0.0f, 1.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 1.0f, 0.0f)));
    TEST_V3(CV3(0.0f, -1.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 0.0f, 1.0f)));
}

static void testRotateY() {
    Matrix m = UNIT_MATRIX;
    rotateMatrixY(&m, 90.0f);
    TEST_V3(CV3(0.0f, 0.0f, -1.0f),
            mulMatrixV3(CV30, &m, CV3(1.0f, 0.0f, 0.0f)));
    TEST_V3(CV3(0.0f, 1.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 1.0f, 0.0f)));
    TEST_V3(CV3(1.0f, 0.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 0.0f, 1.0f)));
}

static void testRotateZ() {
    Matrix m = UNIT_MATRIX;
    rotateMatrixZ(&m, 90.0f);
    TEST_V3(CV3(0.0f, 1.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(1.0f, 0.0f, 0.0f)));
    TEST_V3(CV3(-1.0f, 0.0f, 0.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 1.0f, 0.0f)));
    TEST_V3(CV3(0.0f, 0.0f, 1.0f),
            mulMatrixV3(CV30, &m, CV3(0.0f, 0.0f, 1.0f)));
}

static void testQuaternionMatrix() {
    Quaternion q1 = UNIT_QUATERNION;
    axisAngleQ(&q1, CV3(1.0f, 0.0f, 0.0f), 48.0f);
    Quaternion q2 = UNIT_QUATERNION;
    axisAngleQ(&q2, CV3(0.0f, 1.0f, 0.0f), 52.0f);
    Quaternion q3 = UNIT_QUATERNION;
    axisAngleQ(&q3, CV3(0.0f, 0.0f, 1.0f), 60.0f);

    Matrix m = UNIT_MATRIX;
    translateMatrix(&m, CV3(1.0f, 2.0f, 3.0f));
    rotateMatrix(&m, &q1);
    rotateMatrix(&m, &q2);
    rotateMatrix(&m, &q3);
    translateMatrix(&m, CV3(1.0f, 2.0f, 3.0f));

    Matrix check = UNIT_MATRIX;
    translateMatrix(&check, CV3(1.0f, 2.0f, 3.0f));
    rotateMatrixX(&check, 48.0f);
    rotateMatrixY(&check, 52.0f);
    rotateMatrixZ(&check, 60.0f);
    translateMatrix(&check, CV3(1.0f, 2.0f, 3.0f));

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

static void testToString() {
    Matrix m;
    for(int i = 0; i < 16; i++) {
        ((float*)&m)[i] = (float)i + 1.0f;
    }
    char buffer[1024];
    size_t n = toStringMatrix(&m, buffer, sizeof(buffer));
    TEST_SIZE(127, n);
    TEST_STRING(
        "[[1.000, 2.000, 3.000, 4.000], [5.000, 6.000, 7.000, 8.000], "
        "[9.000, 10.000, 11.000, 12.000], [13.000, 14.000, 15.000, 16.000]]",
        buffer);

    n = toStringMatrix(&m, buffer, 20);
    TEST_SIZE(127, n);
    TEST_STRING("[[1.000, 2.000, 3.0", buffer);
}

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