#include <library/cpp/testing/gtest/gtest.h>
#include <library/cpp/testing/common/env.h>
#include <maps/wikimap/mapspro/services/mrc/libs/position_improvment/impl/rodrigues.h>

namespace maps::mrc::pos_improvment::tests {

namespace {

const double EPS = 0.0001;

#define EXPECT_VECTORS_ARE_NEAR(V1,V2)     \
do {                                       \
    EXPECT_NEAR(V1.x(), V2.x(), EPS);      \
    EXPECT_NEAR(V1.y(), V2.y(), EPS);      \
    EXPECT_NEAR(V1.z(), V2.z(), EPS);      \
} while (0)

#define rDir cameraRightDirection
#define fDir cameraFrontDirection
#define uDir cameraUpDirection

} // anonymous namespace

TEST(rodrigues_angles_tests, test_zero_angles)
{
    CameraOrientation or1{
        UnitVector3({1, 0, 0}),
        UnitVector3({0, 1, 0}),
        UnitVector3({0, 0, 1})};

    std::vector<double> rodrigues = toRodrigues(or1);
    CameraOrientation or2 = fromRodrigues(rodrigues);

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_rotation_around_z)
{
    CameraOrientation or1{
        UnitVector3({0, 1, 0}),
        UnitVector3({-1, 0, 0}),
        UnitVector3({0, 0, 1})};

    std::vector<double> rodrigues = toRodrigues(or1);
    CameraOrientation or2 = fromRodrigues(rodrigues);

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_rotation_around_x)
{
    CameraOrientation or1{
        UnitVector3({1, 0, 0}),
        UnitVector3({0, 0, 1}),
        UnitVector3({0, -1, 0})};

    std::vector<double> rodrigues = toRodrigues(or1);
    CameraOrientation or2 = fromRodrigues(rodrigues);

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_negative_rotation_around_z)
{
    CameraOrientation or1{
        UnitVector3({1, 0, 0}),
        UnitVector3({0, 0, -1}),
        UnitVector3({0, 1, 0})};

    std::vector<double> rodrigues = toRodrigues(or1);
    CameraOrientation or2 = fromRodrigues(rodrigues);

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_3_rotations)
{
    CameraOrientation or1{
        UnitVector3({0, 0, -1}),
        UnitVector3({0, 1, 0}),
        UnitVector3({1, 0, 0})};

    std::vector<double> rodrigues = toRodrigues(or1);
    CameraOrientation or2 = fromRodrigues(rodrigues);

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_newZ_almost_equal_to_z)
{
    geolib3::Vector3 z(1, 0, 10000);
    geolib3::Vector3 t(15, 10, 0);
    geolib3::Vector3 x = geolib3::crossProduct(t, z);
    geolib3::Vector3 y = geolib3::crossProduct(z, x);

    CameraOrientation or1{ UnitVector3(x), UnitVector3(y), UnitVector3(z)};
    CameraOrientation or2 = fromRodrigues(toRodrigues(or1));

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_random_rotation1)
{
    geolib3::Vector3 x(4, 3, -1);
    geolib3::Vector3 t(-2, 10, -5);
    geolib3::Vector3 y = geolib3::crossProduct(t, x);
    geolib3::Vector3 z = geolib3::crossProduct(x, y);

    CameraOrientation or1{ UnitVector3(x), UnitVector3(y), UnitVector3(z)};
    CameraOrientation or2 = fromRodrigues(toRodrigues(or1));

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_random_rotation2)
{
    geolib3::Vector3 x(-26, -1, 5);
    geolib3::Vector3 t(-2, -20, -5);
    geolib3::Vector3 y = geolib3::crossProduct(t, x);
    geolib3::Vector3 z = geolib3::crossProduct(x, y);

    CameraOrientation or1{ UnitVector3(x), UnitVector3(y), UnitVector3(z)};
    CameraOrientation or2 = fromRodrigues(toRodrigues(or1));

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

TEST(rodrigues_angles_tests, test_random_rotation3)
{
    geolib3::Vector3 x(-15, 3, -8);
    geolib3::Vector3 t(-4, -15, -5);
    geolib3::Vector3 y = geolib3::crossProduct(t, x);
    geolib3::Vector3 z = geolib3::crossProduct(x, y);

    CameraOrientation or1{ UnitVector3(x), UnitVector3(y), UnitVector3(z)};
    CameraOrientation or2 = fromRodrigues(toRodrigues(or1));

    EXPECT_VECTORS_ARE_NEAR(or1.rDir, or2.rDir);
    EXPECT_VECTORS_ARE_NEAR(or1.fDir, or2.fDir);
    EXPECT_VECTORS_ARE_NEAR(or1.uDir, or2.uDir);
}

} // namespace maps::mrc::pos_improvment::tests
