#include "test_utils.h"

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

using maps::geolib3::Vector3;

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

constexpr geolib3::Point2 P0{0,0};

TEST(gravity_tests, test_gravity_constant_speed)
{
    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{0, 9.8, 0}},
        {2.5_ts, AccelerationVector{0, 9.8, 0}}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 0, 0}},
        {2.5_ts, RotationSpeedVector{0, 0, 0}}
    };

    GpsEvents gpsEvents {
        {1.0_ts, P0, 0.0_m, 5.0_mps, PI},
        {2.0_ts, P0, 0.0_m, 5.0_mps, PI},
        {3.0_ts, P0, 0.0_m, 5.0_mps, PI}
    };

    UnitVector3 carUpVector({0, 1, 0});
    UnitVector3 carFrontVector({1, 0, 0});

    auto checkResult = [&]() {
        auto [gravityEvents, horizontalFrontVecEvents]
            = findGravityAndHorizontalFrontVector(
                accEvents, gyroEvents, gpsEvents, carUpVector, carFrontVector);

        EXPECT_EQ(gravityEvents.size(), 2u);
        EXPECT_EQ(horizontalFrontVecEvents.size(), 2u);

        EXPECT_NEAR(gravityEvents[0].values().x(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().y(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(gravityEvents[1].values().x(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().y(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[0].values().x(), 1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().y(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[1].values().x(), 1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().y(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().z(), 0.0, 0.001);
    };

    checkResult();

    // car up and front vectors are used almost only to define car up-front plane

    carUpVector = UnitVector3({1, 2, 0});
    carFrontVector = UnitVector3({2, -1, 0});
    checkResult();

    carUpVector = UnitVector3({-1, 2, 0});
    carFrontVector = UnitVector3({2, 1, 0});
    checkResult();
}

TEST(gravity_tests, test_gravity_speed_is_growing)
{
    // both gps speed and speed by sensors are changing by 1 m/s^2

    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{1, 9.8, 0}},
        {2.5_ts, AccelerationVector{1, 9.8, 0}}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 0, 0}},
        {2.5_ts, RotationSpeedVector{0, 0, 0}}
    };

    GpsEvents gpsEvents {
        {1.0_ts, P0, 0.0_m, 5.0_mps, PI},
        {2.0_ts, P0, 0.0_m, 6.0_mps, PI},
        {3.0_ts, P0, 0.0_m, 7.0_mps, PI}
    };

    UnitVector3 carUpVector({0, 1, 0});
    UnitVector3 carFrontVector({1, 0, 0});

    auto checkResult = [&]() {
        auto [gravityEvents, horizontalFrontVecEvents]
            = findGravityAndHorizontalFrontVector(
                accEvents, gyroEvents, gpsEvents, carUpVector, carFrontVector);

        EXPECT_EQ(gravityEvents.size(), 2u);
        EXPECT_EQ(horizontalFrontVecEvents.size(), 2u);

        EXPECT_NEAR(gravityEvents[0].values().x(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().y(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(gravityEvents[1].values().x(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().y(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[0].values().x(), 1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().y(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[1].values().x(), 1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().y(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().z(), 0.0, 0.001);
    };

    checkResult();

    // car up and front vectors are used almost only to define car up-front plane

    carUpVector = UnitVector3({1, 2, 0});
    carFrontVector = UnitVector3({2, -1, 0});
    checkResult();

    carUpVector = UnitVector3({-1, 2, 0});
    carFrontVector = UnitVector3({2, 1, 0});
    checkResult();
}


TEST(gravity_tests, test_gravity_car_is_rotating)
{
    // car rotates 90 degrees (rises front-up)

    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{0, 9.8, 0}},
        {2.5_ts, AccelerationVector{9.8, 0, 0}}
    };

    GyroscopeEvents gyroEvents {
        {2.0_ts, RotationSpeedVector{0, 0, 0}},
        {2.49999_ts, RotationSpeedVector{0, 0, M_PI}} // PI * 0.5sec =
                                                      // PI/2 rotation
    };

    GpsEvents gpsEvents {
        {1.0_ts, P0, 0.0_m, 5.0_mps, PI},
        {2.0_ts, P0, 0.0_m, 5.0_mps, PI},
        {3.0_ts, P0, 0.0_m, 5.0_mps, PI}
    };

    UnitVector3 carUpVector({0, 1, 0});
    UnitVector3 carFrontVector({1, 0, 0});

    auto checkResult = [&]() {
        auto [gravityEvents, horizontalFrontVecEvents]
            = findGravityAndHorizontalFrontVector(
                accEvents, gyroEvents, gpsEvents, carUpVector, carFrontVector);

        EXPECT_EQ(gravityEvents.size(), 2u);
        EXPECT_EQ(horizontalFrontVecEvents.size(), 2u);

        EXPECT_NEAR(gravityEvents[0].values().x(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().y(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(gravityEvents[1].values().x(), -1.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().y(), 0.0, 0.001);
        EXPECT_NEAR(gravityEvents[1].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[0].values().x(), 1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().y(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[0].values().z(), 0.0, 0.001);

        EXPECT_NEAR(horizontalFrontVecEvents[1].values().x(), 0.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().y(), -1.0, 0.001);
        EXPECT_NEAR(horizontalFrontVecEvents[1].values().z(), 0.0, 0.001);
    };

    checkResult();

    // car up and front vectors are used almost only to define car up-front plane

    carUpVector = UnitVector3({1, 2, 0});
    carFrontVector = UnitVector3({2, -1, 0});
    checkResult();

    carUpVector = UnitVector3({-1, 2, 0});
    carFrontVector = UnitVector3({2, 1, 0});
    checkResult();
}

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