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

#include <maps/libs/geolib/include/conversion.h>
#include <maps/libs/geolib/include/units_literals.h>

#include <boost/range/algorithm.hpp>

#include <type_traits>
#include <utility>

namespace maps::mrc::eye {

using namespace geolib3::literals;

namespace {

geolib3::Segment2 getLastSegment(const geolib3::Polyline2& geom, ymapsdf::rd::Direction direction)
{
    if (direction == ymapsdf::rd::Direction::Forward) {
        return geom.segmentAt(geom.segmentsNumber() - 1);
    }

    const geolib3::Segment2 segment = geom.segmentAt(0);
    return geolib3::reverse(segment);
}

geolib3::Segment2 getAdjacentSegment(const object::RoadElement& element, object::TId junctionId)
{
    ASSERT(element.hasJunction(junctionId));

    const geolib3::Polyline2& geom = element.geom();
    ASSERT(geom.pointsNumber() >= 2);

    if (element.startJunctionId() == junctionId) {
        return geom.segmentAt(0);
    }

    return geolib3::reverse(geom.segmentAt(geom.segmentsNumber() - 1));
}

NodeIds collectNodeIds(const RoadIntersections& intersections)
{
    NodeIds nodeIds;

    for (const auto& intersection: intersections) {
        if (intersection.nodeId) {
            nodeIds.push_back(*intersection.nodeId);
        }
    }

    return nodeIds;
}

OptionalNodeId anyNodeId(const RoadIntersections& intersections)
{
    for (const auto& intersection: intersections) {
        if (intersection.nodeId) {
            return intersection.nodeId;
        }
    }

    return std::nullopt;
}

} // namespace

RoadCross::RoadCross(
        RoadIntersections left,
        RoadIntersections forward,
        RoadIntersections right)
    :left_(std::move(left)), forward_(std::move(forward)), right_(std::move(right))
{}

bool RoadCross::isSimple() const { return left_.size() + forward_.size() + right_.size() == 1; }

NodeIds RoadCross::forwardMoves() const { return collectNodeIds(forward_); }

NodeIds RoadCross::leftMoves() const { return collectNodeIds(left_); }

NodeIds RoadCross::rightMoves() const { return collectNodeIds(right_); }

OptionalNodeId RoadCross::anyForwardMove() const { return anyNodeId(forward_); }

OptionalNodeId RoadCross::anyLeftMove() const { return anyNodeId(left_); }

OptionalNodeId RoadCross::anyRightMove() const { return anyNodeId(right_); }

std::optional<NodeId> RoadCross::uniqueForwardMove() const
{
    const NodeIds nodeIds = collectNodeIds(forward_);

    if (nodeIds.size() == 1) {
        return nodeIds.front();
    }

    return std::nullopt;
}

Graph::Graph(
        ymapsdf::rd::AccessId accessId,
        const object::RoadElements& elements,
        const object::RoadJunctions& junctions,
        const object::Conditions& conditions)
    : graph::Graph(accessId, elements, conditions)
{
    for (const auto& junction: junctions) {
        junctionById_.emplace(junction.id(), junction);
    }

    for (const auto& condition: conditions) {
        conditionById_.emplace(condition.id(), condition);
    }
}

bool Graph::hasJunction(object::TId id) const { return junctionById_.count(id); }

const object::RoadJunction& Graph::junctionById(object::TId id) const
{
    const auto it = junctionById_.find(id);
    REQUIRE(it != junctionById_.end(), "No junction with id " << id);
    return it->second;
}

Graph::RoadJunctionRange Graph::junctions() const
{
    return boost::adaptors::values(std::as_const(junctionById_));
}

bool Graph::hasCondition(object::TId id) const { return conditionById_.count(id); }

const object::Condition& Graph::conditionById(object::TId id) const
{
    const auto it = conditionById_.find(id);
    REQUIRE(it != conditionById_.end(), "No condition with id " << id);
    return it->second;
}

Graph::ConditionRange Graph::conditions() const
{
    return boost::adaptors::values(std::as_const(conditionById_));
}

struct RoadElementWithDirectedId {
    DirectedId directedId;
    object::RoadElement element;
};

struct RoadElementCandidate {
    RoadElementWithDirectedId candidate;
    double distance;

