#include "object_single_ray_positioner.h"

#include "camera_azimuth_calibration.h"
#include "points_clustering.h"
#include "object_rays_intersection.h"
#include "utils.h"

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

#include <vector>

namespace maps::mrc::sensors_feature_positioner {

namespace {

const double MAX_RADIUS_OF_POSITION_POINTS_CLUSTER = 15;

double metersBetween(const ObjectPosition& object1,
                     const ObjectPosition& object2)
{
    double mercatorDistance = geolib3::length(
        geolib3::Vector3(
            object1.mercatorPos.x() - object2.mercatorPos.x(),
            object1.mercatorPos.y() - object2.mercatorPos.y(),
            object1.mercatorPos.z() - object2.mercatorPos.z()));
    return geolib3::toMeters(
        mercatorDistance,
        object1.mercatorPos2d());
}

ObjectPositions calculateObjectsPositions(const Rays& rays)
{
    ObjectPositions objects;
    objects.reserve(rays.size());

    for (const auto& ray : rays) {
        geolib3::Point3 cameraPos(ray.cameraPos.mercatorPosition().x(),
                                  ray.cameraPos.mercatorPosition().y(),
                                  0);
        geolib3::Vector3 cameraDir
            = ray.directionToObject
            * geolib3::toMercatorUnits(1, ray.cameraPos.mercatorPosition());
        geolib3::Point3 objectPos = cameraPos + cameraDir * ray.metersToObject;

        objects.push_back(ObjectPosition{objectPos, ray});
    }
    return objects;
}

// Some objects of one type cannot be one object because:
// 1) we can se both objects one one picture
// 2) cameras azimuth is too different
bool cannotBeOneObject(const ObjectPosition& object1, const ObjectPosition& object2)
{
    if (object1.ray.featureId == object2.ray.featureId) {
        return true;
    }
    geolib3::Radians angleBetweenCameras(
        std::acos(geolib3::innerProduct(object1.ray.directionToObject,
                                        object2.ray.directionToObject)));
    if (angleBetweenCameras > MAX_ANGLE_BETWEEN_CAMERAS)
    {
        return true;
    }
    return false;
}

bool mayBeOneObject(const ObjectPosition& object1, const ObjectPosition& object2)
{
    if (!cannotBeOneObject(object1, object2)
        && object1.ray.objectTypeId == object2.ray.objectTypeId
        && metersBetween(object1, object2) <= MAX_RADIUS_OF_POSITION_POINTS_CLUSTER)
    {
        return true;
    } else {
        return false;
    }
}

// Groups similar neighboring objects.
// Groups only objects in each small part of the sequence(of the car track).
// Don't group objects from different part of the sequence.
std::vector<ObjectPositions> groupObjects(const ObjectPositions& objectPositions)
{
    Points points;
    std::unordered_map<RayId, ObjectPosition> rayIdToObjectPos;
    for (const auto& objectPosition : objectPositions) {
        points[objectPosition.ray.rayId] = Point{
            objectPosition.ray.rayId,
            objectPosition.mercatorPos2d(),
            {}, {}};
        rayIdToObjectPos.insert(std::make_pair(objectPosition.ray.rayId, objectPosition));
    }
    for (size_t i = 0; i < objectPositions.size(); i++) {
        for (size_t j = i + 1; j < objectPositions.size(); j++) {
            if (metersBetweenCameras(objectPositions[i].ray, objectPositions[j].ray)
                > MAX_METERS_BETWEEN_CAMERAS)
            {
                break;
            }
            if (mayBeOneObject(objectPositions[i], objectPositions[j])) {
                points[objectPositions[i].ray.rayId].neighbors.push_back(
                    objectPositions[j].ray.rayId);
                points[objectPositions[j].ray.rayId].neighbors.push_back(
                    objectPositions[i].ray.rayId);
            } else if (cannotBeOneObject(objectPositions[i], objectPositions[j])) {
                points[objectPositions[i].ray.rayId].prohibitedNeighbors.push_back(
                    objectPositions[j].ray.rayId);
                points[objectPositions[j].ray.rayId].prohibitedNeighbors.push_back(
                objectPositions[i].ray.rayId);
            }
        }
    }
    auto clusters = getClusters(points, MAX_RADIUS_OF_POSITION_POINTS_CLUSTER);
    std::vector<ObjectPositions> groups(clusters.size());
    for (size_t i = 0; i < clusters.size(); i++) {
        for (const auto& pointId : clusters[i]) {
            RayId rayId = points[pointId].id;
            groups[i].push_back(rayIdToObjectPos.at(rayId));
        }
    }
    return groups;
}

} // anonymous namespace

std::vector<ObjectPositions> calculateObjects(Rays& rays)
{
    ObjectPositions objectPositions = calculateObjectsPositions(rays);
    return groupObjects(objectPositions);
}

} // namespace maps::mrc::sensors_feature_positioner
