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

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

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

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

TEST(gps_tests, test_smooth_azimuth)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.1);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{M_PI * 0.1, 0, 0});
    gyroEvents.emplace_back(1.9999_ts, RotationSpeedVector{M_PI * 0.1, 0, 0});
    gyroEvents.emplace_back(2.0001_ts, RotationSpeedVector{M_PI * 0.2, 0, 0});
    gyroEvents.emplace_back(2.9999_ts, RotationSpeedVector{M_PI * 0.2, 0, 0});

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 1.8, 0.00001);
    EXPECT_NEAR(smoothEvents[1].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_NEAR(smoothEvents[2].direction->value(), M_PI * 2.1, 0.00001);

    // check that the rest of the values were not changed:

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        // return all the changed values back, so we can compare events
        smoothEvents[i].direction = gpsEvents[i].direction;
    }
    EXPECT_THAT(smoothEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test_smooth_azimuth2)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.2);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.1);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(1.9999_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(2.0001_ts, RotationSpeedVector{M_PI * 0.1, 0, 0});
    gyroEvents.emplace_back(2.9999_ts, RotationSpeedVector{M_PI * 0.1, 0, 0});

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_NEAR(smoothEvents[1].direction->value(), M_PI * 2.2, 0.00001);
    EXPECT_NEAR(smoothEvents[2].direction->value(), M_PI * 2.1, 0.00001);
}

TEST(gps_tests, test_smooth_azimuth3)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.1);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{M_PI * -0.2, 0, 0});
    gyroEvents.emplace_back(1.9999_ts, RotationSpeedVector{M_PI * -0.2, 0, 0});

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 0.1, 0.00001);
    EXPECT_NEAR(smoothEvents[1].direction->value(), M_PI * -0.1, 0.00001);
}

TEST(gps_tests, test_smooth_azimuth4)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.2);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(1.9999_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(2.0001_ts, RotationSpeedVector{M_PI * -0.4, 0, 0});
    gyroEvents.emplace_back(2.9999_ts, RotationSpeedVector{M_PI * -0.4, 0, 0});

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_NEAR(smoothEvents[1].direction->value(), M_PI * 2.2, 0.00001);
    EXPECT_NEAR(smoothEvents[2].direction->value(), M_PI * 1.8, 0.00001);
}

