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

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

static void testInit() {
    Q q = CORE_UNIT_QUATERNION;
    char buffer[128];
    coreToStringQ(&q, buffer, sizeof(buffer));
    CORE_TEST_STRING("(0.000 i + 0.000 j + 0.000 k + 1.000)", buffer);
}

static void testAxisAndDegreesInit() {
    Q q = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q, CV3(1.0f, 2.0f, 3.0f), 142.0f);
    char buffer[128];
    coreToStringQ(&q, buffer, sizeof(buffer));
    CORE_TEST_STRING("(0.253 i + 0.505 j + 0.758 k + 0.326)", buffer);
}

static void testLerp() {
    Q q1 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 130.0f);
    Q q2 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q2, CV3(1.0f, 2.0f, 4.0f), 260.0f);
    Q q3;
    coreLerpQ(&q3, &q1, 0.3f, &q2);

    char buffer[128];
    coreToStringQ(&q3, buffer, sizeof(buffer));
    CORE_TEST_STRING("(0.223 i + 0.529 j + 0.810 k + 0.119)", buffer);
}

static void testMulSet() {
    Q q1 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 50.0f);
    Q q2 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), 60.0f);
    Q q3 = CORE_UNIT_QUATERNION;
    coreMulSetQ(&q3, &q1);

    char bufferQ1[128];
    coreToStringQ(&q1, bufferQ1, sizeof(bufferQ1));
    char bufferQ3[128];
    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));
    CORE_TEST_STRING(bufferQ1, bufferQ3);

    coreMulSetQ(&q3, &q2);
    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));

    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 110.0f);
    coreToStringQ(&q1, bufferQ1, sizeof(bufferQ1));

    CORE_TEST_STRING(bufferQ1, bufferQ3);
}

static void testMul() {
    Q q1 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), 50.0f);
    Q q2 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), 60.0f);
    Q q3 = CORE_UNIT_QUATERNION;
    coreMulQ(&q3, &q1, &q2);

    char bufferQ3[128];
    coreToStringQ(&q3, bufferQ3, sizeof(bufferQ3));

    Q q = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q, CV3(2.0f, 5.0f, 7.0f), 110.0f);
    char bufferQ[128];
    coreToStringQ(&q, bufferQ, sizeof(bufferQ));

    CORE_TEST_STRING(bufferQ, bufferQ3);
}

static void testMulVector() {
    Q q1 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q1, CV3(1.0f, 0.0f, 0.0f), 90.0f);
    Q q2 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q2, CV3(0.0f, 1.0f, 0.0f), 90.0f);
    Q q3 = CORE_UNIT_QUATERNION;
    coreAxisAngleQ(&q3, CV3(0.0f, 0.0f, 1.0f), 90.0f);

    CoreVector3 v1 = {{1.0f, 0.0f, 0.0f}};
    CoreVector3 v2 = {{0.0f, 1.0f, 0.0f}};
    CoreVector3 v3 = {{0.0f, 0.0f, 1.0f}};

    CORE_TEST_V3(CV3(1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q1, &v1));
    CORE_TEST_V3(CV3(0.0f, 0.0f, 1.0f), coreMulQV3(CV30, &q1, &v2));
    CORE_TEST_V3(CV3(0.0f, -1.0f, 0.0f), coreMulQV3(CV30, &q1, &v3));
    CORE_TEST_V3(CV3(0.0f, 0.0f, -1.0f), coreMulQV3(CV30, &q2, &v1));
    CORE_TEST_V3(CV3(0.0f, 1.0f, 0.0f), coreMulQV3(CV30, &q2, &v2));
    CORE_TEST_V3(CV3(1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q2, &v3));
    CORE_TEST_V3(CV3(0.0f, 1.0f, 0.0f), coreMulQV3(CV30, &q3, &v1));
    CORE_TEST_V3(CV3(-1.0f, 0.0f, 0.0f), coreMulQV3(CV30, &q3, &v2));
    CORE_TEST_V3(CV3(0.0f, 0.0f, 1.0f), coreMulQV3(CV30, &q3, &v3));
}

void coreTestQuaternion() {
    testInit();
    testAxisAndDegreesInit();
    testLerp();
    testMulSet();
    testMul();
    testMulVector();
}