#include <cruft/util/quaternion.hpp>

#include <cruft/util/tap.hpp>
#include <cruft/util/types.hpp>
#include <cruft/util/maths.hpp>
#include <cruft/util/coord/iostream.hpp>

using cruft::quaternion;
using cruft::quaternionf;


///////////////////////////////////////////////////////////////////////////////
int
main (void)
{
    cruft::TAP::logger tap;

    // identity relations
    tap.expect_eq (
        norm (quaternionf::identity ()), 1.f,
        "identity magnitude is unit"
    );

    tap.expect_eq (
        quaternionf::identity () * quaternionf::identity (),
        quaternionf::identity (),
        "identity multiplication with identity"
    );

    // normalisation
    {
        auto val = normalised (quaternionf {2, 3, 4, 7});

        tap.expect_eq (
            val * quaternionf::identity (),
            val,
            "identity multiplication with quaternion constant"
        );
    }

    {
        // construct two quaternions for a trivial multiplication check.
        //
        // we use vector as an intermediate form so that normalisation can be
        // done before the quaternion code is invoked (which may consider the
        // values to be invalid and throw an exception given it's geared more
        // towards rotations than general maths).
        cruft::vector4f a_v { 2, -11,  5, -17};
        cruft::vector4f b_v { 3,  13, -7, -19};
        a_v = normalised (a_v);
        b_v = normalised (b_v);

        auto a = quaternionf { a_v[0], a_v[1], a_v[2], a_v[3] };
        auto b = quaternionf { b_v[0], b_v[1], b_v[2], b_v[3] };
        auto c = quaternionf {
            -0.27358657116960006f,
            -0.43498209092420004f,
            -0.8443769181970001f,
            -0.15155515799559996f,
        };

        tap.expect_eq  (a * b, c, "multiplication");
        tap.expect_neq (b * a, c, "multiplication is not commutative");
    }

    tap.expect_eq (
        quaternionf::identity ().as_matrix (),
        cruft::matrix4f::identity (),
        "identity quaternion to matrix"
    );


    // check that concatenated transforms are identical to their matrix
    // equivalent (which we are more likely to have correct).
    {
        static const struct {
            float mag;
            cruft::vector3f axis;
        } ROTATIONS[] = {
            { 1.f, { 1.f, 0.f, 0.f } },
            { 1.f, { 0.f, 1.f, 0.f } },
            { 1.f, { 0.f, 0.f, 1.f } },
        };

        for (size_t i = 0; i < std::size (ROTATIONS); ++i) {
            const auto &r = ROTATIONS[i];

            auto q = quaternionf::angle_axis  (r.mag, r.axis).as_matrix ();
            auto m = cruft::rotation<float> (r.mag, r.axis);
            auto diff = cruft::abs (q - m);

            tap.expect_lt (cruft::sum (diff), 1e-6f, "single basis rotation {:d}", i);
        }

        auto q = quaternionf::identity ();
        auto m = cruft::matrix4f::identity ();

        for (auto r: ROTATIONS) {
            q = q.angle_axis (r.mag, r.axis) * q;
            m = cruft::rotation<float> (r.mag, r.axis) * m;
        }

        auto diff = cruft::abs (q.as_matrix () - m);
        tap.expect_lt (cruft::sum (diff), 1e-6f, "chained single axis rotations");
    }

    // ensure vector rotation quaternions actually rotate a vector
    {
        const struct {
            cruft::vector3f src;
            cruft::vector3f dst;
            const char *msg;
        } TESTS[] = {
            { {  1,  0,  0 }, {  1,  0,  0 }, "x-axis identity" },
            { {  0,  1,  0 }, {  0,  1,  0 }, "y-axis identity" },
            { {  0,  0,  1 }, {  0,  0,  1 }, "z-axis identity" },
            { {  0,  0,  1 }, {  0,  0, -1 }, "+z to -z" },
            { {  0,  0,  1 }, { -1,  0,  0 }, "+z to -x" },
            { {  1, -2,  3 }, { -4,  5, -6 }, "incremental" },
        };

        for (const auto &t: TESTS) {
            auto src = normalised (t.src);
            auto dst = normalised (t.dst);

            auto q = quaternionf::from_to (src, dst);
            auto v = rotate (src, q);

            auto diff = std::abs (cruft::sum (dst - v));
            tap.expect_lt (diff, 1e-06f, "quaternion from-to, {:s}", t.msg);
        }
    }

    {
        static struct {
            float x, y, z;
            cruft::quaternionf expected;
            char const *msg;
        } const TESTS[] = {
            { 1.f, 2.f, 3.f, { -0.3688714f, 0.7549338f, -0.2061492f, 0.5015091f, }, "positive rotation", },
        };

        for (auto const &t: TESTS) {
            tap.expect_eq (
                cruft::quaternionf::from_euler ({ t.x, t.y, t.z }),
                t.expected,
                "from_euler: {}",
                t.msg
            );
        }
    }

    return tap.status ();
}