#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_ray.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;

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

const double SIGN_SIZE_METERS = 0.7;

TEST(object_ray_tests, test_create_object_ray)
{
    size_t objectTypeId = static_cast<size_t>(traffic_signs::TrafficSign::ProhibitoryNoParking);

    common::Size photoSize(1920, 1080);
    // right-up from the image center
    size_t bboxX1 = photoSize.width / 2 + 100;
    size_t bboxX2 = photoSize.width / 2 + 200;
    size_t bboxY1 = photoSize.height / 2 - 400;
    size_t bboxY2 = photoSize.height / 2 - 300;
    common::ImageBox objectBbox(bboxX1, bboxY1, bboxX2, bboxY2);

    const geolib3::Radians carDirection(M_PI); // to West
    const GpsEvent gpsEvent(10.0_ts, P0, 0.0_m, 10.0_mps, carDirection);

    UnitVector3 cameraFrontDirection(Vector3(0, 1, 0)); // to North
    UnitVector3 cameraRightDirection(Vector3(1, 0, 0)); // to East
    UnitVector3 cameraUpDirection(Vector3(0, 0, 1)); // to sky
    geolib3::Point2 odometerMercatorPos = P0 + Vector2(2, 4);
    ImprovedGpsEvent cameraPosition(gpsEvent,
                                    odometerMercatorPos,
                                    cameraFrontDirection,
                                    cameraRightDirection,
                                    cameraUpDirection);

    size_t rayId = 15;
    int featureId = 25;
    std::string sourceId = "sourceId456";
    Ray ray = constructRay(objectBbox,
                           cameraPosition,
                           photoSize,
                           rayId,
                           featureId,
                           sourceId,
                           SIGN_SIZE_METERS,
                           objectTypeId);

    EXPECT_EQ(ray.objectTypeId, objectTypeId);
    EXPECT_TRUE(ray.cameraPos == cameraPosition);
    EXPECT_EQ(ray.rayId, rayId);
    EXPECT_EQ(ray.featureId, featureId);
    EXPECT_EQ(ray.sourceId, sourceId);

    double bboxXCenter = (bboxX1 + bboxX2) / 2.0 - photoSize.width / 2.0;
    double bboxYCenter = -((bboxY1 + bboxY2) / 2.0) + photoSize.height / 2.0;
    double imgXHalfFov = IMAGE_X_FOV / 2;
    double imgXHalfFovTg = std::tan(imgXHalfFov);
    double bboxXAngleTg = imgXHalfFovTg / (photoSize.width / 2) * bboxXCenter;
    double bboxYAngleTg = imgXHalfFovTg / (photoSize.width / 2) * bboxYCenter;
    UnitVector3 rayToObject(Vector3(bboxXAngleTg, 1, bboxYAngleTg));

    EXPECT_NEAR(ray.directionToObject.x(), rayToObject.x(), 0.00001);
    EXPECT_NEAR(ray.directionToObject.y(), rayToObject.y(), 0.00001);
    EXPECT_NEAR(ray.directionToObject.z(), rayToObject.z(), 0.00001);

    // should be close to cameraFrontDirection(0, 1, 0), but more
    // east and higher
    EXPECT_NEAR(ray.directionToObject.x(), 0.25, 0.24);
    EXPECT_NEAR(ray.directionToObject.y(), 0.75, 0.24);
    EXPECT_NEAR(ray.directionToObject.z(), 0.25, 0.24);

    double objectSizePixels = std::max(bboxX2 - bboxX1, bboxY2 - bboxY1);
    double metersInPixel = SIGN_SIZE_METERS / objectSizePixels;
    double objectPlaneWidthMeters = photoSize.width * metersInPixel;
    double distanceToPlaneMeters = (objectPlaneWidthMeters / 2) / imgXHalfFovTg;
    double distanceToObjectMeters = std::sqrt(
        distanceToPlaneMeters * distanceToPlaneMeters
        + bboxXCenter * metersInPixel * bboxXCenter * metersInPixel
        + bboxYCenter * metersInPixel * bboxYCenter * metersInPixel);

    EXPECT_NEAR(ray.metersToObject, distanceToObjectMeters, 0.00001);
    EXPECT_NEAR(ray.metersToObject, 30.0, 25.0);
}

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