/******************************************************************************

 *

 * Project:  GDAL/OGR Geography Network support (Geographic Network Model)

 * Purpose:  GNM graph implementation.

 * Authors:  Mikhail Gusev (gusevmihs at gmail dot com)

 *           Dmitry Baryshnikov, polimax@mail.ru

 *

 ******************************************************************************

 * Copyright (c) 2014, Mikhail Gusev

 * Copyright (c) 2014-2015, NextGIS <info@nextgis.com>

 *

 * Permission is hereby granted, free of charge, to any person obtaining a

 * copy of this software and associated documentation files (the "Software"),

 * to deal in the Software without restriction, including without limitation

 * the rights to use, copy, modify, merge, publish, distribute, sublicense,

 * and/or sell copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following conditions:

 *

 * The above copyright notice and this permission notice shall be included

 * in all copies or substantial portions of the Software.

 *

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS

 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL

 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER

 * DEALINGS IN THE SOFTWARE.

 ****************************************************************************/

#include "gnmgraph.h"

#include "gnm_priv.h"

#include <algorithm>

#include <limits>

#include <set>

//! @cond Doxygen_Suppress

GNMGraph::GNMGraph()

{

}

GNMGraph::~GNMGraph()

{

}

void GNMGraph::AddVertex(GNMGFID nFID)

{

    if (m_mstVertices.find(nFID) != m_mstVertices.end())

        return;

    GNMStdVertex stVertex;

    stVertex.bIsBlocked = false;

    m_mstVertices[nFID] = std::move(stVertex);

}

void GNMGraph::DeleteVertex(GNMGFID nFID)

{

    m_mstVertices.erase(nFID);

    // remove all edges with this vertex

    std::vector<GNMGFID> aoIdsToErase;

    for (std::map<GNMGFID, GNMStdEdge>::iterator it = m_mstEdges.begin();

         it != m_mstEdges.end(); ++it)

    {

        if (it->second.nSrcVertexFID == nFID ||

            it->second.nTgtVertexFID == nFID)

            aoIdsToErase.push_back(it->first);

    }

    for (size_t i = 0; i < aoIdsToErase.size(); i++)

        m_mstEdges.erase(aoIdsToErase[i]);

}

void GNMGraph::AddEdge(GNMGFID nConFID, GNMGFID nSrcFID, GNMGFID nTgtFID,

                       bool bIsBidir, double dfCost, double dfInvCost)

{

    // We do not add edge if an edge with the same id already exist

    // because each edge must have only one source and one target vertex.

    std::map<GNMGFID, GNMStdEdge>::iterator it = m_mstEdges.find(nConFID);

    if (it != m_mstEdges.end())

    {

        CPLError(CE_Failure, CPLE_AppDefined, "The edge already exist.");

        return;

    }

    AddVertex(nSrcFID);

    AddVertex(nTgtFID);

    auto itSrs = m_mstVertices.find(nSrcFID);

    auto itTgt = m_mstVertices.find(nTgtFID);

    if (itSrs == m_mstVertices.end() || itTgt == m_mstVertices.end())

    {

        CPLAssert(false);

        return;

    }

    // Insert edge to the array of edges.

    GNMStdEdge stEdge;

    stEdge.nSrcVertexFID = nSrcFID;

    stEdge.nTgtVertexFID = nTgtFID;

    stEdge.bIsBidir = bIsBidir;

    stEdge.dfDirCost = dfCost;

    stEdge.dfInvCost = dfInvCost;

    stEdge.bIsBlocked = false;

    m_mstEdges[nConFID] = stEdge;

    if (bIsBidir)

    {

        itSrs->second.anOutEdgeFIDs.push_back(nConFID);

        itTgt->second.anOutEdgeFIDs.push_back(nConFID);

    }

    else

    {

        itSrs->second.anOutEdgeFIDs.push_back(nConFID);

    }

}

void GNMGraph::DeleteEdge(GNMGFID nConFID)

{

    m_mstEdges.erase(nConFID);

    // remove edge from all vertices anOutEdgeFIDs

    for (auto &it : m_mstVertices)

    {

        it.second.anOutEdgeFIDs.erase(

            std::remove(it.second.anOutEdgeFIDs.begin(),

                        it.second.anOutEdgeFIDs.end(), nConFID),

            it.second.anOutEdgeFIDs.end());

    }

}

void GNMGraph::ChangeEdge(GNMGFID nFID, double dfCost, double dfInvCost)

