#include <library/cpp/testing/gtest/gtest.h>
#include <library/cpp/testing/common/env.h>
#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/separate_rides.h>

namespace maps::mrc::sensors_feature_positioner::tests {

using namespace maps::mrc::pos_improvment;

using maps::geolib3::Vector2;
using maps::geolib3::Point2;

namespace {

constexpr Time operator"" _ts (long double value)
{
    return Time(Seconds(static_cast<double>(value)));
}

db::TrackPoint createTP(Time time,
                        geolib3::Point2 mercatorPos)
{
    db::TrackPoint trackPoint;
    trackPoint.setTimestamp(
        std::chrono::time_point_cast<chrono::TimePoint::duration>(time));
    trackPoint.setMercatorPos(mercatorPos);
    return trackPoint;
}

db::TrackPoint createTP(Time time,
                        geolib3::Point2 mercatorPos,
                        const std::string sourceId)
{
    db::TrackPoint trackPoint = createTP(time, mercatorPos);
    trackPoint.setSourceId(sourceId);
    return trackPoint;
}

} // anonymous namespace

    // at this point 2 mercator meters = 1 real meter
    const Point2 P0 = geolib3::geoPoint2Mercator(Point2{40, 60});

TEST(separate_rides_tests, test_split_into_separate_rides)
{
    db::TrackPoints trackPoints;
    trackPoints.push_back(createTP(1.0_ts, P0 + Vector2{1, 11}));

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0.1, 0, 0});

    AccelerometerEvents accEvents;
    accEvents.emplace_back(1.0_ts, AccelerationVector{0, 0.1, 0});

    auto tracks = splitIntoSeparateRides(trackPoints, {gyroEvents, accEvents});
    EXPECT_EQ(tracks.size(), 1u);

    EXPECT_EQ(tracks[0].trackPoints.size(), trackPoints.size());
    EXPECT_EQ(tracks[0].trackPoints[0].mercatorPos(), trackPoints[0].mercatorPos());
    EXPECT_EQ(tracks[0].trackPoints[0].timestamp(), trackPoints[0].timestamp());

    EXPECT_EQ(tracks[0].sensorEvents.gyroEvents, gyroEvents);
    EXPECT_EQ(tracks[0].sensorEvents.accEvents, accEvents);
}