    RoadElementCandidate(const RoadElementWithDirectedId& candidate, double distance)
        : candidate(candidate)
        , distance(distance)
    {}
};

inline bool operator<(const RoadElementCandidate& lhs, const RoadElementCandidate& rhs)
{
    return lhs.distance < rhs.distance;
}

NodeIds Graph::getClosestCodirectional(
        const DirectedPoint& point,
        double distanceMeters,
        double toleranceMeters,
        geolib3::Degrees angle)
{
    double maxDistance = geolib3::toMercatorUnits(distanceMeters, point.mercator());

    std::vector<RoadElementCandidate> candidates;
    for (const auto& element: elements()) {
        if (!isSet(accessId(), element.accessId()) || !areCodirectional(element, point, angle)) {
            continue;
        }

        const double distance = geolib3::distance(element.geom(), point.mercator());
        if (distance < maxDistance) {
            const DirectedId directedId(element.id(), direction(element, point));

            candidates.emplace_back(RoadElementWithDirectedId{directedId, element}, distance);
        }
    }

    if (candidates.empty()) {
        return {};
    }

    boost::sort(candidates);

    const double minDistance = candidates.front().distance;
    maxDistance = minDistance + geolib3::toMercatorUnits(toleranceMeters, point.mercator());

    NodeIds result;

    for (const auto& [candidate, distance]: candidates) {
        if (distance > maxDistance) {
            break;
        }

        const auto nodeId = getNodeId(candidate.directedId);
        REQUIRE(
            nodeId,
            "Impossible create node for element "
                << candidate.directedId.roadElementId() << ":"
                << candidate.directedId.direction()
        );

        result.push_back(*nodeId);
    }

    return result;
}

RoadCross Graph::getRoadCross(NodeId nodeId, Mode mode)
{
    std::unordered_map<object::TId, NodeId> elementIdToNodeId;

    for (const auto& edge: edges(nodeId)) {
        const NodeId nextNodeId = edge.endNodeId();
        const DirectedId directedId = getDirectedId(nextNodeId);

        elementIdToNodeId.emplace(directedId.roadElementId(), nextNodeId);
    }

    auto getNodeIdByElementId = [&](object::TId elementId) -> OptionalNodeId
    {
        const auto it = elementIdToNodeId.find(elementId);
        if (it != elementIdToNodeId.end()) {
            return it->second;
        }

        return std::nullopt;
    };

    const DirectedId fromId = getDirectedId(nodeId);
    const object::RoadElement& from = elementByDirectedId(fromId);
    const geolib3::Segment2 fromSegment = getLastSegment(from.geom(), fromId.direction());

    const object::RoadJunction& junction = endJunction(nodeId);
    const int zlev = from.zLevel(junction.id());

    static constexpr auto minAngle = geolib3::toRadians(-20_deg);
    static constexpr auto maxAngle = geolib3::toRadians(30_deg);

    RoadIntersections left, forward, right;

    for (const auto id: junction.elementIds()) {
        if (from.id() == id || not hasElement(id)) {
            continue;
        }

        const object::RoadElement& to = elementById(id);
        if (zlev != to.zLevel(junction.id())) {
            continue;
        }

        geolib3::Segment2 toSegment = getAdjacentSegment(to, junction.id());
        auto angle = geolib3::Radians(
            geolib3::signedAngle(fromSegment.vector(), toSegment.vector())
        );

        RoadIntersection intersection{to.id(), getNodeIdByElementId(to.id())};

        if (to.fow() == object::RoadElement::FormOfWay::Roundabout) {
            left.push_back(intersection);
            continue;
        }

        if (angle > maxAngle) {
            left.push_back(intersection);
        } else if (angle < minAngle) {
            right.push_back(intersection);
        } else {
            if (mode == Mode::UseOnlyFirstSegment) {
                forward.push_back(intersection);
                continue;
            }

            const object::RoadJunction opposite = junctionById(to.oppositeJunction(junction.id()));

            toSegment = geolib3::Segment2(junction.geom(), opposite.geom());
            angle = geolib3::Radians(
                geolib3::signedAngle(fromSegment.vector(), toSegment.vector())
            );

            if (angle > maxAngle) {
                left.push_back(intersection);
            } else if (angle < minAngle) {
                right.push_back(intersection);
            } else {
                forward.push_back(intersection);
            }
        }
    }

    return RoadCross {
        std::move(left),
        std::move(forward),
        std::move(right)
    };
}

bool Graph::hasBarrier(NodeId nodeId, double distanceMeters)
{
    constexpr size_t MAX_ELEMENT_N = 3;

    double length = 0;
    for (size_t i = 0; i < MAX_ELEMENT_N; ++i) {
        const RoadCross roadCross = getRoadCross(nodeId);

        if (!roadCross.isSimple()) {
            return false;
        }

        const object::RoadElement& element = elementByNodeId(nodeId);

        length += element.fastLength();
        if (length > distanceMeters) {
            return false;
        }

        const object::RoadJunction& junction = endJunction(nodeId);

        const bool barrier = std::any_of(
            junction.conditionIds().begin(), junction.conditionIds().end(),
            [&, this](auto id) {
                const auto it = conditionById_.find(id);

                if (it == conditionById_.end()) {
                    return false;
                }

                const object::Condition& condition = it->second;

                return !!(accessId() & condition.accessId()) // one enough
                    && condition.type() == object::Condition::Type::Barrier
                    && condition.fromElementId() == element.id();
            }
        );

        if (barrier) {
            return true;
        }

        const auto nextNodeId = roadCross.uniqueForwardMove();
        if (nextNodeId and roadCross.isSimple()) {
            nodeId = *nextNodeId;
        }
    }

    return false;
}

bool Graph::hasUturn(NodeId nodeId)
{
    const object::RoadElement& element = elementByNodeId(nodeId);
    if (element.direction() != ymapsdf::rd::Direction::Both) {
        return false;
    }

    const object::RoadJunction& junction = endJunction(nodeId);

    return std::any_of(
        junction.conditionIds().begin(), junction.conditionIds().end(),
        [&, this](auto id) {
            const auto it = conditionById_.find(id);

            if (it == conditionById_.end()) {
                return false;
            }

            const object::Condition& condition = it->second;

            return isSet(accessId(), condition.accessId()) // require all
                && condition.type() == object::Condition::Type::Uturn
                && condition.fromElementId() == element.id();
        }
    );
}

const object::RoadJunction& Graph::endJunction(NodeId nodeId) const
{
    const DirectedId directedId = getDirectedId(nodeId);
    const object::RoadElement& element = elementByDirectedId(directedId);

    return junctionById(
        directedId.direction() == ymapsdf::rd::Direction::Forward
            ? element.endJunctionId()
            : element.startJunctionId()
    );
}

Graph loadGraph(
        object::Loader& loader,
        const geolib3::Point2& location,
        RoadElementFilter filter,
        double areaRadiusMeters)
{
    auto elements = load<object::RoadElement>(loader, inArea(location, areaRadiusMeters), filter);
    auto junctions = load<object::RoadJunction>(loader, collectJunctionIds(elements));
    auto conditions = load<object::Condition>(loader, collectConditionIds(junctions));

    return {ymapsdf::rd::AccessId::Car, elements, junctions, conditions};
}

OptionalNodeId tryPushForwardMove(Path& path, const RoadCross& roadCross)
{
    const auto nextId = roadCross.uniqueForwardMove();
    if (nextId) {
        const bool contains = std::any_of(
            path.begin(), path.end(),
            [=](auto other) { return other == *nextId; }
        );

        if (not contains) {
            path.push_back(*nextId);
            return nextId;
        }
    }

    return std::nullopt;
}

} // namespace maps::mrc::eye