{

    std::map<GNMGFID, GNMStdEdge>::iterator it = m_mstEdges.find(nFID);

    if (it != m_mstEdges.end())

    {

        it->second.dfDirCost = dfCost;

        it->second.dfInvCost = dfInvCost;

    }

}

void GNMGraph::ChangeBlockState(GNMGFID nFID, bool bBlock)

{

    // check vertices

    std::map<GNMGFID, GNMStdVertex>::iterator itv = m_mstVertices.find(nFID);

    if (itv != m_mstVertices.end())

    {

        itv->second.bIsBlocked = bBlock;

        return;

    }

    // check edges

    std::map<GNMGFID, GNMStdEdge>::iterator ite = m_mstEdges.find(nFID);

    if (ite != m_mstEdges.end())

    {

        ite->second.bIsBlocked = bBlock;

    }

}

bool GNMGraph::CheckVertexBlocked(GNMGFID nFID) const

{

    std::map<GNMGFID, GNMStdVertex>::const_iterator it =

        m_mstVertices.find(nFID);

    if (it != m_mstVertices.end())

        return it->second.bIsBlocked;

    return false;

}

void GNMGraph::ChangeAllBlockState(bool bBlock)

{

    for (std::map<GNMGFID, GNMStdVertex>::iterator itv = m_mstVertices.begin();

         itv != m_mstVertices.end(); ++itv)

    {

        itv->second.bIsBlocked = bBlock;

    }

    for (std::map<GNMGFID, GNMStdEdge>::iterator ite = m_mstEdges.begin();

         ite != m_mstEdges.end(); ++ite)

    {

        ite->second.bIsBlocked = bBlock;

    }

}

GNMPATH

GNMGraph::DijkstraShortestPath(GNMGFID nStartFID, GNMGFID nEndFID,

                               const std::map<GNMGFID, GNMStdEdge> &mstEdges)

{

    std::map<GNMGFID, GNMGFID> mnShortestTree;

    DijkstraShortestPathTree(nStartFID, mstEdges, mnShortestTree);

    // We search for a path in the resulting tree, starting from end point to

    // start point.

    GNMPATH aoShortestPath;

    GNMGFID nNextVertexId = nEndFID;

    std::map<GNMGFID, GNMGFID>::iterator it;

    EDGEVERTEXPAIR buf;

    while (true)

    {

        it = mnShortestTree.find(nNextVertexId);

        if (it == mnShortestTree.end())

        {

            // We haven't found the start vertex - there is no path between

            // to given vertices in a shortest-path tree.

            break;

        }

        else if (it->first == nStartFID)

        {

            // We've reached the start vertex and return an array.

            aoShortestPath.push_back(std::make_pair(nNextVertexId, -1));

            // Revert array because the first vertex is now the last in path.

            int size = static_cast<int>(aoShortestPath.size());

            for (int i = 0; i < size / 2; ++i)

            {

                buf = aoShortestPath[i];

                aoShortestPath[i] = aoShortestPath[size - i - 1];

                aoShortestPath[size - i - 1] = buf;

            }

            return aoShortestPath;

        }

        else

        {

            // There is only one edge which leads to this vertex, because we

            // analyse a tree. We add this edge with its target vertex into

            // final array.

            aoShortestPath.push_back(std::make_pair(nNextVertexId, it->second));

            // An edge has only two vertices, so we get the opposite one to the

            // current vertex in order to continue search backwards.

            nNextVertexId = GetOppositVertex(it->second, it->first);

        }

    }

    // return empty array

    GNMPATH oRet;

    return oRet;

}

GNMPATH GNMGraph::DijkstraShortestPath(GNMGFID nStartFID, GNMGFID nEndFID)

{

    return DijkstraShortestPath(nStartFID, nEndFID, m_mstEdges);

}

std::vector<GNMPATH> GNMGraph::KShortestPaths(GNMGFID nStartFID,

                                              GNMGFID nEndFID, size_t nK)

