#include "extended_road_network.h"

#include <maps/libs/common/include/exception.h>
#include <maps/libs/log8/include/log8.h>

#include <queue>
#include <unordered_set>

namespace maps::mrc::gen_targets {

ExtendedRoadNetwork::ExtendedRoadNetwork(Edges edges)
    : RoadNetworkData(std::move(edges))
    , nextUnusedEdgeId_(0)
{
    for (auto& it : edges_) {
        nextUnusedEdgeId_ = std::max(nextUnusedEdgeId_, it.first + 1);
    }

    NodeId nextNodeId = 1;

    // create node on each target edge end point.
    for (const auto& it : edges_) {
        if (it.second.isTarget && !edgeIdToEndNodeId_.count(it.first)) {
            createNode(it.first, nextNodeId++);
            REQUIRE(edgeIdToEndNodeId_.count(it.first), "edge end node was not created");
        }
    }

    // create nodes on the rest targets(targets with deadend start point)
    for (const auto& it : edges_) {
        if (it.second.isTarget
            && !edgeIdToStartNodeId_.count(it.first)) {
            REQUIRE(it.second.inEdges.size(), "graph is not strongly connected");
            createNode(it.second.inEdges[0], nextNodeId++);
            REQUIRE(edgeIdToStartNodeId_.count(it.first), "edge start node was not created");
        }
    }
}

boost::iterator_range<NodesMap::const_iterator> ExtendedRoadNetwork::nodes() const
{
    return boost::make_iterator_range(nodes_.cbegin(), nodes_.cend());
}

const Node& ExtendedRoadNetwork::node(NodeId id) const
{
    auto it = nodes_.find(id);
    REQUIRE(it != nodes_.end(), "Node " << id << " not found");
    return it->second;
}


void ExtendedRoadNetwork::createNode(EdgeId nodeInputEdge, NodeId newNodeId)
{
    Node& newNode = nodes_[newNodeId];
    newNode.id = newNodeId;
    std::queue<EdgeId> inputEdgesQueue;
    std::queue<EdgeId> outputEdgesQueue;
    edgeIdToEndNodeId_[nodeInputEdge] = newNodeId;
    newNode.inEdges.push_back(nodeInputEdge);
    inputEdgesQueue.push(nodeInputEdge);

    while(inputEdgesQueue.size() || outputEdgesQueue.size()) {
        if (inputEdgesQueue.size()) {
            EdgeId curInputEdgeId = inputEdgesQueue.front();
            inputEdgesQueue.pop();
            Edge& curInputEdge = edges_[curInputEdgeId];
            for (EdgeId outEdgeId : curInputEdge.outEdges) {
                if (!edgeIdToStartNodeId_.count(outEdgeId)) {
                    edgeIdToStartNodeId_[outEdgeId] = newNodeId;
                    newNode.outEdges.push_back(outEdgeId);
                    outputEdgesQueue.push(outEdgeId);
                }
            }
        }
        if (outputEdgesQueue.size()) {
            EdgeId curOutputEdgeId = outputEdgesQueue.front();
            outputEdgesQueue.pop();
            Edge& curOutputEdge = edges_[curOutputEdgeId];
            for (EdgeId inEdgeId : curOutputEdge.inEdges) {
                if (!edgeIdToEndNodeId_.count(inEdgeId)) {
                    edgeIdToEndNodeId_[inEdgeId] = newNodeId;
                    newNode.inEdges.push_back(inEdgeId);
                    inputEdgesQueue.push(inEdgeId);
                }
            }
        }
    }
}

void ExtendedRoadNetwork::addEdge(const Edge& edge) {
    REQUIRE(!edges_.count(edge.id), "Can't add edge, edge already exists");
    edges_[edge.id] = edge;
    for (EdgeId inEdgeId : edge.inEdges) {
        REQUIRE(edges_.count(inEdgeId),
                "can't add edge, provided input edge doesn't exist");
        edges_[inEdgeId].outEdges.push_back(edge.id);
        calculateOutputPenalties(edges_[inEdgeId]);
    }
    for (EdgeId outEdgeId : edge.outEdges) {
        REQUIRE(edges_.count(outEdgeId),
                "can't add edge, provided output edge doesn't exist");
        edges_[outEdgeId].inEdges.push_back(edge.id);
    }

    if (edge.outEdges.size() && edgeIdToStartNodeId_.count(edge.outEdges[0])) {
        NodeId nodeId = edgeIdToStartNodeId_[edge.outEdges[0]];
        edgeIdToEndNodeId_[edge.id] = nodeId;
        nodes_[nodeId].inEdges.push_back(edge.id);
    }
    if (edge.inEdges.size() && edgeIdToEndNodeId_.count(edge.inEdges[0])) {
        NodeId nodeId = edgeIdToEndNodeId_[edge.inEdges[0]];
        edgeIdToStartNodeId_[edge.id] = nodeId;
        nodes_[nodeId].outEdges.push_back(edge.id);
    }
    nextUnusedEdgeId_ = std::max(nextUnusedEdgeId_, edge.id + 1);
}

void ExtendedRoadNetwork::connectEdges(EdgeId fromEdge, EdgeId toEdge, Seconds time) {
    REQUIRE(edges_[fromEdge].outEdges.empty() && edges_[toEdge].inEdges.empty(),
            "addEdgesConnection error: edges are not deadend");
    edges_[fromEdge].outEdges.push_back(toEdge);
    edges_[fromEdge].outEdgesPenalties.push_back(0);
    edges_[toEdge].inEdges.push_back(fromEdge);
    edges_[toEdge].time = time;
}

void ExtendedRoadNetwork::disconnectEdges(EdgeId fromEdge, EdgeId toEdge) {
    REQUIRE(edges_[fromEdge].outEdges.size() == 1
            && edges_[fromEdge].outEdges[0] == toEdge
            && edges_[toEdge].inEdges.size() == 1
            && edges_[toEdge].inEdges[0] == fromEdge,
            "removeEdgesConnection error: removing connection that was not"
            " created in ExtendedRoadNetwork");
    edges_[fromEdge].outEdges.pop_back();
    edges_[fromEdge].outEdgesPenalties.pop_back();
    edges_[toEdge].inEdges.pop_back();
    edges_[toEdge].time = 0;
}

Seconds ExtendedRoadNetwork::getUnrecommendedManeuversPenalty(EdgeId from, EdgeId to) const
{
    auto fromEdgeBadOutputs = unrecommendedManeuvers_.find(from);
    if (fromEdgeBadOutputs == unrecommendedManeuvers_.end()) {
        return 0;
    }
    if (std::find(fromEdgeBadOutputs->second.begin(),fromEdgeBadOutputs->second.end(), to)
        == fromEdgeBadOutputs->second.end()) {
        return 0;
    } else {
        return UNRECOMMENDED_MANEUVER_PENALTY;
    }
}

void ExtendedRoadNetwork::setUnrecommendedManeuvers(
        std::unordered_map<EdgeId, std::vector<EdgeId>> unrecommendedManeuvers)
{
    unrecommendedManeuvers_ = std::move(unrecommendedManeuvers);
}


} // namespace maps::mrc::gen_targets
