#include "catch.hpp"
#include <heinz/Rotations3D.h>
#include <heinz/Vectors3D.h>

using Catch::Matchers::WithinAbs;
using Catch::Matchers::WithinRel;

const double epsilon = 1e-12;

using RotMatrix = Rotation3D<double>;

#undef M_PI
#define M_PI 3.14159265358979323846
const double w0 = M_PI / 5;
const double w1 = M_PI / 7;
const double w2 = M_PI / 11;


const RotMatrix mEul = RotMatrix::EulerZXZ(w0, w1, w2);

void InversionTest(const RotMatrix& mRot, const R3& a0)
{

    const RotMatrix mInv = mRot.Inverse();

    const R3 a1 = mRot.transformed(a0);
    const R3 a2 = mInv.transformed(a1);

    CHECK_THAT(a2.x(), WithinRel(a0.x(), epsilon));
    CHECK_THAT(a2.y(), WithinRel(a0.y(), epsilon));
    CHECK_THAT(a2.z(), WithinRel(a0.z(), epsilon));
}

TEST_CASE("RotMatrixTest:ForthAndBack")
{
    const R3 vector(1.1,2.2,3.3);
    R3 result;
    const double angle = 12.375;

    // Rotating back and forth around X - axis
    const RotMatrix rotX1 = RotMatrix::AroundX(angle);
    const RotMatrix rotX2 = RotMatrix::AroundX(-angle);

    result = rotX2.transformed(rotX1.transformed(vector));

    CHECK_THAT(vector.x(), WithinRel(result.x(), epsilon));
    CHECK_THAT(vector.y(), WithinRel(result.y(), epsilon));
    CHECK_THAT(vector.z(), WithinRel(result.z(), epsilon));

    // Rotating back and forth around Y - axis
    const RotMatrix rotY1 = RotMatrix::AroundY(angle);
    const RotMatrix rotY2 = RotMatrix::AroundY(-angle);

    result = rotY2.transformed(rotY1.transformed(vector));

    CHECK_THAT(vector.x(), WithinRel(result.x(), epsilon));
    CHECK_THAT(vector.y(), WithinRel(result.y(), epsilon));
    CHECK_THAT(vector.z(), WithinRel(result.z(), epsilon));

    // Rotating back and forth around Z - axis
    const RotMatrix rotZ1 = RotMatrix::AroundZ(angle);
    const RotMatrix rotZ2 = RotMatrix::AroundZ(-angle);

    result = rotZ2.transformed(rotZ1.transformed(vector));

    CHECK_THAT(vector.x(), WithinRel(result.x(), epsilon));
    CHECK_THAT(vector.y(), WithinRel(result.y(), epsilon));
    CHECK_THAT(vector.z(), WithinRel(result.z(), epsilon));
}

TEST_CASE("RotMatrixTest:RotateY")
{
    const R3 a(std::sqrt(3.) / 2., 2., 0.5);
    const RotMatrix m2 = RotMatrix::AroundY(M_PI / 6.);
    const R3 v = m2.transformed(a);

    CHECK_THAT(v.x(), WithinRel(1.0, epsilon));
    CHECK(v.y() == 2.0);
    CHECK_THAT(v.z(), WithinAbs(0.0, 1e-9));
}

TEST_CASE("RotMatrixTest:RotateZ")
{
    const R3 a(0.5, std::sqrt(3.) / 2., 2.);
    const RotMatrix m3 = RotMatrix::AroundZ(M_PI / 6.);
    const R3 v = m3.transformed(a);

    CHECK_THAT(v.x(), WithinAbs(0.0, 1e-9));
    CHECK_THAT(v.y(), WithinRel(1.0, epsilon));
    CHECK(v.z() == 2.0);

    const RotMatrix m4 = m3.Inverse();
    const R3 w = m4.transformed(v);

    CHECK_THAT(w.x(), WithinRel(a.x(), epsilon));
    CHECK_THAT(w.y(), WithinRel(a.y(), epsilon));
    CHECK_THAT(w.z(), WithinRel(a.z(), epsilon));
}

TEST_CASE("RotMatrixTest:RecoverEulerAngles")
{
    auto angles = mEul.zxzEulerAngles();

    CHECK_THAT(angles[0], WithinRel(w0, epsilon));
    CHECK_THAT(angles[1], WithinRel(w1, epsilon));
    CHECK_THAT(angles[2], WithinRel(w2, epsilon));
}

TEST_CASE("RotMatrixTest:InvertXMatrix")
{
    InversionTest(RotMatrix::AroundX(M_PI / 7.), R3(4, 5, 6));
}

TEST_CASE("RotMatrixTest:InvertYMatrix")
{
    InversionTest(RotMatrix::AroundY(M_PI / 7.), R3(4, 5, 6));
}

TEST_CASE("RotMatrixTest:InvertZMatrix")
{
    InversionTest(RotMatrix::AroundZ(M_PI / 7.), R3(4, 5, 6));
}

TEST_CASE("RotMatrixTest:InvertEulerMatrix")
{
    InversionTest(mEul, R3(3, 4, 7));
}
