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

using maps::geolib3::Vector3;

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

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

TEST(phone_orientation_tests, test_find_car_up_direction)
{
    AccelerometerEvents events;
    events.emplace_back(1.0_ts, AccelerationVector{0, 1, 0});
    events.emplace_back(2.0_ts, AccelerationVector{1, 1000, -1});
    events.emplace_back(3.0_ts, AccelerationVector{5, 10, 0});
    events.emplace_back(4.0_ts, AccelerationVector{0, -10, 5});
    auto carUpVec = Vector3(findCarUpDirection(events));
    EXPECT_NEAR(carUpVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carUpVec.y(), 1.0, 0.001);
    EXPECT_NEAR(carUpVec.z(), 0.0, 0.001);
}

TEST(phone_orientation_tests, test_find_car_front_direction)
{
    AccelerometerEvents accEvents;
    accEvents.emplace_back(1.0_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(1.33_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(1.66_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(2.0_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(2.33_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(2.66_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(3.0_ts, AccelerationVector{9.8, 0, 0});
    accEvents.emplace_back(3.33_ts, AccelerationVector{9.8, 1, 0});
    accEvents.emplace_back(3.66_ts, AccelerationVector{9.8, 0, 0});
    accEvents.emplace_back(4.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(4.33_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(4.66_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(5.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(5.33_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(5.66_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(6.0_ts, AccelerationVector{9.8, -3, 0});

    GyroscopeEvents gyroEvents;
    // no rotation
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(1.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(1.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(6.0_ts, RotationSpeedVector{0, 0, 0});

    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(6.0_ts, P0, 0.0_m, 5.0_mps, PI);

    auto carFrontVec = Vector3(findCarFrontDirection(accEvents,
                                                     gyroEvents,
                                                     gpsEvents,
                                                     UnitVector3({1, 0, 0})));
    EXPECT_NEAR(carFrontVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.y(), 1.0, 0.001);
    EXPECT_NEAR(carFrontVec.z(), 0.0, 0.001);
}

TEST(phone_orientation_tests, test_find_car_front_direction2)
{
    AccelerometerEvents accEvents;
    accEvents.emplace_back(1.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(1.33_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(1.66_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(2.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(2.33_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(2.66_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(3.0_ts, AccelerationVector{9.8, 0, 0});
    accEvents.emplace_back(3.33_ts, AccelerationVector{9.8, 1, 0});
    accEvents.emplace_back(3.66_ts, AccelerationVector{9.8, 0, 0});
    accEvents.emplace_back(4.0_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(4.33_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(4.66_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(5.0_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(5.33_ts, AccelerationVector{9.8, 4, 0});
    accEvents.emplace_back(5.66_ts, AccelerationVector{9.8, 3, 0});
    accEvents.emplace_back(6.0_ts, AccelerationVector{9.8, 4, 0});

    GyroscopeEvents gyroEvents;
    // no rotation
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(1.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(1.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.33_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.66_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(6.0_ts, RotationSpeedVector{0, 0, 0});

    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(6.0_ts, P0, 0.0_m, 5.0_mps, PI);

    auto carFrontVec = Vector3(findCarFrontDirection(accEvents,
                                                     gyroEvents,
                                                     gpsEvents,
                                                     UnitVector3({1, 0, 0})));
    EXPECT_NEAR(carFrontVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.y(), -1.0, 0.001);
    EXPECT_NEAR(carFrontVec.z(), 0.0, 0.001);
}

TEST(phone_orientation_tests, test_find_car_front_direction_rotation_points)
{
    AccelerometerEvents accEvents;
    // start rotation
    accEvents.emplace_back(1.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(2.0_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(3.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(4.0_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(5.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(6.0_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(7.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(8.0_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(9.0_ts, AccelerationVector{9.8, 4, 10});
    // finish rotation
    accEvents.emplace_back(10.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(11.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(12.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(13.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(14.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(15.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(16.0_ts, AccelerationVector{9.8, -3, 0});

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(2.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(3.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(4.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(5.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(6.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(7.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(8.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(9.0_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(10.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(11.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(12.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(13.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(14.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(15.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(16.0_ts, RotationSpeedVector{0, 0, 0});

    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(7.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(10.0_ts, P0, 0.0_m, 20.0_mps, PI);
    gpsEvents.emplace_back(13.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(16.0_ts, P0, 0.0_m, 10.0_mps, PI);

    auto carFrontVec = Vector3(findCarFrontDirection(accEvents,
                                                     gyroEvents,
                                                     gpsEvents,
                                                     UnitVector3({1, 0, 0})));
    EXPECT_NEAR(carFrontVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.y(), 1.0, 0.001);
    EXPECT_NEAR(carFrontVec.z(), 0.0, 0.001);
}

TEST(phone_orientation_tests, test_find_car_front_direction_different_acc_and_gyro_times)
{
    // the same as test_find_car_front_direction_rotation_points,
    // but gyroscope timestamps are not equal to accelerometer timestamps

    AccelerometerEvents accEvents;
    // start rotation
    accEvents.emplace_back(1.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(1.33_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(1.66_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(2.0_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(2.33_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(2.66_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(3.0_ts, AccelerationVector{9.8, 4, 10});
    accEvents.emplace_back(3.33_ts, AccelerationVector{9.8, 3, 10});
    accEvents.emplace_back(3.66_ts, AccelerationVector{9.8, 4, 10});
    // finish rotation
    accEvents.emplace_back(4.0_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(4.33_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(4.66_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(5.0_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(5.33_ts, AccelerationVector{9.8, -4, 0});
    accEvents.emplace_back(5.66_ts, AccelerationVector{9.8, -3, 0});
    accEvents.emplace_back(6.0_ts, AccelerationVector{9.8, -4, 0});

    GyroscopeEvents gyroEvents;
    gyroEvents.emplace_back(0.9_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(1.23_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(1.56_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(1.9_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(2.23_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(2.56_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(2.9_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(3.23_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(3.56_ts, RotationSpeedVector{0.1, 0, 0});
    gyroEvents.emplace_back(3.9_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.23_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.56_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.9_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.23_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.56_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.9_ts, RotationSpeedVector{0, 0, 0});

    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(2.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(3.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 20.0_mps, PI);
    gpsEvents.emplace_back(5.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(6.0_ts, P0, 0.0_m, 10.0_mps, PI);

    auto carFrontVec = Vector3(findCarFrontDirection(accEvents,
                                                     gyroEvents,
                                                     gpsEvents,
                                                     UnitVector3({1, 0, 0})));
    EXPECT_NEAR(carFrontVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.y(), 1.0, 0.001);
    EXPECT_NEAR(carFrontVec.z(), 0.0, 0.001);
}


TEST(phone_orientation_tests, test_find_car_front_direction_non_perpendicular_to_gravity)
{
    AccelerometerEvents accEvents;
    // with acceleration dow
    accEvents.emplace_back(1.0_ts, AccelerationVector{8.8, 1, 0});
    accEvents.emplace_back(2.0_ts, AccelerationVector{7.8, 2, 0});
    accEvents.emplace_back(3.0_ts, AccelerationVector{8.8, 1, 0});
    accEvents.emplace_back(4.0_ts, AccelerationVector{7.8, 2, 0});
    accEvents.emplace_back(5.0_ts, AccelerationVector{8.8, 1, 0});
    accEvents.emplace_back(6.0_ts, AccelerationVector{7.8, 2, 0});
    accEvents.emplace_back(7.0_ts, AccelerationVector{8.8, 1, 0});
    accEvents.emplace_back(8.0_ts, AccelerationVector{7.8, 2, 0});
    accEvents.emplace_back(9.0_ts, AccelerationVector{8.8, 1, 0});
    // finished acceleration down
    accEvents.emplace_back(10.0_ts, AccelerationVector{9.8, 0, 1});
    accEvents.emplace_back(11.0_ts, AccelerationVector{9.8, 0, 2});
    accEvents.emplace_back(12.0_ts, AccelerationVector{9.8, 0, 1});
    accEvents.emplace_back(13.0_ts, AccelerationVector{9.8, 0, 2});
    accEvents.emplace_back(14.0_ts, AccelerationVector{9.8, 0, 1});
    accEvents.emplace_back(15.0_ts, AccelerationVector{9.8, 0, 2});
    accEvents.emplace_back(16.0_ts, AccelerationVector{9.8, 0, 1});

    GyroscopeEvents gyroEvents;
    // no rotation
    gyroEvents.emplace_back(1.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(2.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(3.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(4.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(5.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(6.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(7.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(8.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(9.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(10.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(11.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(12.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(13.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(14.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(15.0_ts, RotationSpeedVector{0, 0, 0});
    gyroEvents.emplace_back(16.0_ts, RotationSpeedVector{0, 0, 0});

    GpsEvents gpsEvents;
    gpsEvents.emplace_back(1.0_ts, P0, 0.0_m, 5.0_mps, PI);
    gpsEvents.emplace_back(4.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(7.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(10.0_ts, P0, 0.0_m, 15.0_mps, PI);
    gpsEvents.emplace_back(13.0_ts, P0, 0.0_m, 10.0_mps, PI);
    gpsEvents.emplace_back(16.0_ts, P0, 0.0_m, 5.0_mps, PI);

    auto carFrontVec = Vector3(findCarFrontDirection(accEvents,
                                                     gyroEvents,
                                                     gpsEvents,
                                                     UnitVector3({1, 0, 0})));
    EXPECT_NEAR(carFrontVec.x(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.y(), 0.0, 0.001);
    EXPECT_NEAR(carFrontVec.z(), -1.0, 0.001);
}

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