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

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

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

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

TEST(graph_matching_tests, test_simple_match)
{
    // gps direction and speed should be ignored, but saved in new events
    GpsEvents gpsEvents {
        {1.0_ts, P0 + Vector2{10, 0}, 1.0_m, 5.0_mps, PI},
        {2.0_ts, P0 + Vector2{10, 40}, 1.0_m, 5.0_mps, PI},
        {3.0_ts, P0 + Vector2{10, 80}, 1.0_m, 5.0_mps, PI}
    };
    GpsSegments matchedTrack {
        {{1.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI / 2},
         {3.0_ts, P0 + Vector2{0, 80}, 1.0_m, 5.0_mps, PI / 2}}
    };

    // matching algorithm allows +-1 meter shift from matchedTrack,
    // so newTrack.x = 0 + 1meter = 2

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2},
        {EventType::CarGroundDirection, 3.5_ts, PI / 2}
    };

    GpsEvents newGpsEvents
        = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

    EXPECT_EQ(newGpsEvents.size(), 3u);

    EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
    EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y(), 0.001);

    EXPECT_EQ(newGpsEvents[1].time, 2.0_ts);
    EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 40, 0.001);

    EXPECT_EQ(newGpsEvents[2].time, 3.0_ts);
    EXPECT_NEAR(newGpsEvents[2].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[2].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.y(), P0.y() + 80, 0.001);
}

TEST(graph_matching_tests, test_simple_match_time_shift)
{
    // gps direction and speed should be ignored, but saved in new events
    GpsEvents gpsEvents {
        {1.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI},
        {2.0_ts, P0 + Vector2{0, 40}, 1.0_m, 5.0_mps, PI},
        {3.0_ts, P0 + Vector2{0, 80}, 1.0_m, 5.0_mps, PI}
    };
    // matchedTrack time is slightly different
    GpsSegments matchedTrack {
        {{0.0_ts, P0 + Vector2{10, -10}, 1.0_m, 5.0_mps, PI / 2},
         {0.5_ts, P0 + Vector2{10, 100}, 1.0_m, 5.0_mps, PI / 2}}
    };

    // matching algorithm allows +-1 meter shift from matchedTrack,
    // so newTrack.x = 10 - 1m = 8

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2},
        {EventType::CarGroundDirection, 3.5_ts, PI / 2}
    };

    GpsEvents newGpsEvents
        = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

    EXPECT_EQ(newGpsEvents.size(), 3u);

    EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
    EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x() + 8, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y(), 0.001);

    EXPECT_EQ(newGpsEvents[1].time, 2.0_ts);
    EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x() + 8, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 40, 0.001);

    EXPECT_EQ(newGpsEvents[2].time, 3.0_ts);
    EXPECT_NEAR(newGpsEvents[2].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[2].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.x(), P0.x() + 8, 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.y(), P0.y() + 80, 0.001);
}

// TEST(graph_matching_tests, test_match_ignore_unparallel_directed_points)
// {
//     // gps direction and speed should be ignored, but saved in new events
//     GpsEvents gpsEvents {
//         {1.0_ts, P0 + Vector2{0, 10}, 1.0_m, 5.0_mps, PI},
//         {2.0_ts, P0 + Vector2{40, 10}, 1.0_m, 5.0_mps, PI},
//         {3.0_ts, P0 + Vector2{80, 10}, 1.0_m, 5.0_mps, PI}
//     };
//     GpsSegments matchedTrack {
//         {{0.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI / 2},
//          {0.5_ts, P0 + Vector2{80, 0}, 1.0_m, 5.0_mps, PI / 2}}
//     };

//     // matching algorithm allows +-1 meter shift from matchedTrack,
//     // so newTrack.y = 0 + 1meter = 2

//     // direction at 0.2_ts is not parallel to the matched track, so
//     // thist point should be ignored
//     CarGroundDirectionEvents directionEvents {
//         {EventType::CarGroundDirection, 0.5_ts, PI * 0},
//         {EventType::CarGroundDirection, 1.98_ts, PI * 0},
//         {EventType::CarGroundDirection, 1.99_ts, PI * 0.2},
//         {EventType::CarGroundDirection, 2.01_ts, PI * 0.2},
//         {EventType::CarGroundDirection, 2.02_ts, PI * 0},
//         {EventType::CarGroundDirection, 3.5_ts, PI * 0}
//     };

//     GpsEvents newGpsEvents
//         = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

//     EXPECT_EQ(newGpsEvents.size(), 2u);

//     EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
//     EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
//     EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI, 0.001);
//     EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x(), 0.001);
//     EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y() + 2, 0.001);

//     EXPECT_EQ(newGpsEvents[1].time, 3.0_ts);
//     EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
//     EXPECT_NEAR(newGpsEvents[1].direction->value(), M_PI, 0.001);
//     EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x() + 80, 0.001);
//     EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 2, 0.001);
// }

