#include <cruft/util/vector.hpp>

#include <cruft/util/maths.hpp>
#include <cruft/util/tap.hpp>

#include <cruft/util/float.hpp>
#include <cruft/util/coord/iostream.hpp>

using cruft::vector;
using cruft::vector2f;


///////////////////////////////////////////////////////////////////////////////
void
test_polar (cruft::TAP::logger &tap)
{
    static const struct {
        cruft::vector2f polar;
        cruft::vector2f cartesian;
        const char *desc;
    } TESTS[] {
        {
            { 0.f, 0.f },
            { 0.f, 0.f },
            "all zeroes"
        },

        {
            { 1.f, 0.f },
            { 1.f, 0.f },
            "unit length, unrotated"
        },

        {
            { 1.f, cruft::pi<float> / 2.f },
            { 0.f, 1.f },
            "unit length, rotated"
        },

        {
            { 1.f, 2 * cruft::pi<float> },
            { 1.f, 0.f },
            "full rotation, unit length"
        }
    };

    for (const auto &t: TESTS) {
        // Compare the difference of cartesian representations. Don't use
        // direct equality comparisons here as the numeric stability can be
        // poor and we have nice whole numbers to start with.
        auto in_cart = t.cartesian;
        auto to_cart = cruft::polar_to_cartesian (t.polar);

        tap.expect_lt (norm (in_cart - to_cart), 0.00001f, "{:s}", t.desc);

        // Compare polar representations. Make sure to normalise them first.
        auto in_polar = t.polar;
        auto to_polar = cruft::cartesian_to_polar (t.cartesian);

        in_polar[1] = std::fmod (in_polar[1], 2 * cruft::pi<float>);
        to_polar[1] = std::fmod (to_polar[1], 2 * cruft::pi<float>);

        tap.expect_eq (in_polar, to_polar, "{:s}", t.desc);
    }
}


///////////////////////////////////////////////////////////////////////////////
void
test_euler (cruft::TAP::logger &tap)
{
    static const struct {
        cruft::vector3f dir;
        cruft::vector2f euler;
        const char *name;
    } TESTS[] = {
        // y-axis
        { {  0,  0, -1 }, { 0.5f,  0.5f }, "forward" },
        { { -1,  0,  0 }, { 0.5f, -1.0f }, "left" },
        { {  0,  0,  1 }, { 0.5f, -0.5f }, "back" },
        { {  1,  0,  0 }, { 0.5f,  0.0f }, "right" },

        // x-axis
        { {  0,  1,  0 }, { 0, 0 }, "up"   },
        { {  0, -1,  0 }, { 1, 0 }, "down" },
    };

    // check that simple axis rotations look correct
    for (auto i: TESTS) {
        tap.expect_eq (cruft::to_euler (i.dir),
                       i.euler * cruft::pi<float>,
                       "to euler, {:s}", i.name);
    }

    // check error in round trip through euler angles
    for (auto i: TESTS) {
        auto trip = cruft::from_euler (cruft::to_euler (i.dir));
        auto diff = i.dir - trip;
        auto mag  = norm (diff);

        // trig functions reduce precision above almost_equal levels, so we
        // hard code a fairly low bound here instead.
        tap.expect_lt (mag, 1e-7, "euler round-trip error, {:s}", i.name);
    }
}