{

    // Resulting array with paths.

    // A will be sorted by the path costs' descending.

    std::vector<GNMPATH> A;

    if (nK == 0)

        return A;  // return empty array if K is incorrect.

    // Temporary array for storing paths-candidates.

    // B will be automatically sorted by the cost descending order. We

    // need multimap because there can be physically different paths but

    // with the same costs.

    std::multimap<double, GNMPATH> B;

    // Firstly get the very shortest path.

    // Note, that it is important to obtain the path from DijkstraShortestPath()

    // as vector, rather than the map, because we need the correct order of the

    // path segments in the Yen's algorithm iterations.

    GNMPATH aoFirstPath = DijkstraShortestPath(nStartFID, nEndFID);

    if (aoFirstPath.empty())

        return A;  // return empty array if there is no path between points.

    A.push_back(std::move(aoFirstPath));

    size_t i, k, l;

    GNMPATH::iterator itAk, tempIt, itR;

    std::vector<GNMPATH>::iterator itA;

    std::map<GNMGFID, double>::iterator itDel;

    GNMPATH aoRootPath, aoRootPathOther, aoSpurPath;

    GNMGFID nSpurNode, nVertexToDel, nEdgeToDel;

    double dfSumCost;

    std::map<GNMGFID, GNMStdEdge> mstEdges = m_mstEdges;

    for (k = 0; k < nK - 1; ++k)  // -1 because we have already found one

    {

        std::map<GNMGFID, double>

            mDeletedEdges;  // for infinity costs assignment

        itAk = A[k].begin();

        for (i = 0; i < A[k].size() - 1; ++i)  // avoid end node

        {

            // Get the current node.

            nSpurNode = A[k][i].first;

            // Get the root path from the 0 to the current node.

            // Equivalent to A[k][i]

            // because we will use std::vector::assign, which assigns [..)

            // range, not [..]

            ++itAk;

            aoRootPath.assign(A[k].begin(), itAk);

            // Remove old incidence edges of all other best paths.

            // i.e. if the spur vertex can be reached in already found best

            // paths we must remove the following edge after the end of root

            // path from the graph in order not to take in account these already

            // seen best paths.

            // i.e. it ensures that the spur path will be different.

            for (itA = A.begin(); itA != A.end(); ++itA)

            {

                // check if the number of node exceed the number of last node in

                // the path array (i.e. if one of the A paths has less amount of

                // segments than the current candidate path)

                if (i >= itA->size())

                    continue;

                // + 1, because we will use std::vector::assign, which assigns

                // [..) range, not [..]

                aoRootPathOther.assign(itA->begin(), itA->begin() + i + 1);

                // Get the edge which follows the spur node for current path

                // and delete it.

                //

                // NOTE: we do not delete edges due to performance reasons,

                // because the deletion of edge and all its GFIDs in vertex

                // records is slower than the infinity cost assignment.

                // also check if node number exceed the number of the last node

                // in root array.

                if ((aoRootPath == aoRootPathOther) &&

                    (i < aoRootPathOther.size()))

                {

                    tempIt = itA->begin() + i + 1;

                    mDeletedEdges.insert(std::make_pair(

                        tempIt->second, mstEdges[tempIt->second].dfDirCost));

                    mstEdges[tempIt->second].dfDirCost =

                        std::numeric_limits<double>::infinity();

                }

            }

            // Remove root path nodes from the graph. If we do not delete them

            // the path will be found backwards and some parts of the path will

            // duplicate the parts of old paths.

            // Note: we "delete" all the incidence to the root nodes edges, so

            // to restore them in a common way.

            // end()-1, because we should not remove the spur node

            for (itR = aoRootPath.begin(); itR != aoRootPath.end() - 1; ++itR)

            {

                nVertexToDel = itR->first;

                for (l = 0;

                     l < m_mstVertices[nVertexToDel].anOutEdgeFIDs.size(); ++l)

                {

                    nEdgeToDel = m_mstVertices[nVertexToDel].anOutEdgeFIDs[l];

                    mDeletedEdges.insert(std::make_pair(

                        nEdgeToDel, mstEdges[nEdgeToDel].dfDirCost));

                    mstEdges[nEdgeToDel].dfDirCost =

                        std::numeric_limits<double>::infinity();

                }

            }

            // Find the new best path in the modified graph.

            aoSpurPath = DijkstraShortestPath(nSpurNode, nEndFID, mstEdges);

            // Firstly, restore deleted edges in order to calculate the summary

            // cost of the path correctly later, because the costs will be

            // gathered from the initial graph.

            // We must do it here, after each edge removing, because the later

            // Dijkstra searches must consider these edges.

            for (itDel = mDeletedEdges.begin(); itDel != mDeletedEdges.end();

                 ++itDel)

            {

                mstEdges[itDel->first].dfDirCost = itDel->second;

            }

            mDeletedEdges.clear();

            // If the part of a new best path has been found we form a full one

            // and add it to the candidates array.

            if (!aoSpurPath.empty())

            {

                // + 1 so not to consider the first node in the found path,

                // which is already the last node in the root path

                aoRootPath.insert(aoRootPath.end(), aoSpurPath.begin() + 1,

                                  aoSpurPath.end());

                // Calculate the summary cost of the path.

                // TODO: get the summary cost from the Dejkstra method?

                dfSumCost = 0.0;

                for (itR = aoRootPath.begin(); itR != aoRootPath.end(); ++itR)

                {

                    // TODO: check: Note, that here the current cost can not be

                    // infinity, because every time we assign infinity costs for

                    // edges of old paths, we anyway have the alternative edges

                    // with non-infinity costs.

                    dfSumCost += mstEdges[itR->second].dfDirCost;

                }

                B.insert(std::make_pair(dfSumCost, aoRootPath));

            }

        }

        if (B.empty())

            break;

        // The best path is the first, because the map is sorted accordingly.

        // Note, that here we won't clear the path candidates array and select

        // the best path from all of the rest paths, even from those which were

        // found on previous iterations. That's why we need k iterations at all.

        // Note, that if there were two paths with the same costs and it is the

        // LAST iteration the first occurred path will be added, rather than

        // random.

        A.push_back(B.begin()->second);

        // Sometimes B contains fully duplicate paths. Such duplicates have been

        // formed during the search of alternative for almost the same paths

        // which were already in A.

        // We allowed to add them into B so here we must delete all duplicates.

        while (!B.empty() && B.begin()->second == A.back())

        {

            B.erase(B.begin());

        }

    }

    return A;

}

