#include <maps/wikimap/mapspro/services/mrc/eye/lib/detection/include/position_matcher.h>
#include <maps/wikimap/mapspro/services/mrc/eye/lib/detection/include/store.h>

#include <maps/wikimap/mapspro/services/mrc/eye/lib/location/include/location.h>

#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/direction.h>

namespace maps::mrc::eye {

namespace {

static const std::map<db::eye::DetectionType, double>
DISTANCE_METERS_BY_TYPE{
    {db::eye::DetectionType::HouseNumber, 50.},
    {db::eye::DetectionType::Sign, 20.},
    {db::eye::DetectionType::TrafficLight, 30.},
    {db::eye::DetectionType::RoadMarking, 30.},
};

static const std::map<db::eye::DetectionType, std::optional<geolib3::Degrees>>
ANGLE_EPSILON_BY_TYPE{
    {db::eye::DetectionType::HouseNumber, std::nullopt},
    {db::eye::DetectionType::Sign, geolib3::Degrees{90.}},
    {db::eye::DetectionType::TrafficLight, geolib3::Degrees{90.}},
    {db::eye::DetectionType::RoadMarking, geolib3::Degrees{90.}},
};

std::optional<Location> findSignLocation(
    const DetectionStore& store, db::TId detectionId)
{
    return findLocationBySingleView(
        store.deviceByDetectionId(detectionId),
        store.frameByDetectionId(detectionId),
        store.locationByDetectionId(detectionId),
        store.detectionById(detectionId),
        defaultSignPattern()
    );
}

std::optional<Location> findTrafficLightLocation(
    const DetectionStore& store, db::TId detectionId)
{
    return findLocationBySingleView(
        store.deviceByDetectionId(detectionId),
        store.frameByDetectionId(detectionId),
        store.locationByDetectionId(detectionId),
        store.detectionById(detectionId),
        defaultTrafficLightPattern()
    );
}

Location findHouseNumberLocation(
    const DetectionStore& store, db::TId detectionId)
{
    const auto& frameLocation = store.locationByDetectionId(detectionId);
    return Location{
        frameLocation.mercatorPos(),
        reverseRotationHeading(frameLocation.rotation())
    };
}

Location findRoadMarkingLocation(
    const DetectionStore& store, db::TId detectionId)
{
    const db::eye::FrameLocation& frameLocation
        = store.locationByDetectionId(detectionId);

    return findRoadMarkingLocationBySingleView(frameLocation);
}

} // namespace

std::optional<Location> findLocation(const DetectionStore& store, db::TId detectionId) {
    const auto& group = store.groupByDetectionId(detectionId);
    switch (group.type()) {
        case db::eye::DetectionType::Sign:
            return findSignLocation(store, detectionId);
        case db::eye::DetectionType::TrafficLight:
            return findTrafficLightLocation(store, detectionId);
        case db::eye::DetectionType::HouseNumber:
            return findHouseNumberLocation(store, detectionId);
        case db::eye::DetectionType::RoadMarking:
            return findRoadMarkingLocation(store, detectionId);
        default:
            throw RuntimeError() << "Unknown detection type - " << group.type();
    };
}

MatchedFrameDetections PositionDetectionMatcher::makeMatches(
    const DetectionStore& store,
    const DetectionIdPairSet& detectionPairs,
    const FrameMatcher* /*frameMatcherPtr*/) const
{
    constexpr float HIGH_RELEVANCE = 1.;
    db::TIdSet detectionIds;
    for (const auto& [detectionId0, detectionId1] : detectionPairs) {
        detectionIds.insert(detectionId0);
        detectionIds.insert(detectionId1);
    }

    db::IdTo<Location> locationByDetectionId;
    for (db::TId detectionId : detectionIds) {
        std::optional<Location> location = findLocation(store, detectionId);
        if (!location.has_value()) {
            continue;
        }
        locationByDetectionId[detectionId] = location.value();
    }

    MatchedFrameDetections matches;
    for (const auto& [detectionId0, detectionId1] : detectionPairs) {
        db::eye::DetectionType type0 = store.groupByDetectionId(detectionId0).type();
        db::eye::DetectionType type1 = store.groupByDetectionId(detectionId1).type();

        REQUIRE(type0 == type1, "Invalid types in detection pair");

        auto locationIt0 = locationByDetectionId.find(detectionId0);
        if (locationByDetectionId.end() == locationIt0) {
            continue;
        }
        auto locationIt1 = locationByDetectionId.find(detectionId1);
        if (locationByDetectionId.end() == locationIt1) {
            continue;
        }

        db::TId frameId0 = store.frameId(detectionId0);
        db::TId frameId1 = store.frameId(detectionId1);
        if (frameId0 == frameId1) {
            // Matches in the same frame are forbidden
            continue;
        }

        const double distanceMeters = DISTANCE_METERS_BY_TYPE.at(type0);
        const std::optional<geolib3::Degrees> angleEpsilon = ANGLE_EPSILON_BY_TYPE.at(type0);

        const auto& [position0, rotation0] = locationIt0->second;
        const auto& [position1, rotation1] = locationIt1->second;

        const double distance = geolib3::toMercatorUnits(distanceMeters, position0);
        if (distance > geolib3::distance(position0, position1) &&
            (!angleEpsilon.has_value() ||
             absDiffInDegrees(rotation0, rotation1) < angleEpsilon.value()))
        {
            matches.emplace_back(
                FrameDetectionId{frameId0, detectionId0},
                FrameDetectionId{frameId1, detectionId1},
                HIGH_RELEVANCE
            );
        }
    }

    return matches;
}

} // namespace maps::mrc::eye
