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

using maps::geolib3::Vector3;

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

TEST(utils_tests, test_find_most_popular_direction)
{
    std::vector<Vector3> vectors;
    vectors.emplace_back(0, 1, 0);
    vectors.emplace_back(1, 1000, -1); // almost (0, 1, 0)
    vectors.emplace_back(5, 10, 0);
    vectors.emplace_back(0, -10, 5);

    auto result = findMostPopularDirection(vectors, 100);
    Vector3 mostPopVec = result.vector;
    EXPECT_NEAR(mostPopVec.x(), 0.0, 0.001);
    EXPECT_NEAR(mostPopVec.y(), 1.0, 0.001);
    EXPECT_NEAR(mostPopVec.z(), 0.0, 0.001);
}

TEST(utils_tests, test_find_most_popular_direction2)
{
    std::vector<Vector3> vectors;
    vectors.emplace_back(5, 5, 5);
    vectors.emplace_back(5, 5, -5);
    vectors.emplace_back(5, 5.01, -5);

    auto result = findMostPopularDirection(vectors, 300);
    Vector3 mostPopVec = result.vector; // (1, 1, -1)
    EXPECT_NEAR(mostPopVec.x(), std::sqrt(1 / 3.0), 0.001);
    EXPECT_NEAR(mostPopVec.y(), std::sqrt(1 / 3.0), 0.001);
    EXPECT_NEAR(mostPopVec.z(), -std::sqrt(1 / 3.0), 0.001);
}

TEST(utils_tests, test_find_most_popular_direction3)
{
    std::vector<Vector3> vectors;
    vectors.emplace_back(1, 2, 3);
    for (int i = 0; i < 100; i++) {
        vectors.emplace_back(0, -10, 10); //(0, -1, 1)
    }

    auto result = findMostPopularDirection(vectors, 50);
    Vector3 mostPopVec = result.vector; // (0, -1, 1)
    EXPECT_NEAR(mostPopVec.x(), 0, 0.001);
    EXPECT_NEAR(mostPopVec.y(), -std::sqrt(0.5), 0.001);
    EXPECT_NEAR(mostPopVec.z(), std::sqrt(0.5), 0.001);
}

TEST(utils_tests, test_filter_events_zero_filter_time)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    events.emplace_back(2.0_ts, RotationSpeedVector{4, 5, 6});
    events.emplace_back(3.0_ts, RotationSpeedVector{7, 8, 9});

    GyroscopeEvents filteredEvents;
    filteredEvents.emplace_back(1.5_ts, RotationSpeedVector{4, 5, 6});
    filteredEvents.emplace_back(2.5_ts, RotationSpeedVector{7, 8, 9});
    EXPECT_THAT(getFilteredValues(events, Seconds(0.001)),
                testing::Eq(filteredEvents));
}

TEST(utils_tests, test_filter_events_join_2_neighboring_events)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    events.emplace_back(2.0_ts, RotationSpeedVector{4, 5, 6});
    events.emplace_back(3.0_ts, RotationSpeedVector{7, 8, 9});
    events.emplace_back(4.0_ts, RotationSpeedVector{10, 11, 12});

    GyroscopeEvents filteredEvents;
    filteredEvents.emplace_back(2.0_ts, RotationSpeedVector{(4 + 7) / 2.0,
                                                            (5 + 8) / 2.0,
                                                            (6 + 9) / 2.0});
    filteredEvents.emplace_back(3.0_ts, RotationSpeedVector{(7 + 10) / 2.0,
                                                            (8 + 11) / 2.0,
                                                            (9 + 12) / 2.0});
    EXPECT_THAT(getFilteredValues(events, Seconds(1.5)),
                testing::Eq(filteredEvents));
}

TEST(utils_tests, test_filter_events_join_3_neighboring_events)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    events.emplace_back(2.0_ts, RotationSpeedVector{4, 5, 6});
    events.emplace_back(3.0_ts, RotationSpeedVector{7, 8, 9});
    events.emplace_back(4.0_ts, RotationSpeedVector{10, 11, 12});

    GyroscopeEvents filteredEvents;
    filteredEvents.emplace_back(2.5_ts, RotationSpeedVector{(4 + 7 + 10) / 3.0,
                                                            (5 + 8 + 11) / 3.0,
                                                            (6 + 9 + 12) / 3.0});
    EXPECT_THAT(getFilteredValues(events, Seconds(2.5)),
                testing::Eq(filteredEvents));
}

TEST(utils_tests, test_filter_events_different_time_intervals)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    events.emplace_back(2.0_ts, RotationSpeedVector{4, 5, 6});
    events.emplace_back(4.0_ts, RotationSpeedVector{7, 8, 9});

    GyroscopeEvents filteredEvents;
    filteredEvents.emplace_back(2.5_ts, RotationSpeedVector{(4 + 7 * 2) / 3.0,
                                                            (5 + 8 * 2) / 3.0,
                                                            (6 + 9 * 2) / 3.0});
    EXPECT_THAT(getFilteredValues(events, Seconds(1.5)),
                testing::Eq(filteredEvents));
}

TEST(utils_tests, test_filter_empty_input_data)
{
    GyroscopeEvents events;
    GyroscopeEvents filteredEvents;
    EXPECT_THAT(getFilteredValues(events, Seconds(1.5)),
                testing::Eq(filteredEvents));
}

TEST(utils_tests, test_filter_too_few_input_events)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    GyroscopeEvents filteredEvents;
    EXPECT_THAT(getFilteredValues(events, Seconds(1.5)),
                testing::Eq(filteredEvents));
}


TEST(utils_tests, test_filter_events_too_big_filter_time)
{
    GyroscopeEvents events;
    events.emplace_back(1.0_ts, RotationSpeedVector{1, 2, 3});
    events.emplace_back(2.0_ts, RotationSpeedVector{4, 5, 6});
    events.emplace_back(3.0_ts, RotationSpeedVector{7, 8, 9});

    GyroscopeEvents filteredEvents;
    EXPECT_THAT(getFilteredValues(events, Seconds(2.5)),
                testing::Eq(filteredEvents));
}

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