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

#include <maps/wikimap/mapspro/services/mrc/eye/lib/generate_hypothesis/include/common.h>
#include <maps/wikimap/mapspro/services/mrc/eye/lib/generate_hypothesis/include/load.h>

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

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

namespace maps::mrc::eye {

namespace {

// загружаем все объекты в прямоугольнике
template <typename Object>
std::vector<Object> loadObjects(
    object::Loader& loader,
    const db::eye::ObjectLocation& location,
    double radiusMeters)
{
    double radiusMercator = toMercatorUnits(radiusMeters, location.mercatorPos());
    return load<Object>(
        loader,
        inArea(location.mercatorPos(), radiusMercator)
    );
}

// загружаем дороги в прямоугольнике, по которым могут передвигаться машины
std::map<db::TId, object::RoadElement> loadElements(
    object::Loader& loader,
    const db::eye::ObjectLocation& location,
    double radiusMeters)
{
    std::vector<object::RoadElement> elements
        = loadObjects<object::RoadElement>(loader, location, radiusMeters);

    std::map<db::TId, object::RoadElement> elementById;
    for (const object::RoadElement& element : elements) {
        if (element.isAccessibleToCars()) {
            elementById.emplace(element.revisionId().objectId(), element);
        }
    }

    return elementById;
}

// загружаем перекрестки, у которых есть дороги, по которым могут передвигаться машины
std::map<db::TId, object::RoadJunction> loadJunctions(
    object::Loader& loader,
    const db::eye::ObjectLocation& location,
    const std::map<db::TId, object::RoadElement>& elementById,
    double radiusMeters)
{
    std::vector<object::RoadJunction> junctions
        = loadObjects<object::RoadJunction>(loader, location, radiusMeters);

    std::map<db::TId, object::RoadJunction> junctionById;
    for (const object::RoadJunction& junction : junctions) {
        for (db::TId elementId : junction.elementIds()) {
            auto elementIt = elementById.find(elementId);
            if (elementIt != elementById.end() && elementIt->second.isAccessibleToCars()) {
                junctionById.emplace(junction.revisionId().objectId(), junction);
                break;
            }
        }
    }

    return junctionById;
}

// вычисляем расстояния до всех перекрестков
std::map<db::TId, double> calculateDistances(
    const db::eye::ObjectLocation& location,
    const std::map<db::TId, object::RoadJunction>& junctionById)
{
    std::map<db::TId, double> distanceById;
    for (const auto& [id, junction] : junctionById) {
        distanceById[id] = toMeters(
            geolib3::distance(junction.geom(), location.mercatorPos()),
            location.mercatorPos()
        );
    }
    return distanceById;
}

/*
  Проверяем, что у перекрестка есть дороги сонаправленные направлению светофора
  1) Перебираем все дороги на перекрестке
  2) Находим ближайший элемент дороги к перекрестку
  3) Проверяем, что отклонения угла не более 45 градусов
*/
bool isCodirectional(
    const db::eye::ObjectLocation& location,
    const object::RoadJunction& junction,
    const std::map<db::TId, object::RoadElement>& elementById)
{
    constexpr geolib3::Radians MAX_ANGLE = geolib3::toRadians(geolib3::Degrees(45));

    for (db::TId elementId : junction.elementIds()) {
        auto elementIt = elementById.find(elementId);
        if (elementById.end() == elementIt) {
            continue;
        }

        const object::RoadElement& element = elementIt->second;

        REQUIRE(
            element.geom().segmentsNumber() > 0,
            "Failed to process empty polyline"
        );

        geolib3::Segment2 segment;
        if (element.startJunctionId() == junction.id()) {
            segment = element.geom().segmentAt(0);
        } else if (element.endJunctionId() == junction.id()) {
            segment = element.geom().segmentAt(element.geom().segmentsNumber() - 1);
        } else {
            throw maps::RuntimeError()
                << "Road element " << elementId << "not correspond to"
                << " junction " << junction.id();
        }


        const auto& [heading, orientation, pitch] = decomposeRotation(location.rotation());
        geolib3::Direction2 direction = geolib3::Direction2(heading);
        geolib3::Direction2 segmentDirection(segment);

        return geolib3::angleBetween(segmentDirection, direction) < MAX_ANGLE ||
               geolib3::angleBetween(segmentDirection, -direction) < MAX_ANGLE;
    }

    return false;
}

// если у перекрестка нет светофора, то соответствующий id == 0
bool hasTrafficLight(const object::RoadJunction& junction) {
    return junction.trafficLightId() != db::NO_OBJECT_ID;
}

/*
  Выбираем ближайший перекресток без светофоров
  Если нет ни одного перекрестка без светофоров, то возвращаем end,
  аналогично методу std::map::find
*/
std::map<db::TId, object::RoadJunction>::const_iterator
findClosestJunctionWithoutTrafficLight(
    const std::map<db::TId, object::RoadJunction>& junctionById,
    const std::map<db::TId, double>& distanceById)
{
    auto closestIt = junctionById.cend();
    double minDistance = std::numeric_limits<double>::max();
    for (auto it = junctionById.cbegin(); it != junctionById.cend(); it++) {
        const auto& [id, junction] = *it;
        if (!hasTrafficLight(junction)) {
            double distance = distanceById.at(id);
            if (distance < minDistance) {
                closestIt = it;
                minDistance = distance;
            }
        }
    }
    return closestIt;
}
// ищем соответствующий светофору элемент дороги - ближайший, но не дальше 30 метров
std::map<db::TId, object::RoadElement>::const_iterator
findCorrespondedRoadElement(
    const db::eye::ObjectLocation& location,
    const std::map<db::TId, object::RoadElement>& elementById)
{
    constexpr double ELEMENT_DISTANCE_THRESHOLD = 30.;

    auto closestIt = elementById.end();
    double minDistance = std::numeric_limits<double>::max();
    for (auto it = elementById.cbegin(); it != elementById.cend(); it++) {
        const auto& [id, element] = *it;
        double distance = toMeters(
            geolib3::distance(location.mercatorPos(), element.geom()),
            location.mercatorPos()
        );
        if (distance < ELEMENT_DISTANCE_THRESHOLD && distance < minDistance) {
            minDistance = distance;
            closestIt = it;
        }
    }
    return closestIt;
}

/*
  Проверяем, что на карте уже есть подходящий светофор:
  1) Если он ближе 30 метров
  2) Он есть и сонаправлен дорогам на перекрестке
*/
bool isAlreadyOnMap(
    const db::eye::ObjectLocation& location,
    const std::map<db::TId, object::RoadJunction>& junctionById,
    const std::map<db::TId, double>& distanceToJunctionById,
    const std::map<db::TId, object::RoadElement>& elementById)
{
    constexpr double CLOSE_DISTANCE_THRESHOLD = 30.;

    for (const auto& [id, junction] : junctionById) {
        if (hasTrafficLight(junction)) {
            if (distanceToJunctionById.at(id) < CLOSE_DISTANCE_THRESHOLD ||
                isCodirectional(location, junction, elementById))
            {
                return true;
            }
        }
    }

    return false;
}

// создаем гипотезу, привязанную к перекрестку
db::eye::Hypothesis makeHypothesis(const object::RoadJunction& junction)
{
    return db::eye::Hypothesis(
        junction.geom(),
        db::eye::AbsentTrafficLightAttrs{junction.revisionId()}
    );
}

// создаем гипотезу, которая не привязана к перекресткам
db::eye::Hypothesis makeHypothesis(const db::eye::ObjectLocation& location) {
    return db::eye::Hypothesis(
        location.mercatorPos(),
        db::eye::AbsentTrafficLightAttrs()
    );
}

// извлекаем все перекрестки, которые у нас есть для соответствующего элемента дороги
std::map<db::TId, object::RoadJunction> extractElementJunctions(
    const object::RoadElement& element,
    const std::map<db::TId, object::RoadJunction>& junctionById)
{
    std::map<db::TId, object::RoadJunction> elementEndById;

    auto startIt = junctionById.find(element.startJunctionId());
    if (startIt != junctionById.end()) {
        elementEndById.emplace(startIt->first, startIt->second);
    }
    auto endIt = junctionById.find(element.endJunctionId());
    if (endIt != junctionById.end()) {
        elementEndById.emplace(endIt->first, endIt->second);
    }

    return elementEndById;
}

} // namespace

bool AbsentTrafficLightGeneratorImpl::appliesToObject(const db::eye::Object&)
{
    // Сразу загружаются объекты с типом db::eye::ObjectType::TrafficLight,
    // поэтому тут не нужны дополнительные проверки
    return true;
}

bool AbsentTrafficLightGeneratorImpl::hasDuplicate(
    pqxx::transaction_base& txn,
    const db::eye::Hypothesis& hypothesis,
    db::TId /*objectId*/)
{
    constexpr double SEARCH_RADIUS_METERS = 30.;
    return hasDuplicateDefaultCheck(txn, hypothesis, SEARCH_RADIUS_METERS);
}

db::eye::Hypotheses AbsentTrafficLightGeneratorImpl::validate(
    const db::eye::Object& /*object*/,
    const db::eye::ObjectLocation& location,
    const db::eye::Objects& /*slaveObjects*/,
    object::Loader& loader)
{
    // all distances in meters
    constexpr double TRAFFIC_LIGHT_SEARCH_RADIUS = 65.;
    constexpr double JUNCTION_DISTANCE_BOTTOM_THRESHOLD = 30.;
    constexpr double JUNCTION_DISTANCE_UPPER_THRESHOLD = 50.;

