#include "utils.h"

#include <library/cpp/testing/gtest/gtest.h>
#include <library/cpp/testing/common/env.h>
#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/object_positioner.h>

namespace maps::mrc::sensors_feature_positioner::tests {

using namespace maps::mrc::pos_improvment;

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

namespace {

ResultObjects sortByRayId(ResultObjects objects) {
    for (auto& resultObject : objects) {
        std::sort(resultObject.rays.begin(), resultObject.rays.end(),
                  [](const Ray& lhs, const Ray& rhs) {
                      return lhs.rayId < rhs.rayId;
                  });
    }
    std::sort(objects.begin(), objects.end(),
              [](const ResultObject& lhs, const ResultObject& rhs) {
                  REQUIRE(lhs.rays.size(), "empty ObjectPositions array");
                  REQUIRE(rhs.rays.size(), "empty ObjectPositions array");
                  return lhs.rays[0].rayId < rhs.rays[0].rayId;
              });
    return objects;
}

} // anonymous namespace

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

TEST(object_positioner_tests, test_object_from_one_ray)
{
    auto signType = traffic_signs::TrafficSign::ProhibitoryNoParking;

    geolib3::Point2 cameraMercPos = P0;
    geolib3::Point2 cameraOdoMercPos = P0 + Vector2{5, 15};
    pos_improvment::UnitVector3 directionToObject(Vector3(0, 1, 0)); // to North
    double metersToObject = 20;
    RayId rayId = 45;
    db::TId featureId = 37;

    Ray ray = createRay(signType,
                        cameraMercPos,
                        cameraOdoMercPos,
                        directionToObject,
                        metersToObject,
                        rayId,
                        featureId);

    Rays rays{ray};
    auto objects = calculateObjectsPositionsFromOneRide(rays);

    ASSERT_EQ(objects.size(), 1u);
    ASSERT_EQ(objects[0].rays.size(), 1u);

    EXPECT_TRUE(objects[0].rays[0].rayId == ray.rayId);
    EXPECT_TRUE(objects[0].rays[0].featureId == ray.featureId);
    EXPECT_NEAR(objects[0].mercatorPos.x(), P0.x(), 0.001);
    EXPECT_NEAR(objects[0].mercatorPos.y(), P0.y() + 40, 0.001);
}

TEST(object_positioner_tests, test_several_rays)
{
    auto signType = traffic_signs::TrafficSign::ProhibitoryNoParking;

    geolib3::Point2 cameraMercPos = P0;
    geolib3::Point2 cameraOdoMercPos = P0 + Vector2{5, 15};
    pos_improvment::UnitVector3 directionToObject(Vector3(0, 1, 0)); // to North
    double metersToObject = 5;
    RayId rayId = 45;
    db::TId featureId = 37;

    Ray ray1 = createRay(signType,
                         cameraMercPos,
                         cameraOdoMercPos,
                         directionToObject,
                         metersToObject,
                         rayId,
                         featureId);

    pos_improvment::UnitVector3 directionToObject2(Vector3(3, 4, 0)); // to East-North

    Ray ray2 = createRay(signType,
                         cameraMercPos - Vector2(30, 0),
                         cameraOdoMercPos - Vector2(30, 0),
                         directionToObject2,
                         metersToObject,
                         rayId + 1,
                         featureId + 1);

    pos_improvment::UnitVector3 directionToObject3(Vector3(-3, 4, 0.1)); // to West-North
    Ray ray3 = createRay(signType,
                         cameraMercPos + Vector2(30, 0),
                         cameraOdoMercPos + Vector2(30, 0),
                         directionToObject3,
                         metersToObject,
                         rayId + 2,
                         featureId + 2);


    pos_improvment::UnitVector3 directionToObject4(Vector3(0, 1, 0)); // to North
    geolib3::Point2 cameraMercPos4 = cameraMercPos + Vector2{40, 0};
    geolib3::Point2 cameraOdoMercPos4 = cameraOdoMercPos + Vector2{40, 0};
    double metersToObject4 = 20;
    Ray ray4 = createRay(signType,
                         cameraMercPos4,
                         cameraOdoMercPos4,
                         directionToObject4,
                         metersToObject4,
                         rayId + 3,
                         featureId + 3);

    // rays1-3 intersect with each other
    // ray4 doesn't intersect with other rays

    auto rays = Rays{ray1, ray2, ray3, ray4};
    ResultObjects objects = calculateObjectsPositionsFromOneRide(rays);
    objects = sortByRayId(objects);

    ASSERT_EQ(objects.size(), 2u);

    ASSERT_EQ(objects[0].rays.size(), 3u);
    EXPECT_EQ(objects[0].rays[0].rayId, ray1.rayId);
    EXPECT_EQ(objects[0].rays[1].rayId, ray2.rayId);
    EXPECT_EQ(objects[0].rays[2].rayId, ray3.rayId);
    EXPECT_NEAR(objects[0].mercatorPos.x(), cameraMercPos.x(), 0.02);
    EXPECT_NEAR(objects[0].mercatorPos.y(), cameraMercPos.y() + 40, 0.02);
    // cameraMercPos.y() + 40 is the point of the intersection.
    // Positioning using metersToObject value returns another y() value

    ASSERT_EQ(objects[1].rays.size(), 1u);
    EXPECT_EQ(objects[1].rays[0].rayId, ray4.rayId);
    EXPECT_NEAR(objects[1].mercatorPos.x(), cameraMercPos4.x(), 0.02);
    EXPECT_NEAR(objects[1].mercatorPos.y(), cameraMercPos4.y() + 40, 0.02);
}

TEST(object_positioner_tests, test_split_and_sort_rays)
{
    auto signType = traffic_signs::TrafficSign::ProhibitoryNoParking;

    geolib3::Point2 cameraMercPos = P0;
    geolib3::Point2 cameraOdoMercPos = P0 + Vector2{5, 15};
    pos_improvment::UnitVector3 directionToObject(Vector3(0, 1, 0)); // to North
    double metersToObject = 5;
    db::TId featureId = 37;

    RayId rayId = 11;
    Ray ray1 = createRay(signType,
                         cameraMercPos,
                         cameraOdoMercPos,
                         directionToObject,
                         metersToObject,
                         rayId,
                         featureId,
                         "sourceId1",
                         5.0_ts);

    rayId = 12;
    Ray ray2 = createRay(signType,
                         cameraMercPos,
                         cameraOdoMercPos,
                         directionToObject,
                         metersToObject,
                         rayId,
                         featureId,
                         "sourceId1",
                         7.0_ts);

    rayId = 13;
    Ray ray3 = createRay(signType,
                         cameraMercPos,
                         cameraOdoMercPos,
                         directionToObject,
                         metersToObject,
                         rayId,
                         featureId,
                         "sourceId2",
                         8.0_ts);

    rayId = 14;
    Ray ray4 = createRay(signType,
                         cameraMercPos,
                         cameraOdoMercPos,
                         directionToObject,
                         metersToObject,
                         rayId,
                         featureId,
                         "sourceId2",
                         6.0_ts);

    std::vector<Rays> rides = separateBySourceIdAndSort(
        {ray1, ray2, ray3, ray4});

    ASSERT_EQ(rides.size(), 2u);

    ASSERT_EQ(rides[0].size(), 2u);
    ASSERT_EQ(rides[0][0].rayId, 11u);
    ASSERT_EQ(rides[0][1].rayId, 12u);

    ASSERT_EQ(rides[1].size(), 2u);
    ASSERT_EQ(rides[1][0].rayId, 14u);
    ASSERT_EQ(rides[1][1].rayId, 13u);
}

} // namespace maps::mrc::sensors_feature_positioner::tests