TEST(gps_tests, test_smooth_azimuth_several_circles)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.2);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.6);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.0);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.4);
    gpsEvents.emplace_back(6.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.7);
    gpsEvents.emplace_back(7.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.1);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(1.9999_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(2.0001_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(2.9999_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(3.0001_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(3.9999_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(4.0001_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(4.9999_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(5.0001_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(5.9999_ts, RotationSpeedVector{M_PI * 0.3, 0, 0});
    gyroEvents.emplace_back(6.0001_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});
    gyroEvents.emplace_back(6.9999_ts, RotationSpeedVector{M_PI * 0.4, 0, 0});

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 1.8, 0.00001);
    EXPECT_NEAR(smoothEvents[1].direction->value(), M_PI * 2.2, 0.00001);
    EXPECT_NEAR(smoothEvents[2].direction->value(), M_PI * 2.6, 0.00001);
    EXPECT_NEAR(smoothEvents[3].direction->value(), M_PI * 3.0, 0.00001);
    EXPECT_NEAR(smoothEvents[4].direction->value(), M_PI * 3.4, 0.00001);
    EXPECT_NEAR(smoothEvents[5].direction->value(), M_PI * 3.7, 0.00001);
    EXPECT_NEAR(smoothEvents[6].direction->value(), M_PI * 4.1, 0.00001);
}

TEST(gps_tests, test_smooth_azimuth_points_wo_azimuth)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, std::nullopt);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 10.0_mps, std::nullopt);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 10.0_mps, PI * 0.1);

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

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents smoothEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(smoothEvents[0].direction->value(), M_PI * 1.8, 0.00001);
    EXPECT_TRUE(smoothEvents[1].direction == std::nullopt);
    EXPECT_NEAR(smoothEvents[2].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_TRUE(smoothEvents[3].direction == std::nullopt);
    EXPECT_NEAR(smoothEvents[4].direction->value(), M_PI * 2.1, 0.00001);
}


TEST(gps_tests, test_no_reverse_moving)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 2.1);

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test_reverse_moving)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 11.0_mps, PI * 0.9);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 12.0_mps, PI * 1.0);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 13.0_mps, PI * 1.9);
    for (size_t i = 0; i < 100; i++) {
        gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 14.0_mps, PI * 2.1);
    }

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(modifiedEvents[1].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(), -11.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].direction->value(), M_PI * 2.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(), -12.0, 0.00001);

    // check that the rest of the values were not changed:

    for (size_t i = 1; i <= 2; i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].direction = gpsEvents[i].direction;
        modifiedEvents[i].speed = gpsEvents[i].speed;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test_reverse_moving2)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 11.0_mps, PI * 2.9);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 12.0_mps, PI * 3.0);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 13.0_mps, PI * 1.9);
    for (size_t i = 0; i < 100; i++) {
        gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 14.0_mps, PI * 2.1);
    }

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(modifiedEvents[1].direction->value(), M_PI * 1.9, 0.00001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(), -11.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].direction->value(), M_PI * 2.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(), -12.0, 0.00001);

    // check that the rest of the values were not changed:

    for (size_t i = 1; i <= 2; i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].direction = gpsEvents[i].direction;
        modifiedEvents[i].speed = gpsEvents[i].speed;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test_reverse_moving3)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 11.0_mps, PI * 2);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 12.0_mps, PI * 1.75);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 13.0_mps, PI * 1.5);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 14.0_mps, PI * 2.5);
    for (size_t i = 0; i < 100; i++) {
        gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 15.0_mps, PI * 2.5);
    }

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(modifiedEvents[0].direction->value(), M_PI * 1, 0.00001);
    EXPECT_NEAR(modifiedEvents[0].speed->value(), 10.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[1].direction->value(), M_PI * 1, 0.00001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(), -11.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].direction->value(), M_PI * 0.75, 0.00001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(), -12.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[3].direction->value(), M_PI * 0.5, 0.00001);
    EXPECT_NEAR(modifiedEvents[3].speed->value(), -13.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[4].direction->value(), M_PI * 0.5, 0.00001);
    EXPECT_NEAR(modifiedEvents[4].speed->value(), 14.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[5].direction->value(), M_PI * 0.5, 0.00001);
    EXPECT_NEAR(modifiedEvents[5].speed->value(), 15.0, 0.00001);
}

TEST(gps_tests, test_reverse_moving_first_point_is_reverse)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.9);
    for (size_t i = 0; i < 100; i++) {
        gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 11.0_mps, PI * 1.0);
    }

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(std::fmod(modifiedEvents[0].direction->value(), M_PI * 2.0), M_PI * 0.9, 0.00001);
    EXPECT_NEAR(modifiedEvents[0].speed->value(), -10.0, 0.00001);
    for (size_t i = 1; i < modifiedEvents.size(); i++) {
        EXPECT_NEAR(std::fmod(modifiedEvents[i].direction->value(), M_PI * 2.0), M_PI * 1.0, 0.00001);
        EXPECT_NEAR(modifiedEvents[i].speed->value(), 11.0, 0.00001);
    }
}

TEST(gps_tests, test_reverse_moving_points_without_speed)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 0.0_mps, PI * 0.9);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 10.0_mps, PI * 1.8);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 12.0_mps, PI * 1.0);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 13.0_mps, PI * 1.9);
    for (size_t i = 0; i < 100; i++) {
        gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 14.0_mps, PI * 2.1);
    }

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    // algorithm should ignore small difference between gyroscope values and gps azimuth

    UnitVector3 carUpVector(geolib3::Vector3(1,0,0));

    GpsEvents modifiedEvents = smoothAzimuth(gpsEvents, gyroEvents, carUpVector);
    EXPECT_NEAR(modifiedEvents[3].direction->value(), M_PI * 2.0, 0.00001);
    EXPECT_NEAR(modifiedEvents[3].speed->value(), -12.0, 0.00001);

    // check that the rest of the values were not changed:

    // return all the changed values back, so we can compare events
    modifiedEvents[3].direction = gpsEvents[3].direction;
    modifiedEvents[3].speed = gpsEvents[3].speed;

    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test2PointsGpsSpeed)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0 + Vector2{0, 0}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0 + Vector2{0, 20}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0 + Vector2{0, 60}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0 + Vector2{0, 60}, 0.0_m, 5.0_mps, PI);

    GpsEvents modifiedEvents = use2PointsGpsSpeed(gpsEvents);
    // the first and the last points shouldn't be changed
    EXPECT_NEAR(modifiedEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(), 15.0, 0.001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(), 10.0, 0.001);
    EXPECT_NEAR(modifiedEvents[3].speed->value(), 5.0, 0.001);

    // check that the rest of the values were not changed:

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].speed = gpsEvents[i].speed;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test2PointsGpsSpeedCorrectedByAzimuth)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0 + Vector2{0, 0}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0 + Vector2{40, 20}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0 + Vector2{90, 60}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0 + Vector2{120, 60}, 0.0_m, 5.0_mps, PI);

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.5_ts, PI / 2}, // to the north
        {EventType::CarGroundDirection, 5.0_ts, PI / 2}
    };
    // x speed should be ignored

    GpsEvents modifiedEvents = use2PointsGpsSpeed(gpsEvents, directionEvents);
    // the first and the last points shouldn't be changed
    EXPECT_NEAR(modifiedEvents[0].speed->value(), 5.0, 0.001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(), 15.0, 0.001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(), 10.0, 0.001);
    EXPECT_NEAR(modifiedEvents[3].speed->value(), 5.0, 0.001);

    // check that the rest of the values were not changed:

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].speed = gpsEvents[i].speed;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

