#include <yandex/maps/wiki/graph/shortest_path.h>

#include <yandex/maps/wiki/graph/common.h>
#include <yandex/maps/wiki/graph/graph.h>

#include <algorithm>
#include <map>
#include <set>
#include <utility>

namespace maps {
namespace wiki {
namespace graph {

namespace {

typedef std::map<NodeID, NodeID> NodeIdMap;
typedef std::map<NodeID, double> NodeIdToWeight;

Path restorePath(const NodeIdMap& previousNodeIds, const NodeID fromNodeId, const NodeID toNodeId)
{
    Path path{toNodeId};
    for (auto nodeId = toNodeId; nodeId != fromNodeId; ) {
        nodeId = previousNodeIds.at(nodeId);
        path.push_back(nodeId);
    }

    std::reverse(path.begin(), path.end());
    return path;
}

class CompareNodeByWeight {

public:
    CompareNodeByWeight(const NodeIdToWeight& weights): weights_(weights) {}

    bool operator()(NodeID lhs, NodeID rhs) const {
        const double lhsWeight = weights_.at(lhs);
        const double rhsWeight = weights_.at(rhs);
        return (lhsWeight < rhsWeight) || (lhsWeight == rhsWeight && lhs < rhs);
    }

private:
    const NodeIdToWeight& weights_;
};

} // namespace

// Variation of Dijkstra’s algorithm
ShortestPath findShortestPath(GetOutEdges outEdges, NodeID fromNodeId, NodePredicate isFinishNode)
{
    NodeIdToWeight weights{{fromNodeId, 0}};
    NodeIdMap previousNodeIds;

    const CompareNodeByWeight compare(weights);
    std::set<NodeID, CompareNodeByWeight> nodeQueue(compare);
    nodeQueue.insert(fromNodeId);

    while (!nodeQueue.empty()) {
        const NodeID nodeId = *nodeQueue.begin();
        nodeQueue.erase(nodeQueue.begin());

        if (isFinishNode(nodeId)) {
            return {
                restorePath(previousNodeIds, fromNodeId, nodeId),
                weights[nodeId]
            };
        }

        for (const auto& edge: outEdges(nodeId)) {
            const double weight = weights[nodeId] + edge.weight();
            auto it = weights.find(edge.endNodeId());
            if (it == weights.end()) {
                weights.insert({edge.endNodeId(), weight});
                previousNodeIds.insert({edge.endNodeId(), nodeId});
                nodeQueue.insert(edge.endNodeId());
            } else if (weight < it->second) {
                nodeQueue.erase(edge.endNodeId());
                it->second = weight;
                nodeQueue.insert(edge.endNodeId());
                previousNodeIds.at(edge.endNodeId()) = nodeId;
            }
        }
    }

    return {Path(), 0.0};
}

ShortestPath findShortestPath(GetOutEdges outEdges, NodeID fromNodeId, NodeID toNodeId)
{
    return findShortestPath(
        std::move(outEdges),
        fromNodeId,
        [&toNodeId](const NodeID nodeId) { return toNodeId == nodeId; }
    );
}

} // namespace graph
} // namespace wiki
} // namespace maps