///////////////////////////////////////////////////////////////////////////////
void
test_spherical (cruft::TAP::logger &tap)
{
    constexpr auto q = cruft::pi<float> / 2.f;

    static constexpr struct {
        cruft::vector3f spherical;
        cruft::vector3f cartesian;
        const char *message;
    } S2C [] = {
        { {  1,  0 * q,  0 * q }, {  0,  0,  1 }, "+zero", },
        { {  1,  2 * q,  0 * q }, {  0,  0, -1 }, "-zero", },

        { {  1,  1 * q,  0 * q }, {  1,  0,  0 }, "90-theta", },
        { {  1,  2 * q,  0 * q }, {  0,  0, -1 }, "180-theta", },
        { {  1,  3 * q,  0 * q }, { -1,  0,  0 }, "270-theta", },

        { {  1,  0 * q,  1 * q }, {  0,  0,  1 }, "90-phi", },
        { {  1,  0 * q,  2 * q }, {  0,  0,  1 }, "180-phi", },
        { {  1,  0 * q,  3 * q }, {  0,  0,  1 }, "270-phi", },

        { {  1,  1 * q,  1 * q }, {  0,  1,  0 }, "90-theta,  90-phi" },
        { {  1,  1 * q,  2 * q }, { -1,  0,  0 }, "90-theta, 180-phi" },
        { {  1,  1 * q,  3 * q }, {  0, -1,  0 }, "90-theta, 270-phi" },
    };

    for (const auto t: S2C) {
        tap.expect (
            all (abs (cruft::spherical_to_cartesian (t.spherical) - t.cartesian) < 1e-7f),
            "{:s}, spherical-cartesian",
            t.message
        );
    }


    // double check origin points are transformed correctly. the zeros and
    // multiple representations need to be accounted for.
    tap.expect_eq (
        cruft::cartesian_to_spherical (cruft::vector3f{0}).x,
        0,
        "origin, cartesian-spherical"
    );

    tap.expect_eq (
        cruft::spherical_to_cartesian (cruft::vector3f{0,1,-1}),
        cruft::vector3f{0},
        "origin, cartesian-spherical"
    );


    //-------------------------------------------------------------------------
    // dedicated cartesian-spherical test cases. it's hard to use the
    // spherical-cartesian cases from above because. some of the tests don't
    // have unique representations in spherical space,
    //      eg, {1,0,z} is always {0,0,1})
    // so the reverse transform isn't usable as a test.
    static constexpr struct {
        cruft::vector3f cartesian;
        cruft::vector3f spherical;
        const char *message;
    } C2S[] = {
        { {  0,  0,  1 }, { 1,   0, 0 }, "+z" },
        { {  0,  0, -1 }, { 1, q*2, 0 }, "-z" },

        { {  0,  1,  0 }, { 1, q,  q }, "+y" },
        { {  0, -1,  0 }, { 1, q, -q }, "-y" },

        { {  1,  0,  0 }, { 1, q,   0 }, "+x" },
        { { -1,  0,  0 }, { 1, q, q*2 }, "-x" },

        { {  1,  1,  1 }, { 1.732f, 0.95539f, q/2 }, "+ve" },
        { { -1, -1, -1 }, { 1.732f, 2.18619942f,  -2.35619f}, "-ve" },

        { {  9,  9,  9 }, { 15.5885f, 0.955317f, q/2 }, "+9ve" },
    };


    for (const auto &t: C2S) {
        const auto s0 = cruft::canonical_spherical (cruft::cartesian_to_spherical (t.cartesian));
        const auto s1 = cruft::canonical_spherical (t.spherical);
        tap.expect (
            all (abs (s0 - s1) < 1e-4f),
            "{:s}, cartesian-spherical",
            t.message
        );
    }
        //auto s = cruft::cartesian_to_spherical (t.cartesian);
        //std::clog << s << " == " << t.spherical << '\n';
        //tap.expect_eq (
        //    cruft::cartesian_to_spherical (t.cartesian),
        //    t.spherical,
        //    "{:s}, cartesian-spherical",
        //    t.message
        //);


    {
        //cruft::vector3f s { 1, .5f, 2/3.f };
        //cruft::vector3f c { 0.35f, 0.61f, 0.71f };
        //tap.expect_eq
    }
};


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

    test_polar (tap);
    test_euler (tap);
    test_spherical (tap);

    tap.expect (!is_normalised (cruft::vector3f::zeros ()), "zeros isn't normalised");
    tap.expect (!is_normalised (cruft::vector3f::ones  ()), "ones isn't normalised");

    tap.expect_eq (
        cruft::hypot (cruft::vector3f{0,1,2} - cruft::vector3f{3,2,4}),
        std::sqrt (14.f),
        "vector3f hypot"
    );

    return tap.status ();
}