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

using maps::geolib3::Vector3;

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

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

TEST(azimuth_tests, test_azimuth_perfect_accuracy_no_rotation)
{
    GravityEvents gravityEvents {
        {1.5_ts, UnitVector3({0, -1, 0})},
        {2.5_ts, UnitVector3({0, -1, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 0, 0}},
        {2.5_ts, RotationSpeedVector{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}
    };

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), M_PI, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), M_PI, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

TEST(azimuth_tests, test_azimuth_no_rotation)
{
    GravityEvents gravityEvents {
        {1.5_ts, UnitVector3({0, -1, 0})},
        {2.5_ts, UnitVector3({0, -1, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 0, 0}},
        {2.5_ts, RotationSpeedVector{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 + 0.5}, // this error should
                                                // be ignored
        {3.0_ts, P0, 0.0_m, 5.0_mps, PI}
    };

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), M_PI, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), M_PI, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

TEST(azimuth_tests, test_azimuth_rotation_perfect_accuracy)
{
    // azimuth rotation 1(1/sec)

    GravityEvents gravityEvents {
        {1.5_ts, UnitVector3({0, -1, 0})},
        {2.50001_ts, UnitVector3({0, -1, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 1, 0}},
        {2.5_ts, RotationSpeedVector{0, 2, 0}}
    };

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

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), 1.5, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), 3, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

TEST(azimuth_tests, test_azimuth_rotation_bad_gps_accuracy)
{
    // the same as test_azimuth_rotation_perfect_accuracy
    // but azimuth decreases and there is an error in gps azimuth

    // the error in gps should be ignored

    GravityEvents gravityEvents {
        {1.5_ts, UnitVector3({0, -1, 0})},
        {2.5_ts, UnitVector3({0, -1, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, -1, 0}},
        {2.5_ts, RotationSpeedVector{0, -1, 0}}
    };

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

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), 2.5, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), 1.5, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

TEST(azimuth_tests, test_azimuth_rotation_gravity_changed)
{
    // azimuth rotation 1(1/sec)

    GravityEvents gravityEvents {
        {1.99999_ts, UnitVector3({0, -1, 0})},
        {2.00000_ts, UnitVector3({1, 0, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 1, 0}},
        // azimuth changing speed is still 1.0
        {2.5_ts, RotationSpeedVector{-1, 0, 0}}
    };

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

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), 1.5, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), 2.5, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

TEST(azimuth_tests, test_azimuth_rotation_bad_gps_ingored)
{
    // azimuth rotation 1(1/sec)

    GravityEvents gravityEvents {
        {1.5_ts, UnitVector3({0, -1, 0})},
        {2.5_ts, UnitVector3({0, -1, 0})}
    };

    GyroscopeEvents gyroEvents {
        {1.5_ts, RotationSpeedVector{0, 1, 0}},
        {2.5_ts, RotationSpeedVector{0, 1, 0}}
    };

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

    CarGroundDirectionEvents directionEvents
        = calculateAzimuthEvents(gpsEvents, gravityEvents, gyroEvents);

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

    EXPECT_NEAR(directionEvents[0].angle.value(), 1.5, 0.001);
    EXPECT_EQ(directionEvents[0].time, 1.5_ts);

    EXPECT_NEAR(directionEvents[1].angle.value(), 2.5, 0.001);
    EXPECT_EQ(directionEvents[1].time, 2.5_ts);
}

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