#include <library/cpp/testing/gtest/gtest.h>
#include <maps/libs/geolib/include/test_tools/comparison.h>
#include <maps/wikimap/mapspro/services/mrc/eye/lib/location/include/move.h>

namespace maps::mrc::eye::tests {

inline bool equal(geolib3::Heading lhs, geolib3::Heading rhs)
{
    static constexpr geolib3::Degrees EPS(1e-6);
    return geolib3::test_tools::approximateEqual(geolib3::normalize(lhs), geolib3::normalize(rhs), EPS);
}

inline Eigen::Vector3d normalize(const Eigen::Vector3d& v) { return v / v.norm(); }

TEST(move, to_move)
{
    {
        const geolib3::Heading heading(0);
        const Eigen::Vector3d expected{0, 1, 0};

        EXPECT_TRUE(toMoveVector(heading).isApprox(expected));
    }

    {
        const geolib3::Heading heading(90);
        const Eigen::Vector3d expected{1, 0, 0};

        EXPECT_TRUE(toMoveVector(heading).isApprox(expected));
    }

    {
        const geolib3::Heading heading(-90);
        const Eigen::Vector3d expected{-1, 0, 0};

        EXPECT_TRUE(toMoveVector(heading).isApprox(expected));
    }

    {
        const geolib3::Heading heading(180);
        const Eigen::Vector3d expected{0, -1, 0};

        EXPECT_TRUE(toMoveVector(heading).isApprox(expected));
    }
}

TEST(move, to_heading)
{
    {
        const Eigen::Vector3d move{0, 1, 0};
        const geolib3::Heading expected(0);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const Eigen::Vector3d move{1, 0, 0};
        const geolib3::Heading expected(90);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const Eigen::Vector3d move{0, -1, 0};
        const geolib3::Heading expected(180);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const Eigen::Vector3d move{-1, 0, 0};
        const geolib3::Heading expected(-90);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const auto move = normalize({1, 1, 1});
        const geolib3::Heading expected(45);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const auto move = normalize({-1, 1, 1});
        const geolib3::Heading expected(-45);

        EXPECT_TRUE(equal(*toHeading(move), expected));
    }

    {
        const auto move = normalize({0, 0, 1});
        EXPECT_EQ(toHeading(move), std::nullopt);
    }
}

TEST(move, abs_diff_in_degrees)
{
    const Eigen::Vector3d a{1, 0, 0};
    const Eigen::Vector3d b{0, 1, 0};

    EXPECT_EQ(absDiffInDegrees(a, a), geolib3::Degrees(0));
    EXPECT_EQ(absDiffInDegrees(a, b), geolib3::Degrees(90));
    EXPECT_EQ(absDiffInDegrees(b, a), geolib3::Degrees(90));
}

} // namespace maps::mrc::eye::tests