TEST(graph_matching_tests, test_simple_match_to_segment_not_to_line)
{
    // gps direction and speed should be ignored, but saved in new events
    GpsEvents gpsEvents {
        {1.0_ts, P0 + Vector2{10, 0}, 1.0_m, 5.0_mps, PI},
        {2.0_ts, P0 + Vector2{10, 40}, 1.0_m, 5.0_mps, PI},
        {3.0_ts, P0 + Vector2{10, 80}, 1.0_m, 5.0_mps, PI}
    };
    // matched track is shorter
    GpsSegments matchedTrack {
        {{0.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI / 2},
            {0.5_ts, P0 + Vector2{0, 70}, 1.0_m, 5.0_mps, PI / 2}}
    };

    // matching algorithm allows +-1 meter shift from matchedTrack,
    // so newTrack.x = 0 + 1meter = 2

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2},
        {EventType::CarGroundDirection, 3.5_ts, PI / 2}
    };

    GpsEvents newGpsEvents
        = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

    EXPECT_EQ(newGpsEvents.size(), 3u);

    EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
    EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y(), 0.001);

    EXPECT_EQ(newGpsEvents[1].time, 2.0_ts);
    EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 40, 0.001);

    EXPECT_EQ(newGpsEvents[2].time, 3.0_ts);
    EXPECT_NEAR(newGpsEvents[2].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[2].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.x(), P0.x() + std::sqrt(2), 0.001);
    EXPECT_NEAR(newGpsEvents[2].mercatorPos.y(), P0.y() + 70 + std::sqrt(2), 0.001);
}

TEST(graph_matching_tests, test_simple_match_to_several_segments)
{
    // gps direction and speed should be ignored, but saved in new events
    GpsEvents gpsEvents {
        {1.0_ts, P0 + Vector2{10, 0}, 1.0_m, 5.0_mps, PI / 2},
        {2.0_ts, P0 + Vector2{10, 40}, 1.0_m, 5.0_mps, PI / 2},
    };
    GpsSegments matchedTrack {
        {{1.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI / 2},
         {2.0_ts, P0 + Vector2{0, 10}, 1.0_m, 5.0_mps, PI / 2}},
        {{2.0_ts, P0 + Vector2{20, 40}, 1.0_m, 5.0_mps, PI / 2},
         {3.0_ts, P0 + Vector2{20, 50}, 1.0_m, 5.0_mps, PI / 2}}
    };

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2},
        {EventType::CarGroundDirection, 3.5_ts, PI / 2}
    };

    GpsEvents newGpsEvents
        = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

    EXPECT_EQ(newGpsEvents.size(), 2u);

    EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
    EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI / 2, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x() + 2, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y(), 0.001);

    EXPECT_EQ(newGpsEvents[1].time, 2.0_ts);
    EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].direction->value(), M_PI / 2, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x() + 18, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 40, 0.001);
}

TEST(graph_matching_tests, test_simple_match_to_several_segments_with_different_direction)
{
    // gps direction and speed should be ignored, but saved in new events
    GpsEvents gpsEvents {
        {1.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI / 2},
        {2.0_ts, P0 + Vector2{0, 0}, 1.0_m, 5.0_mps, PI * 0},
    };

    // algorithm should select segment by direction
    GpsSegments matchedTrack {
        {{1.0_ts, P0 + Vector2{-10, 0}, 1.0_m, 5.0_mps, PI / 2},
         {2.0_ts, P0 + Vector2{-10, 10}, 1.0_m, 5.0_mps, PI / 2}},
        {{2.0_ts, P0 + Vector2{0, 15}, 1.0_m, 5.0_mps, PI * 0},
         {3.0_ts, P0 + Vector2{10, 15}, 1.0_m, 5.0_mps, PI * 0}}
    };

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2},
        {EventType::CarGroundDirection, 1.1_ts, PI / 2},
        {EventType::CarGroundDirection, 1.9_ts, PI * 0},
        {EventType::CarGroundDirection, 3.5_ts, PI * 0}
    };

    GpsEvents newGpsEvents
        = getProjectionOnMatchedTrack(gpsEvents, matchedTrack, directionEvents);

    EXPECT_EQ(newGpsEvents.size(), 2u);

    EXPECT_EQ(newGpsEvents[0].time, 1.0_ts);
    EXPECT_NEAR(newGpsEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[0].direction->value(), M_PI / 2, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.x(), P0.x() -8, 0.001);
    EXPECT_NEAR(newGpsEvents[0].mercatorPos.y(), P0.y(), 0.001);

    EXPECT_EQ(newGpsEvents[1].time, 2.0_ts);
    EXPECT_NEAR(newGpsEvents[1].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].direction->value(), 0, 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.x(), P0.x(), 0.001);
    EXPECT_NEAR(newGpsEvents[1].mercatorPos.y(), P0.y() + 13, 0.001);
}

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