TEST(gps_tests, test2PointsGpsSpeedWhenTurning)
{
    // a car moves to the north, then it makes a 90 degrees turn and
    // moves to the east. The algorighm should handle the turn as a 90
    // degrees arc of radius 20 mercator meters
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0 + Vector2{0, 0}, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0 + Vector2{0, 20}, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0 + Vector2{20, 40}, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0 + Vector2{40, 40}, 0.0_m, 15.0_mps, PI);

    CarGroundDirectionEvents directionEvents {
        {EventType::CarGroundDirection, 0.99999_ts, PI / 2}, // to the north
        {EventType::CarGroundDirection, 1.99999_ts, PI / 2},
        {EventType::CarGroundDirection, 2.00001_ts, PI / 2},
        {EventType::CarGroundDirection, 2.99999_ts, PI * 0}, // to the east
        {EventType::CarGroundDirection, 3.99999_ts, PI * 0}
    };

    Meters radius(10); // = 20 mercator meters
    Meters arcLength = (2 * M_PI * radius) / 4;
    MetersPerSec turningSpeed = arcLength / Seconds(1);

    // segments between gps points
    MetersPerSec segment1Speed(10);
    MetersPerSec segment2Speed = turningSpeed;
    MetersPerSec segment3Speed(10);

    GpsEvents modifiedEvents = use2PointsGpsSpeed(gpsEvents, directionEvents);
    // the first and the last points shouldn't be changed
    EXPECT_NEAR(modifiedEvents[0].speed->value(), 15.0, 0.001);
    EXPECT_NEAR(modifiedEvents[1].speed->value(),
                (segment1Speed + segment2Speed).value() / 2, 0.001);
    EXPECT_NEAR(modifiedEvents[2].speed->value(),
                (segment2Speed + segment3Speed).value() / 2, 0.001);
    EXPECT_NEAR(modifiedEvents[3].speed->value(), 15.0, 0.001);

    // check that the rest of the values were not changed:

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].speed = gpsEvents[i].speed;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}


TEST(gps_tests, test2PointsAzimuth)
{
    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0 + Vector2{0, 0}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0 + Vector2{0, 20}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0 + Vector2{0, 60}, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0 + Vector2{0, 60}, 0.0_m, 5.0_mps, PI);

    GpsEvents modifiedEvents = use2PointsGpsAzimuth(gpsEvents);
    // the first and the last points shouldn't be changed
    EXPECT_NEAR(modifiedEvents[0].direction->value(), M_PI, 0.001);
    EXPECT_NEAR(modifiedEvents[1].direction->value(), M_PI / 2, 0.001);
    // no direction with zero speed
    EXPECT_TRUE(modifiedEvents[2].direction == std::nullopt);
    EXPECT_NEAR(modifiedEvents[3].direction->value(), M_PI, 0.001);

    // check that the rest of the values were not changed:

    for (size_t i = 0; i < gpsEvents.size(); i++) {
        // return all the changed values back, so we can compare events
        modifiedEvents[i].direction = gpsEvents[i].direction;
    }
    EXPECT_THAT(modifiedEvents, testing::Eq(gpsEvents));
}

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