    std::map<db::TId, object::RoadElement> elementById
        = loadElements(
            loader,
            location, TRAFFIC_LIGHT_SEARCH_RADIUS
        );

    std::map<db::TId, object::RoadJunction> junctionById
        = loadJunctions(
            loader,
            location, elementById, TRAFFIC_LIGHT_SEARCH_RADIUS
        );

    db::eye::Hypotheses hypotheses;

    std::map<db::TId, double> distanceById = calculateDistances(location, junctionById);
    if (!isAlreadyOnMap(location, junctionById, distanceById, elementById)) {
        auto junctionIt = findClosestJunctionWithoutTrafficLight(junctionById, distanceById);
        if (junctionIt != junctionById.cend() &&
            distanceById.at(junctionIt->first) < JUNCTION_DISTANCE_BOTTOM_THRESHOLD)
        {
            const object::RoadJunction& junction = junctionIt->second;
            hypotheses.push_back(makeHypothesis(junction));
        } else {
            auto elementIt = findCorrespondedRoadElement(location, elementById);
            if (elementIt != elementById.end()) {
                std::map<db::TId, object::RoadJunction> elementEndById
                    = extractElementJunctions(elementIt->second, junctionById);
                auto closestEndIt = findClosestJunctionWithoutTrafficLight(elementEndById, distanceById);
                if (closestEndIt != elementEndById.end()) {
                    const auto& [junctionId, junction] = *closestEndIt;
                    if (distanceById.at(junctionId) < JUNCTION_DISTANCE_UPPER_THRESHOLD) {
                        hypotheses.push_back(makeHypothesis(junction));
                    }
                }
            }
        }

        if (hypotheses.empty()) {
            hypotheses.push_back(makeHypothesis(location));
        }
    }

    return hypotheses;
}

} // namespace maps::mrc::eye
