//
//  DualQuaternionTests.cpp
//  tests/shared/src
//
//  Copyright 2017 High Fidelity, Inc.
//
//  Distributed under the Apache License, Version 2.0.
//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//

#include "DualQuaternionTests.h"

#include <iostream>

#include <DualQuaternion.h>
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include <StreamUtils.h>

#include <test-utils/GLMTestUtils.h>
#include <test-utils/QTestExtensions.h>

QTEST_MAIN(DualQuaternionTests)

static void quatComp(const glm::quat& q1, const glm::quat& q2) {
    QCOMPARE_WITH_ABS_ERROR(q1.x, q2.x, EPSILON);
    QCOMPARE_WITH_ABS_ERROR(q1.y, q2.y, EPSILON);
    QCOMPARE_WITH_ABS_ERROR(q1.z, q2.z, EPSILON);
    QCOMPARE_WITH_ABS_ERROR(q1.w, q2.w, EPSILON);
}

void DualQuaternionTests::ctor() {
    glm::quat real = angleAxis(PI / 2.0f, Vectors::UNIT_Y);
    glm::quat dual(0.0f, 1.0f, 2.0f, 3.0f);

    DualQuaternion dq(real, dual);
    quatComp(real, dq.real());
    quatComp(dual, dq.dual());

    glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X);
    glm::vec3 translation(1.0, 2.0f, 3.0f);
    dq = DualQuaternion(rotation, translation);
    quatComp(rotation, dq.getRotation());
    QCOMPARE_WITH_ABS_ERROR(translation, dq.getTranslation(), EPSILON);

    rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z);
    translation = glm::vec3(-1.0, 12.0f, 2.0f);
    glm::mat4 m = createMatFromQuatAndPos(rotation, translation);
    dq = DualQuaternion(m);
    quatComp(rotation, dq.getRotation());
    QCOMPARE_WITH_ABS_ERROR(translation, dq.getTranslation(), EPSILON);
}

void DualQuaternionTests::mult() {

    glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X);
    glm::vec3 translation(1.0, 2.0f, 3.0f);
    glm::mat4 m1 = createMatFromQuatAndPos(rotation, translation);
    DualQuaternion dq1(m1);

    rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z);
    translation = glm::vec3(-1.0, 12.0f, 2.0f);
    glm::mat4 m2 = createMatFromQuatAndPos(rotation, translation);
    DualQuaternion dq2(m2);

    DualQuaternion dq3 = dq1 * dq2;
    glm::mat4 m3 = m1 * m2;

    rotation = glmExtractRotation(m3);
    translation = extractTranslation(m3);

    quatComp(rotation, dq3.getRotation());
    QCOMPARE_WITH_ABS_ERROR(translation, dq3.getTranslation(), EPSILON);
}

void DualQuaternionTests::xform() {

    glm::quat rotation = angleAxis(PI / 3.0f, Vectors::UNIT_X);
    glm::vec3 translation(1.0, 2.0f, 3.0f);
    glm::mat4 m1 = createMatFromQuatAndPos(rotation, translation);
    DualQuaternion dq1(m1);

    rotation = angleAxis(-2.0f * PI / 7.0f, Vectors::UNIT_Z);
    translation = glm::vec3(-1.0, 12.0f, 2.0f);
    glm::mat4 m2 = createMatFromQuatAndPos(rotation, translation);
    DualQuaternion dq2(m2);

    DualQuaternion dq3 = dq1 * dq2;
    glm::mat4 m3 = m1 * m2;

    glm::vec3 p(1.0f, 2.0f, 3.0f);

    glm::vec3 p1 = transformPoint(m3, p);
    glm::vec3 p2 = dq3.xformPoint(p);

    QCOMPARE_WITH_ABS_ERROR(p1, p2, 0.001f);

    p1 = transformVectorFast(m3, p);
    p2 = dq3.xformVector(p);

    QCOMPARE_WITH_ABS_ERROR(p1, p2, 0.001f);
}

void DualQuaternionTests::trans() {
    glm::vec3 t1 = glm::vec3();
    DualQuaternion dq1(Quaternions::IDENTITY, t1);
    glm::vec3 t2 = glm::vec3(1.0f, 2.0f, 3.0f);
    DualQuaternion dq2(angleAxis(PI / 3.0f, Vectors::UNIT_X), t2);
    glm::vec3 t3 = glm::vec3(3.0f, 2.0f, 1.0f);
    DualQuaternion dq3(angleAxis(PI / 5.0f, Vectors::UNIT_Y), t3);

    QCOMPARE_WITH_ABS_ERROR(t1, dq1.getTranslation(), 0.001f);
    QCOMPARE_WITH_ABS_ERROR(t2, dq2.getTranslation(), 0.001f);
    QCOMPARE_WITH_ABS_ERROR(t3, dq3.getTranslation(), 0.001f);
}