GNMPATH GNMGraph::ConnectedComponents(const GNMVECTOR &anEmittersIDs)

{

    GNMPATH anConnectedIDs;

    if (anEmittersIDs.empty())

    {

        CPLError(CE_Failure, CPLE_IllegalArg, "Emitters list is empty.");

        return anConnectedIDs;

    }

    std::set<GNMGFID> anMarkedVertIDs;

    std::queue<GNMGFID> anStartQueue;

    GNMVECTOR::const_iterator it;

    for (it = anEmittersIDs.begin(); it != anEmittersIDs.end(); ++it)

    {

        anStartQueue.push(*it);

    }

    // Begin the iterations of the Breadth-first search.

    TraceTargets(anStartQueue, anMarkedVertIDs, anConnectedIDs);

    return anConnectedIDs;

}

void GNMGraph::Clear()

{

    m_mstVertices.clear();

    m_mstEdges.clear();

}

void GNMGraph::DijkstraShortestPathTree(

    GNMGFID nFID, const std::map<GNMGFID, GNMStdEdge> &mstEdges,

    std::map<GNMGFID, GNMGFID> &mnPathTree)

{

    // Initialize all vertices in graph with infinity mark.

    double dfInfinity = std::numeric_limits<double>::infinity();

    std::map<GNMGFID, double> mMarks;

    std::map<GNMGFID, GNMStdVertex>::iterator itv;

    for (itv = m_mstVertices.begin(); itv != m_mstVertices.end(); ++itv)

    {

        mMarks[itv->first] = dfInfinity;

    }

    mMarks[nFID] = 0.0;

    mnPathTree[nFID] = -1;

    // Initialize all vertices as unseen (there are no seen vertices).

    std::set<GNMGFID> snSeen;

    // We use multimap to maintain the ascending order of costs and because

    // there can be different vertices with the equal cost.

    std::multimap<double, GNMGFID> to_see;

    std::multimap<double, GNMGFID>::iterator it;

    to_see.insert(std::pair<double, GNMGFID>(0.0, nFID));

    LPGNMCONSTVECTOR panOutcomeEdgeId;

    size_t i;

    GNMGFID nCurrentVertId, nCurrentEdgeId, nTargetVertId;

    double dfCurrentEdgeCost, dfCurrentVertMark, dfNewVertexMark;

    std::map<GNMGFID, GNMStdEdge>::const_iterator ite;

    // Continue iterations while there are some vertices to see.

    while (!to_see.empty())

    {

        // We must see vertices with minimal costs at first.

        // In multimap the first cost is the minimal.

        it = to_see.begin();

        nCurrentVertId = it->second;

        dfCurrentVertMark = it->first;

        snSeen.insert(it->second);

        to_see.erase(it);

        // For all neighbours for the current vertex.

        panOutcomeEdgeId = GetOutEdges(nCurrentVertId);

        if (nullptr == panOutcomeEdgeId)

            continue;

        for (i = 0; i < panOutcomeEdgeId->size(); ++i)

        {

            nCurrentEdgeId = panOutcomeEdgeId->operator[](i);

            ite = mstEdges.find(nCurrentEdgeId);

            if (ite == mstEdges.end() || ite->second.bIsBlocked)

                continue;

            // We go in any edge from source to target so we take only

            // direct cost (even if an edge is bi-directed).

            dfCurrentEdgeCost = ite->second.dfDirCost;

            // While we see outcome edges of current vertex id we definitely

            // know that target vertex id will be target for current edge id.

            nTargetVertId = GetOppositVertex(nCurrentEdgeId, nCurrentVertId);

            // Calculate a new mark assuming the full path cost (mark of the

            // current vertex) to this vertex.

            dfNewVertexMark = dfCurrentVertMark + dfCurrentEdgeCost;

            // Update mark of the vertex if needed.

            if (snSeen.find(nTargetVertId) == snSeen.end() &&

                dfNewVertexMark < mMarks[nTargetVertId] &&

                !CheckVertexBlocked(nTargetVertId))

            {

                mMarks[nTargetVertId] = dfNewVertexMark;

                mnPathTree[nTargetVertId] = nCurrentEdgeId;

                // The vertex with minimal cost will be inserted to the

                // beginning.

                to_see.insert(

                    std::pair<double, GNMGFID>(dfNewVertexMark, nTargetVertId));

            }

        }

    }

}

