#include <maps/wikimap/mapspro/services/mrc/libs/sensors_feature_positioner/include/object_ray.h>

#include <maps/wikimap/mapspro/services/mrc/libs/common/include/exif.h>
#include <maps/libs/log8/include/log8.h>

#include <vector>

namespace maps::mrc::sensors_feature_positioner {

// We assume that the camera is similar to a pinhole camera
//
//
// ______  the "big" image on the same plane as the object
// \    /
//  \  /
//   \/
//   /\
//   --    the "small" image pixels
//
// In this model all the pixels on the small image have a constant
// size in meters on the big image.
// For each object we have its size in pixels and in meters, so
// we can convert pixels to meters using these 2 values.

// all the calculations use a half of the camera model where both the
// triangles are right triangles.
//  ___
// |  /
// | /
// |/
// |\
// |-

double getMetersToObject(
    double objectX, // object center coordinates in [-width/2, width/2],
    double objectY, //                            [-height/2,height/2] range
    double objectSizePixels,
    common::Size imgPixelsSize,
    double objectMaxSideLengthMeters)
{
    auto pixelsToMeters = [=](double distanceInPixels) {
        return distanceInPixels / objectSizePixels * objectMaxSideLengthMeters;
    };
    // lets assume that there is a big image on the same plane as the object
    double imageWidthMeters = pixelsToMeters(imgPixelsSize.width);

    // meters distance from the camera to the big image
    double distanceToPlaneMeters = imageWidthMeters / 2.0 / std::tan(IMAGE_X_FOV / 2.0);

    // pixel distance from the small image center to the object
    double fromCenterToObjectDistancePixels = std::sqrt(objectX * objectX + objectY * objectY);

    // meters distance from the big image center to the object
    double fromCenterToObjectDistanceMtrs = pixelsToMeters(
        fromCenterToObjectDistancePixels);

    return std::sqrt(distanceToPlaneMeters * distanceToPlaneMeters
                     + fromCenterToObjectDistanceMtrs * fromCenterToObjectDistanceMtrs);
}

pos_improvment::UnitVector3 getDirectionToObject(
    const pos_improvment::ImprovedGpsEvent& cameraPos,
    double objectX, // object center coordinates in [-width/2, width/2],
    double objectY, //                            [-height/2,height/2] range
    common::Size imgPixelsSize)
{
    double distanceToImagePixels
        = imgPixelsSize.width / 2.0 / std::tan(IMAGE_X_FOV / 2.0);

    // We have the phone orientation for portrait mode. But the phone can
    // be in landscape mode

    // if phone is in portrait mode
    auto realCameraRightDirection = cameraPos.cameraRightDirection();
    auto realCameraUpDirection = cameraPos.cameraUpDirection();

    if (abs(cameraPos.cameraRightDirection().z())
        > cameraPos.cameraUpDirection().z())
    {
        // phone is in landscape mode
        if (cameraPos.cameraRightDirection().z() > 0) {
            realCameraUpDirection = cameraPos.cameraRightDirection();
            realCameraRightDirection = pos_improvment::UnitVector3(
                -cameraPos.cameraUpDirection());
        } else {
            realCameraUpDirection = pos_improvment::UnitVector3(
                -cameraPos.cameraRightDirection());
            realCameraRightDirection = cameraPos.cameraUpDirection();
        }
    }

    auto directionToObject = pos_improvment::UnitVector3(
        cameraPos.cameraFrontDirection() * distanceToImagePixels
        + realCameraRightDirection * objectX
        + realCameraUpDirection * objectY);

    return directionToObject;
}

Ray constructRay(
    common::ImageBox bbox,
    pos_improvment::ImprovedGpsEvent cameraPosition,
    common::Size photoSize,
    size_t rayId,
    db::TId featureId,
    const std::string& sourceId,
    double objectMaxSideLengthMeters,
    size_t objectTypeId)
{
    double objectSizePixels = std::max(bbox.maxX() - bbox.minX(),
                                       bbox.maxY() - bbox.minY());

    // object coordinates in image centered axis (where x in [-width/2, width/2])
    double objectX = (bbox.minX() + bbox.maxX()) / 2.0
        - photoSize.width / 2.0;
    double objectY = (bbox.minY() + bbox.maxY()) / 2.0
        - photoSize.height / 2.0;
    objectY = -objectY;

    return Ray{
        objectTypeId,
        cameraPosition,
        getDirectionToObject(cameraPosition, objectX, objectY, photoSize),
        getMetersToObject(objectX, objectY, objectSizePixels, photoSize, objectMaxSideLengthMeters),
        rayId,
        featureId,
        sourceId};
}

} // namespace maps::mrc::sensors_feature_positioner
