#include "signs.h"

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

#include <vector>

namespace maps::mrc::tracks_with_sensors {

const double SIGN_SIZE_METERS = 0.7;
constexpr double IMAGE_X_FOV = 70.0 * M_PI / 180.0;

// We assume that the camera is similar to a pinhole camera
//
//
// ______  the "big" image on the same plane as the sign
// \    /
//  \  /
//   \/
//   /\
//   --    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 sign 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 getMetersToSign(
    double signX, // sign center coordinates in [-width/2, width/2],
    double signY, //                            [-height/2,height/2] range
    double signSizePixels,
    common::Size imgPixelsSize)
{
    auto pixelsToMeters = [=](double distanceInPixels) {
        return distanceInPixels / signSizePixels * SIGN_SIZE_METERS;
    };
    // lets assume that there is a big image on the same plane as the sign
    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 sign
    double fromCenterToSignDistancePixels = std::sqrt(signX * signX + signY * signY);

    // meters distance from the big image center to the sign
    double fromCenterToSignDistanceMtrs = pixelsToMeters(
        fromCenterToSignDistancePixels);

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

pos_improvment::UnitVector3 getDirectionToSign(
    const pos_improvment::ImprovedGpsEvent& cameraPos,
    double signX, // sign center coordinates in [-width/2, width/2],
    double signY, //                            [-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 directionToSign = pos_improvment::UnitVector3(
        cameraPos.cameraFrontDirection() * distanceToImagePixels
        + realCameraRightDirection * signX
        + realCameraUpDirection * signY);

    return directionToSign;
}

SignPhoto constructSignPhoto(const Photo& photo,
                             const db::Sign& sign,
                             const db::SignFeature& signFeature,
                             traffic_signs::TrafficSign signType)
{
    common::ImageBox signBbox(signFeature.minX(), signFeature.minY(),
                              signFeature.maxX(), signFeature.maxY());

    if (photo.feature.hasOrientation()) {
        signBbox = common::transformByImageOrientation(
            signBbox,
            common::Size{photo.feature.size().width, photo.feature.size().height},
            photo.feature.orientation());
    }

    double signSizePixels = std::max(signBbox.maxX() - signBbox.minX(),
                                     signBbox.maxY() - signBbox.minY());

    // sign coordinates in image centered axis (where x in [-width/2, width/2])
    double signX = (signBbox.minX() + signBbox.maxX()) / 2.0
        - photo.feature.size().width / 2.0;
    double signY = (signBbox.minY() + signBbox.maxY()) / 2.0
        - photo.feature.size().height / 2.0;
    signY = -signY;

    return SignPhoto{
        signType,
        photo.cameraPos,
        getDirectionToSign(photo.cameraPos, signX, signY, photo.feature.size()),
        getMetersToSign(signX, signY, signSizePixels, photo.feature.size()),
        photo.feature,
        sign};
}

SignPhotos loadSigns(
    wiki::common::PoolHolder& poolHolder,
    const Photos& photos)
{
    auto txn = poolHolder.pool().slaveTransaction();
    db::SignFeatureGateway signFeatureGtw(*txn);

    std::vector<db::TId> featureIds;
    std::map<db::TId, const Photo*> photosMap;
    for (auto& photo : photos) {
        auto featureId = photo.feature.id();
        featureIds.push_back(featureId);
        photosMap[featureId] = &photo;
    }
    db::SignFeatures signFeatures = signFeatureGtw.load(
        db::table::SignFeature::featureId.in({featureIds}));

    std::vector<db::TId> signIds;
    for (const auto& signFeature : signFeatures) {
        signIds.push_back(signFeature.signId());
    }

    db::SignGateway signGtw(*txn);
    db::Signs signs = signGtw.load(db::table::Sign::id.in({signIds}));

    std::map<db::TId, db::Sign*> signsMap;
    for (auto& sign : signs) {
        signsMap[sign.id()] = &sign;
    }

    SignPhotos signPhotos;
    for (const auto& signFeature : signFeatures) {
        signPhotos.push_back(
            constructSignPhoto(
                *photosMap[signFeature.featureId()],
                *signsMap[signFeature.signId()],
                signFeature,
                signsMap[signFeature.signId()]->type()));
    }

    INFO() << "loaded " << signPhotos.size() << "signs";
    return signPhotos;
}

} // namespace maps::mrc::tracks_with_sensors
