#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/rotation.h>

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

namespace {

using namespace geolib3::test_tools;

std::vector<double> makeCameraRodrigues(
        const Eigen::Vector3d& cameraRight,
        const Eigen::Vector3d& cameraFront,
        const Eigen::Vector3d& cameraUp)
{
    constexpr double EPS = 1e-6;

    Eigen::Matrix3d rotationMatrix;
    rotationMatrix << cameraRight, cameraFront, cameraUp;

    const Eigen::AngleAxisd rotation(rotationMatrix);
    if (rotation.angle() < EPS) {
        return std::vector<double>(3, 0);
    }

    const Eigen::Vector3d result =  rotation.angle() * rotation.axis();
    return std::vector<double>(result.data(), result.data() + result.size());
}

static const auto X = Eigen::Vector3d::UnitX();
static const auto Y = Eigen::Vector3d::UnitY();
static const auto Z = Eigen::Vector3d::UnitZ();

} // namespace

TEST(rotation, heading_orientation_to_frame_rotation)
{
    {
        const geolib3::Heading heading(0);
        const common::ImageOrientation orientation(common::Rotation::CW_0);

        const Eigen::Matrix3d expected = makeRotationMatrix(X, -Z, Y);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(90);
        const common::ImageOrientation orientation(common::Rotation::CW_0);

        const Eigen::Matrix3d expected = makeRotationMatrix(-Y, -Z, X);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(-90);
        const common::ImageOrientation orientation(common::Rotation::CW_0);

        const Eigen::Matrix3d expected = makeRotationMatrix(Y, -Z, -X);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(180);
        const common::ImageOrientation orientation(common::Rotation::CW_0);

        const Eigen::Matrix3d expected = makeRotationMatrix(-X, -Z, -Y);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(90);
        const common::ImageOrientation orientation(common::Rotation::CW_90);

        const Eigen::Matrix3d expected = makeRotationMatrix(Z, -Y, X);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(0);
        const common::ImageOrientation orientation(common::Rotation::CW_180);

        const Eigen::Matrix3d expected = makeRotationMatrix(-X, Z, Y);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(-90);
        const common::ImageOrientation orientation(common::Rotation::CW_270);

        const Eigen::Matrix3d expected = makeRotationMatrix(-Z, -Y, -X);
        const Eigen::Quaterniond result = toRotation(heading, orientation);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const geolib3::Heading heading(0);
        const common::ImageOrientation orientation(true, common::Rotation::CW_0);

        /* Invalid oientation */
        EXPECT_THROW(toRotation(heading, orientation), RuntimeError);
    }
}

TEST(rotation, camera_rodrigues_to_frame_orientation)
{
    {
        const std::vector<double> cameraRodrigues = makeCameraRodrigues(X, Y, Z);

        const Eigen::Matrix3d expected = makeRotationMatrix(-Z, -X, Y);
        const Eigen::Quaterniond result = toRotation(cameraRodrigues);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const std::vector<double> cameraRodrigues = makeCameraRodrigues(Z, Y, -X);

        const Eigen::Matrix3d expected = makeRotationMatrix(X, -Z, Y);
        const Eigen::Quaterniond result = toRotation(cameraRodrigues);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const std::vector<double> cameraRodrigues = makeCameraRodrigues(Y, -X, Z);

        const Eigen::Matrix3d expected = makeRotationMatrix(-Z, -Y, -X);
        const Eigen::Quaterniond result = toRotation(cameraRodrigues);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }

    {
        const std::vector<double> cameraRodrigues = makeCameraRodrigues(-Z, X, -Y);

        const Eigen::Matrix3d expected = makeRotationMatrix(Y, Z, X);
        const Eigen::Quaterniond result = toRotation(cameraRodrigues);

        EXPECT_TRUE(result.toRotationMatrix().isApprox(expected));
    }
}

TEST(rotation, decompose_frame_orientation)
{
    constexpr geolib3::Degrees EPS_DEG(1e-6);

    const Eigen::Quaterniond base(makeRotationMatrix(X, -Z, Y));

    {
        const Eigen::Quaterniond rotation = base;

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(0), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_0));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(0), EPS_DEG));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(-M_PI/4, -Y);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(45), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_0));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(0), EPS_DEG));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(M_PI/4, -Y)
            * Eigen::AngleAxisd(M_PI/6, X);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(-45), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_0));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(30), EPS_DEG));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(M_PI, -Y)
            * Eigen::AngleAxisd(M_PI/6, X)
            * Eigen::AngleAxisd(M_PI/2, Z);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(180), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CCW_90));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(30), EPS_DEG));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(-M_PI/4, -Y)
            * Eigen::AngleAxisd(-M_PI/6, X)
            * Eigen::AngleAxisd(M_PI, Z);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(45), EPS_DEG));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(-30), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_180));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(M_PI/2, -Y)
            * Eigen::AngleAxisd(-M_PI/4, X)
            * Eigen::AngleAxisd(M_PI + M_PI/6, Z);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(-90), EPS_DEG));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(-45), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_180));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(3*M_PI/4, X);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(180), EPS_DEG));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(45), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation(common::Rotation::CW_180));
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(M_PI/2, X);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        // Extreme pitch, use default values
        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(0), EPS_DEG));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(90), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation());
    }

    {
        const Eigen::Quaterniond rotation = base
            * Eigen::AngleAxisd(-M_PI/2, X);

        const auto [heading, orientation, pitch] = decomposeRotation(rotation);

        // Extreme pitch, use default values
        EXPECT_TRUE(approximateEqual(heading, geolib3::Heading(0), EPS_DEG));
        EXPECT_TRUE(approximateEqual(pitch, geolib3::Degrees(-90), EPS_DEG));
        EXPECT_EQ(orientation, common::ImageOrientation());
    }
}

TEST(rotation, abs_diff_in_angles)
{
    const Eigen::Quaterniond a(Eigen::AngleAxisd(M_PI/4, X));
    const Eigen::Quaterniond b(Eigen::AngleAxisd(M_PI/2, X));

    constexpr geolib3::Degrees EPS_DEG(1e-6);

    EXPECT_TRUE(approximateEqual(absDiffInDegrees(b, b), geolib3::Degrees(0), EPS_DEG));
    EXPECT_TRUE(approximateEqual(absDiffInDegrees(a, b), geolib3::Degrees(45), EPS_DEG));
    EXPECT_TRUE(approximateEqual(absDiffInDegrees(b, a), geolib3::Degrees(45), EPS_DEG));
}

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