TEST(separate_rides_tests, test_split_into_separate_rides2)
{
    db::TrackPoints trackPoints;
    trackPoints.push_back(createTP(1.0_ts, P0 + Vector2{1, 11}));
    trackPoints.push_back(createTP(2.0_ts, P0 + Vector2{2, 12}));
    trackPoints.push_back(createTP(3.0_ts, P0 + Vector2{3, 13}));
    trackPoints.push_back(createTP(4.0_ts, P0 + Vector2{4, 14}));
    trackPoints.push_back(createTP(5.0_ts, P0 + Vector2{5, 15}));
    trackPoints.push_back(createTP(6.0_ts, P0 + Vector2{6, 16}));
    trackPoints.push_back(createTP(7.01_ts, P0 + Vector2{7, 17}));
    // gap (7.0_ts, 21_ts)
    trackPoints.push_back(createTP(20.99_ts, P0 + Vector2{21, 121}));
    trackPoints.push_back(createTP(22.0_ts, P0 + Vector2{22, 122}));

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0.1, 0, 0});
    // gap (1.0_ts, 3.0_ts)
    gyroEvents.emplace_back(3.0_ts, RotationSpeedVector{0.3, 0, 0});
    for (double i = 3.5; i < 30; i += 0.5) {
        gyroEvents.emplace_back(Time(Seconds(i)), RotationSpeedVector{i / 10, 0, 0});
    }

    AccelerometerEvents accEvents;
    accEvents.emplace_back(1.0_ts, AccelerationVector{0, 0.1, 0});
    accEvents.emplace_back(1.5_ts, AccelerationVector{0, 0.15, 0});
    accEvents.emplace_back(2.0_ts, AccelerationVector{0, 0.1, 0});
    accEvents.emplace_back(2.5_ts, AccelerationVector{0, 0.25, 0});
    accEvents.emplace_back(3.0_ts, AccelerationVector{0, 0.3, 0});
    accEvents.emplace_back(3.5_ts, AccelerationVector{0, 0.35, 0});
    accEvents.emplace_back(4.0_ts, AccelerationVector{0, 0.4, 0});
    accEvents.emplace_back(4.5_ts, AccelerationVector{0, 0.45, 0});
    accEvents.emplace_back(5.0_ts, AccelerationVector{0, 0.5, 0});
    // gap (5.0_ts, 8.0_ts)
    accEvents.emplace_back(8.0_ts, AccelerationVector{0, 0.5, 0});
    for (double i = 8.5; i < 30; i += 0.5) {
        accEvents.emplace_back(Time(Seconds(i)), AccelerationVector{0, i / 10, 0});
    }

    auto tracks = splitIntoSeparateRides(trackPoints, {gyroEvents, accEvents});
    EXPECT_EQ(tracks.size(), 3u);

    EXPECT_EQ(tracks[0].trackPoints.size(), 1u);
    EXPECT_EQ(tracks[0].trackPoints[0].mercatorPos(), trackPoints[0].mercatorPos());
    EXPECT_EQ(tracks[0].trackPoints[0].timestamp(), trackPoints[0].timestamp());

    EXPECT_THAT(tracks[0].sensorEvents.gyroEvents, testing::ElementsAre(gyroEvents[0]));
    EXPECT_THAT(tracks[0].sensorEvents.accEvents, testing::ElementsAre(accEvents[0]));


    EXPECT_EQ(tracks[1].trackPoints.size(), 3u);
    EXPECT_EQ(tracks[1].trackPoints[0].mercatorPos(), trackPoints[2].mercatorPos());
    EXPECT_EQ(tracks[1].trackPoints[0].timestamp(), trackPoints[2].timestamp());
    EXPECT_EQ(tracks[1].trackPoints[1].mercatorPos(), trackPoints[3].mercatorPos());
    EXPECT_EQ(tracks[1].trackPoints[1].timestamp(), trackPoints[3].timestamp());
    EXPECT_EQ(tracks[1].trackPoints[2].mercatorPos(), trackPoints[4].mercatorPos());
    EXPECT_EQ(tracks[1].trackPoints[2].timestamp(), trackPoints[4].timestamp());

    EXPECT_THAT(tracks[1].sensorEvents.gyroEvents,
                testing::ElementsAre(gyroEvents[1], gyroEvents[2],
                                     gyroEvents[3], gyroEvents[4], gyroEvents[5]));
    EXPECT_THAT(tracks[1].sensorEvents.accEvents,
                testing::ElementsAre(accEvents[4], accEvents[5], accEvents[6],
                                     accEvents[7], accEvents[8]));


    EXPECT_EQ(tracks[2].trackPoints.size(), 2u);
    EXPECT_EQ(tracks[2].trackPoints[0].mercatorPos(), trackPoints[7].mercatorPos());
    EXPECT_EQ(tracks[2].trackPoints[0].timestamp(), trackPoints[7].timestamp());
    EXPECT_EQ(tracks[2].trackPoints[1].mercatorPos(), trackPoints[8].mercatorPos());
    EXPECT_EQ(tracks[2].trackPoints[1].timestamp(), trackPoints[8].timestamp());

    EXPECT_THAT(tracks[2].sensorEvents.gyroEvents,
                testing::ElementsAre(gyroEvents[37], gyroEvents[38], gyroEvents[39]));
    EXPECT_THAT(tracks[2].sensorEvents.accEvents,
                testing::ElementsAre(accEvents[35], accEvents[36], accEvents[37]));
}

TEST(separate_rides_tests, test_separate_by_source_id)
{
    db::TrackPoints trackPoints;
    trackPoints.push_back(createTP(1.0_ts, P0 + Vector2{1, 11}, "source1"));
    trackPoints.push_back(createTP(2.0_ts, P0 + Vector2{2, 12}, "source2"));
    trackPoints.push_back(createTP(3.0_ts, P0 + Vector2{3, 13}, "source1"));

    auto tracks = separateBySourceId(trackPoints);

    EXPECT_EQ(tracks.size(), 2u);
    db::TrackPoints trackPoints1;
    db::TrackPoints trackPoints2;
    if (tracks[0].size() == 2) {
        trackPoints1 = tracks[0];
        trackPoints2 = tracks[1];
    } else {
        trackPoints1 = tracks[1];
        trackPoints2 = tracks[0];
    }

    EXPECT_EQ(trackPoints1.size(), 2u);
    EXPECT_EQ(trackPoints1[0].mercatorPos(), trackPoints[0].mercatorPos());
    EXPECT_EQ(trackPoints1[0].timestamp(), trackPoints[0].timestamp());
    EXPECT_EQ(trackPoints1[0].sourceId(), trackPoints[0].sourceId());

    EXPECT_EQ(trackPoints1[1].mercatorPos(), trackPoints[2].mercatorPos());
    EXPECT_EQ(trackPoints1[1].timestamp(), trackPoints[2].timestamp());
    EXPECT_EQ(trackPoints1[1].sourceId(), trackPoints[2].sourceId());

    EXPECT_EQ(trackPoints2.size(), 1u);
    EXPECT_EQ(trackPoints2[0].mercatorPos(), trackPoints[1].mercatorPos());
    EXPECT_EQ(trackPoints2[0].timestamp(), trackPoints[1].timestamp());
    EXPECT_EQ(trackPoints2[0].sourceId(), trackPoints[1].sourceId());
}

} // namespace maps::mrc::sensors_feature_positioner::tests
