#include "../Tests.h"
#include "core/Generic.h"
#include "core/Utility.h"

#define CV3(a, b, c) (&(Vector3){{a, b, c}})
#define CV30 CV3(0.0f, 0.0f, 0.0f)

static void testInit() {
    Quaternion q = UNIT_QUATERNION;
    char buffer[128];
    toStringQ(&q, buffer, sizeof(buffer));
    TEST_STRING("(0.000 i + 0.000 j + 0.000 k + 1.000)", buffer);
}

static void testAxisAndDegreesInit() {
    Quaternion q = UNIT_QUATERNION;
    axisAngleQ(&q, CV3(1.0f, 2.0f, 3.0f), degreeToRadian(142.0f));
    char buffer[128];
    toStringQ(&q, buffer, sizeof(buffer));
    TEST_STRING("(0.253 i + 0.505 j + 0.758 k + 0.326)", buffer);
}

static void testLerp() {
    Quaternion q1 = UNIT_QUATERNION;
    axisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(130.0f));
    Quaternion q2 = UNIT_QUATERNION;
    axisAngleQ(&q2, CV3(1.0f, 2.0f, 4.0f), degreeToRadian(260.0f));
    Quaternion q3;
    lerpQ(&q3, &q1, 0.3f, &q2);

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

static void testMulSet() {
    Quaternion q1 = UNIT_QUATERNION;
    axisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(50.0f));
    Quaternion q2 = UNIT_QUATERNION;
    axisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(60.0f));
    Quaternion q3 = UNIT_QUATERNION;
    mulSet(&q3, &q1);

    char bufferQ1[128];
    toStringQ(&q1, bufferQ1, sizeof(bufferQ1));
    char bufferQ3[128];
    toStringQ(&q3, bufferQ3, sizeof(bufferQ3));
    TEST_STRING(bufferQ1, bufferQ3);

    mulSet(&q3, &q2);
    toStringQ(&q3, bufferQ3, sizeof(bufferQ3));

    axisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(110.0f));
    toStringQ(&q1, bufferQ1, sizeof(bufferQ1));

    TEST_STRING(bufferQ1, bufferQ3);
}

static void testMul() {
    Quaternion q1 = UNIT_QUATERNION;
    axisAngleQ(&q1, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(50.0f));
    Quaternion q2 = UNIT_QUATERNION;
    axisAngleQ(&q2, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(60.0f));
    Quaternion q3 = UNIT_QUATERNION;
    mul(&q3, &q1, &q2);

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

    Quaternion q = UNIT_QUATERNION;
    axisAngleQ(&q, CV3(2.0f, 5.0f, 7.0f), degreeToRadian(110.0f));
    char bufferQ[128];
    toStringQ(&q, bufferQ, sizeof(bufferQ));

    TEST_STRING(bufferQ, bufferQ3);
}

static void testMulVector() {
    Quaternion q1 = UNIT_QUATERNION;
    axisAngleQ(&q1, CV3(1.0f, 0.0f, 0.0f), degreeToRadian(90.0f));
    Quaternion q2 = UNIT_QUATERNION;
    axisAngleQ(&q2, CV3(0.0f, 1.0f, 0.0f), degreeToRadian(90.0f));
    Quaternion q3 = UNIT_QUATERNION;
    axisAngleQ(&q3, CV3(0.0f, 0.0f, 1.0f), degreeToRadian(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}};

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

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