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

using maps::geolib3::Vector3;

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

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

TEST(speed_tests, test_speed_no_acceleration)
{
    CarHorizontalFrontVecEvents horFrontEvents {
        {1.5_ts, UnitVector3({1, 0, 0})},
        {2.5_ts, UnitVector3({1, 0, 0})}
    };

    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{0, 0, 0}},
        {2.5_ts, AccelerationVector{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}
    };

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 5.0, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 5.0, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

TEST(speed_tests, test_speed_no_acceleration_gps_error)
{
    CarHorizontalFrontVecEvents horFrontEvents {
        {1.5_ts, UnitVector3({1, 0, 0})},
        {2.5_ts, UnitVector3({1, 0, 0})}
    };

    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{0, 0, 0}},
        {2.5_ts, AccelerationVector{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}, // this error should be ignored
        {3.0_ts, P0, 0.0_m, 5.0_mps, PI}
    };

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 5.0, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 5.0, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

TEST(speed_tests, test_speed_acceleration_perfect_accuracy)
{
    CarHorizontalFrontVecEvents horFrontEvents {
        {1.5_ts, UnitVector3({1, 0, 0})},
        {2.50001_ts, UnitVector3({1, 0, 0})}
    };

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

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

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 6, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 7.5, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

TEST(speed_tests, test_speed_acceleration_bad_gps_accuracy)
{
    // the same as test_speed_acceleration_perfect_accuracy
    // but speed decreases and there is an error in gps speed

    // the error in gps should be ignored

    CarHorizontalFrontVecEvents horFrontEvents {
        {1.5_ts, UnitVector3({1, 0, 0})},
        {2.50001_ts, UnitVector3({1, 0, 0})}
    };

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

    GpsEvents gpsEvents {
        {1.0_ts, P0, 0.0_m, 8.0_mps, 0.0_rad},
        {2.0_ts, P0, 0.0_m, 7.5_mps, 0.0_rad}, // should be 7
        {3.0_ts, P0, 0.0_m, 5.0_mps, 0.0_rad}
    };

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 7.5, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 6, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

TEST(speed_tests, test_speed_gravity_changed)
{
    // acceleration 2(m/sec/sec)

    CarHorizontalFrontVecEvents horFrontEvents {
        {1.99999_ts, UnitVector3({1, 0, 0})},
        {2.00000_ts, UnitVector3({0, 1, 0})}
    };

    AccelerometerEvents accEvents {
        {1.5_ts, AccelerationVector{2, 0, 0}},

        // gravity was changed
        // front acceleration is still 2 (m/sec/sec)
        {2.5_ts, AccelerationVector{0, 2, 0}}
    };

    GpsEvents gpsEvents {
        {1.0_ts, P0, 0.0_m, 15.0_mps, 1.0_rad},
        {2.0_ts, P0, 0.0_m, 17.0_mps, 2.0_rad},
        {3.0_ts, P0, 0.0_m, 19.0_mps, 3.0_rad}
    };

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 16, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 18, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

TEST(speed_tests, test_speed_bad_gps_ignored)
{
    CarHorizontalFrontVecEvents horFrontEvents {
        {1.5_ts, UnitVector3({1, 0, 0})},
        {2.5_ts, UnitVector3({1, 0, 0})}
    };

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

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

    CarGroundSpeedEvents speedEvents
        = calculateSpeedEvents(gpsEvents, horFrontEvents, accEvents);

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

    EXPECT_NEAR(speedEvents[0].speed.value(), 5.5, 0.001);
    EXPECT_EQ(speedEvents[0].time, 1.5_ts);

    EXPECT_NEAR(speedEvents[1].speed.value(), 6.5, 0.001);
    EXPECT_EQ(speedEvents[1].time, 2.5_ts);
}

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