LPGNMCONSTVECTOR GNMGraph::GetOutEdges(GNMGFID nFID) const

{

    std::map<GNMGFID, GNMStdVertex>::const_iterator it =

        m_mstVertices.find(nFID);

    if (it != m_mstVertices.end())

        return &it->second.anOutEdgeFIDs;

    return nullptr;

}

GNMGFID GNMGraph::GetOppositVertex(GNMGFID nEdgeFID, GNMGFID nVertexFID) const

{

    std::map<GNMGFID, GNMStdEdge>::const_iterator it =

        m_mstEdges.find(nEdgeFID);

    if (it != m_mstEdges.end())

    {

        if (nVertexFID == it->second.nSrcVertexFID)

        {

            return it->second.nTgtVertexFID;

        }

        else if (nVertexFID == it->second.nTgtVertexFID)

        {

            return it->second.nSrcVertexFID;

        }

    }

    return -1;

}

void GNMGraph::TraceTargets(std::queue<GNMGFID> &vertexQueue,

                            std::set<GNMGFID> &markedVertIds,

                            GNMPATH &connectedIds)

{

    std::queue<GNMGFID> neighbours_queue;

    // See all given vertices except thouse that have been already seen.

    while (!vertexQueue.empty())

    {

        GNMGFID nCurVertID = vertexQueue.front();

        // There may be duplicate unmarked vertices in a current queue. Check

        // it.

        if (markedVertIds.find(nCurVertID) == markedVertIds.end())

        {

            markedVertIds.insert(nCurVertID);

            // See all outcome edges, add them to connected and than see the

            // target vertex of each edge. Add it to the queue, which will be

            // recursively seen the same way on the next iteration.

            LPGNMCONSTVECTOR panOutcomeEdgeIDs = GetOutEdges(nCurVertID);

            if (nullptr != panOutcomeEdgeIDs)

            {

                for (const GNMGFID nCurEdgeID : *panOutcomeEdgeIDs)

                {

                    // ISSUE: think about to return a sequence of vertices and

                    // edges (which is more universal), as now we are going to

                    // return only sequence of edges.

                    connectedIds.push_back(

                        std::make_pair(nCurVertID, nCurEdgeID));

                    // Get the only target vertex of this edge. If edge is

                    // bidirected get not that vertex that with nCurVertID.

                    GNMGFID nTargetVertID;

                    /*

                    std::vector<GNMGFID> target_vert_ids =

                    _getTgtVert(cur_edge_id); std::vector<GNMGFID>::iterator

                    itt; for (itt = target_vert_ids.begin(); itt !=

                    target_vert_ids.end(); ++itt)

                    {

                        if ((*itt) != cur_vert_id)

                        {

                            target_vert_id = *itt;

                            break;

                        }

                    }

                    */

                    nTargetVertID = GetOppositVertex(nCurEdgeID, nCurVertID);

                    // Avoid marked or blocked vertices.

                    if ((markedVertIds.find(nTargetVertID) ==

                         markedVertIds.end()) &&

                        (!CheckVertexBlocked(nTargetVertID)))

                        neighbours_queue.push(nTargetVertID);

                }

            }

        }

        vertexQueue.pop();

    }

    if (!neighbours_queue.empty())

        TraceTargets(neighbours_queue, markedVertIds, connectedIds);

}

//! @endcond