//==================================================================================================
// G r a p h _ p r o b l e m                                                              Interface
// S h o r t e s t _ p a t h
//                                                                                By Bruno Bachelet
//==================================================================================================
// Copyright (c) 1999-2016
// Bruno Bachelet - bruno@nawouak.net - http://www.nawouak.net
//
// This file is part of the B++ Library. This library is free software; you can redistribute it
// and/or modify it under the terms of the GNU Library General Public License as published by the
// Free Software Foundation; either version 2 of the License, or (at your option) any later
// version.
//
// This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
// the GNU Library General Public License for more details (http://www.gnu.org).

/*DESCRIPTION*/
/* This module provides facilities to solve shortest path problems in graphs. */

// File Name //-------------------------------------------------------------------------------------
#line __LINE__ "graph_problem/shortest_path.hpp"

// Guardian //--------------------------------------------------------------------------------------
#ifndef guGraphProblemShortestPath
#define guGraphProblemShortestPath

// Headers //---------------------------------------------------------------------------------------
#include <algorithm> /*INCLUDE*/
#include <queue> /*INCLUDE*/
#include <bpp/graph.hpp> /*INCLUDE*/

namespace bpp {

// Importation/Exportation //-----------------------------------------------------------------------
#ifdef GRAPH_PROBLEM_DLL
 #define dll_export DLL_EXPORT
#else
 #define dll_export DLL_IMPORT
#endif

// Namespaces //------------------------------------------------------------------------------------
#define public_area  graphProblemShortestPath
#define private_area graphProblemShortestPath_private

namespace public_area  { /*NAMESPACE*/ using namespace graph; }
namespace private_area { using namespace public_area; }

extern_module_name;

// Initialization //--------------------------------------------------------------------------------
#define iniGraphProblemShortestPath
has_initializer;

// Macrocommands //---------------------------------------------------------------------------------
/*ALIAS*/
#define tdGraph class prArcData,class prNodeData //

/*ALIAS*/
#define tuGraph prArcData,prNodeData //

// Types & Classes //-------------------------------------------------------------------------------
namespace public_area {
 //------------------------------------------------------------------------------------------Classes
 template <tdGraph> class clBellmanAlgo;
 template <tdGraph> class clDijkstraAlgo;
 template <tdGraph> class clFordAlgo;
 template <tdGraph> class clSolveAlgo;
 //-----------------------------------------------------------------------------------Variable Types
 /*TYPE*/ /* Arc of a graph with a path problem. */
 typedef clArc<clLengthData,clNoData> clLengthArc;

 /*TYPE*/ /* Graph with a path problem. */
 typedef clGraph<clLengthData,clNoData> clLengthGraph;

 /*TYPE*/ /* Node of a graph with a path problem. */
 typedef clNode<clLengthData,clNoData> clLengthNode;
 //-----------------------------------------------------------------------------------Constant Types
 typedef const clLengthArc   ctLengthArc;
 typedef const clLengthGraph ctLengthGraph;
 typedef const clLengthNode  ctLengthNode;
}

namespace private_area {
 template <tdGraph> class clNodeInfo1;
 template <tdGraph> class clNodeInfo2;
}

// Functions Interface //---------------------------------------------------------------------------
namespace public_area {
 template <tdGraph>
 void computeAllShortestDistance(const clGraph<tuGraph> &,
                                 std_map(std_pair(clNode<tuGraph> *,clNode<tuGraph> *),tyReal) &);

 template <tdGraph> tyReal length(const std_vector(clArc<tuGraph> *) &);
}

namespace private_area { testing_mode ( function void test(void); ) }

// Errors //----------------------------------------------------------------------------------------
namespace public_area {
 /*ERROR*/ extern_error erNegativeArcLength; /* The length of an arc is negative. */
 /*ERROR*/ extern_error erNegativeCycle; /* A cycle is negative. */
}

// Constants & Variables //-------------------------------------------------------------------------
extern_dynamic_constant(private,clString,goDataLocation,?);

// B e l l m a n A l g o  Interface //--------------------------------------------------------------
namespace public_area {
 /*CLASS clBellmanAlgo */
 /* Represents Bellman's algorithm to solve the shortest path problem between two nodes in a graph.
    The graph must have no directed cycle. */
 template <tdGraph> class clBellmanAlgo : public clSolveAlgo<tuGraph> {
  //-----------------------------------------------------------------------------------------Private
  private_property constructor clBellmanAlgo(const clBellmanAlgo &);
  private_property clBellmanAlgo & operator = (const clBellmanAlgo &);
  //------------------------------------------------------------------------------------------Public
  public_property constructor clBellmanAlgo(void);
  public_property virtual destructor clBellmanAlgo(void);

  public_property void run(clGraph<tuGraph> &,tyNodeKey,tyNodeKey,
                           std_vector(clArc<tuGraph> *) &) const;
 };
}

// D i j k s t r a A l g o  Interface //------------------------------------------------------------
namespace public_area {
 /*CLASS clDijkstraAlgo */
 /* Represents Dijkstra's algorithm to solve the shortest path problem between two nodes in a
    graph. The arc lengths must be positive or zero. */
 template <tdGraph> class clDijkstraAlgo : public clSolveAlgo<tuGraph> {
  //-----------------------------------------------------------------------------------------Private
  private_property constructor clDijkstraAlgo(const clDijkstraAlgo &);
  private_property clDijkstraAlgo & operator = (const clDijkstraAlgo &);
  //------------------------------------------------------------------------------------------Public
  public_property constructor clDijkstraAlgo(void);
  public_property virtual destructor clDijkstraAlgo(void);

  public_property void run(clGraph<tuGraph> &,tyNodeKey,tyNodeKey,
                           std_vector(clArc<tuGraph> *) &) const;
 };
}

// F o r d A l g o  Interface //--------------------------------------------------------------------
namespace public_area {
 /*CLASS clFordAlgo */
 /* Represents Bellman & Ford's algorithm to solve the shortest path problem between two nodes
    in a graph. The graph must have no negative cycle. */
 template <tdGraph> class clFordAlgo : public clSolveAlgo<tuGraph> {
  //------------------------------------------------------------------------------------------Public
  /*TYPE clFordAlgo */ /* Type of the information in the workspace of each node. */
  public_property typedef private_area::clNodeInfo2<tuGraph> cpWorkspace;
  //-----------------------------------------------------------------------------------------Private
  private_property constructor clFordAlgo(const clFordAlgo &);
  private_property clFordAlgo & operator = (const clFordAlgo &);
  //------------------------------------------------------------------------------------------Public
  /*ATTRIBUTE clFordAlgo */
  /* Indicates if the algorithm must keep its workspace (i.e. the potentials of the nodes)
     available for further use after its execution. */
  read_only_attribute(tyBoolean,atKeepWorkspace,keepWorkspace);

  public_property constructor clFordAlgo(void);
  public_property constructor clFordAlgo(tyBoolean);
  public_property virtual destructor clFordAlgo(void);

  public_property void run(clGraph<tuGraph> &,tyNodeKey,tyNodeKey,
                           std_vector(clArc<tuGraph> *) &) const;
 };
}

// S o l v e A l g o  Interface //------------------------------------------------------------------
namespace public_area {
 /*CLASS clSolveAlgo */
 /* Represents an algorithm to solve the shortest path problem between two nodes in a graph. It is
    an abstract class. */
 template <tdGraph> class clSolveAlgo {
  //-----------------------------------------------------------------------------------------Private
  private_property constructor clSolveAlgo(const clSolveAlgo &);
  private_property clSolveAlgo & operator = (const clSolveAlgo &);
  //------------------------------------------------------------------------------------------Public
  public_property constructor clSolveAlgo(void);
  public_property virtual destructor clSolveAlgo(void);

  /*AMETHOD clSolveAlgo */ /* Executes the algorithm. Abstract method. */
  public_property virtual void run(clGraph<tuGraph> & agGraph,tyNodeKey agSourceKey,
                                   tyNodeKey agTargetKey,
                                   std_vector(clArc<tuGraph> *) & agPath) const = 0;

  public_property static void defaultRun(clGraph<tuGraph> &,tyNodeKey,tyNodeKey,
                                         std_vector(clArc<tuGraph> *) &);
 };
}

// N o d e I n f o 1  Interface //------------------------------------------------------------------
namespace private_area {
 template <tdGraph> class clNodeInfo1 {
  //-------------------------------------------------------------------------------------------Types
  private_property typedef clArc<tuGraph> * cpArc;
  //------------------------------------------------------------------------------------------Public
  read_write_attribute(tyCardinal,atIncoming,incoming);
  read_write_attribute(tyReal,atPotential,potential);
  read_write_attribute(cpArc,atPredecessor,predecessor);

  public_property constructor clNodeInfo1(void);
  public_property constructor clNodeInfo1(tyCardinal,tyReal,cpArc);
  public_property destructor clNodeInfo1(void) {}
 };
}

// N o d e I n f o 2  Interface //------------------------------------------------------------------
namespace private_area {
 template <tdGraph> class clNodeInfo2 {
  //-------------------------------------------------------------------------------------------Types
  private_property typedef clArc<tuGraph> * cpArc;
  //------------------------------------------------------------------------------------------Public
  read_write_attribute(tyBoolean,atWaiting,waiting);
  read_write_attribute(tyReal,atPotential,potential);
  read_write_attribute(cpArc,atPredecessor,predecessor);

  public_property constructor clNodeInfo2(void);
  public_property constructor clNodeInfo2(tyBoolean,tyReal,cpArc);
  public_property destructor clNodeInfo2(void) {}
 };
}

// Functions Inline //------------------------------------------------------------------------------
namespace public_area  {}
namespace private_area {}

// B e l l m a n A l g o  Inline //-----------------------------------------------------------------
namespace public_area {
 //--------------------------------------------------------------------------------------Constructor
 /*METHOD clBellmanAlgo */
 /* Builds Bellman's algorithm that solves the shortest path problem between two nodes in a
    graph. */
 template <tdGraph> inline clBellmanAlgo<tuGraph>::clBellmanAlgo(void) {}
 //---------------------------------------------------------------------------------------Destructor
 /*METHOD clBellmanAlgo */ /* Destructs the algorithm. */
 template <tdGraph> inline clBellmanAlgo<tuGraph>::~clBellmanAlgo(void) {}
}

// D i j k s t r a A l g o  Inline //---------------------------------------------------------------
namespace public_area {
 //--------------------------------------------------------------------------------------Constructor
 /*METHOD clDijkstraAlgo */
 /* Builds Dijkstra's algorithm that solves the shortest path problem between two nodes in a
    graph. */
 template <tdGraph> inline clDijkstraAlgo<tuGraph>::clDijkstraAlgo(void) {}
 //---------------------------------------------------------------------------------------Destructor
 /*METHOD clDijkstraAlgo */ /* Destructs the algorithm. */
 template <tdGraph> inline clDijkstraAlgo<tuGraph>::~clDijkstraAlgo(void) {}
}

// F o r d A l g o  Inline //-----------------------------------------------------------------------
namespace public_area {
 //--------------------------------------------------------------------------------------Constructor
 /*METHOD clFordAlgo */
 /* Builds Bellman & Ford's algorithm that solves the shortest path problem between two nodes in
    a graph. */
 template <tdGraph> inline clFordAlgo<tuGraph>::clFordAlgo(void) : atKeepWorkspace(false) {}
 //--------------------------------------------------------------------------------------Constructor
 /*METHOD clFordAlgo */
 /* Builds Bellman & Ford's algorithm that solves the shortest path problem between two nodes in
    a graph. If the argument of the constructor is set to <CODE>true</CODE>, the workspace of the
    algorithm (i.e. the potentials on the nodes) is kept for further use. */
 template <tdGraph> inline clFordAlgo<tuGraph>::clFordAlgo(tyBoolean agKeepWorkspace)
 : atKeepWorkspace(agKeepWorkspace) {}
 //---------------------------------------------------------------------------------------Destructor
 /*METHOD clFordAlgo */ /* Destructs the algorithm. */
 template <tdGraph> inline clFordAlgo<tuGraph>::~clFordAlgo(void) {}
}

// S o l v e A l g o  Inline //---------------------------------------------------------------------
namespace public_area {
 //--------------------------------------------------------------------------------------Constructor
 /*METHOD clSolveAlgo */
 /* Builds an algorithm to solve the shortest path problem between two nodes in a graph. */
 template <tdGraph> inline clSolveAlgo<tuGraph>::clSolveAlgo(void) {}
 //---------------------------------------------------------------------------------------Destructor
 /*METHOD clSolveAlgo */ /* Destructs the algorithm. */
 template <tdGraph> inline clSolveAlgo<tuGraph>::~clSolveAlgo(void) {}
}

// N o d e I n f o 1  Inline //---------------------------------------------------------------------
namespace private_area {
 //--------------------------------------------------------------------------------------Constructor
 template <tdGraph> inline
 clNodeInfo1<tuGraph>::clNodeInfo1(void) : atIncoming(0),atPotential(0.0),atPredecessor(nil) {}
 //--------------------------------------------------------------------------------------Constructor
 template <tdGraph> inline
 clNodeInfo1<tuGraph>::clNodeInfo1(tyCardinal agIncoming,tyReal agPotential,
                                   clArc<tuGraph> * agPredecessor)
 : atIncoming(agIncoming),atPotential(agPotential),atPredecessor(agPredecessor) {}
}

// N o d e I n f o 2  Inline //---------------------------------------------------------------------
namespace private_area {
 //--------------------------------------------------------------------------------------Constructor
 template <tdGraph> inline
 clNodeInfo2<tuGraph>::clNodeInfo2(void) : atWaiting(false),atPotential(0.0),atPredecessor(nil) {}
 //--------------------------------------------------------------------------------------Constructor
 template <tdGraph> inline
 clNodeInfo2<tuGraph>::clNodeInfo2(tyBoolean agWaiting,tyReal agPotential,
                                   clArc<tuGraph> * agPredecessor)
 : atWaiting(agWaiting),atPotential(agPotential),atPredecessor(agPredecessor) {}
}

// Functions Implementation //----------------------------------------------------------------------
namespace public_area {
 //-----------------------------------------------------------------------ComputeAllShortestDistance
 /*FUNCTION*/
 /* Computes the shortest distance between all pairs of nodes in a graph, using Dantzig's
    algorithm. */
 template <tdGraph> void computeAllShortestDistance(const clGraph<tuGraph> & agGraph,
                    std_map(std_pair(clNode<tuGraph> *,clNode<tuGraph> *),tyReal) & agDistanceX) {
  typedef clArc<tuGraph>              cpArc;
  typedef clNode<tuGraph>             cpNode;
  typedef std_pair(cpNode *,cpNode *) cpNodePair;

  typedef typename std_map(cpNodePair,tyReal)::value_type    cpDistancePair;
  typedef typename cpNode::cpArcX::const_iterator            cpArcIterator;
  typedef typename clGraph<tuGraph>::cpNodeX::const_iterator cpNodeIterator;

  cpArc *        lcArc;
  cpArcIterator  lcCurrentArc;
  cpNodeIterator lcCurrentNode1;
  cpNodeIterator lcCurrentNode2;
  cpNodeIterator lcCurrentNode3;
  cpArcIterator  lcLastArc;
  cpNodeIterator lcLastNode;
  tyReal         lcMin;
  cpNode *       lcNode1;
  cpNode *       lcNode2;
  cpNode *       lcNode3;
  tyReal         lcValue1;
  tyReal *       lcValue2;

  // Initialization //
  lcCurrentNode1=agGraph.nodes().begin();
  lcLastNode=agGraph.nodes().end();

  while (lcCurrentNode1!=lcLastNode) {
   lcNode1=(*lcCurrentNode1).second;
   lcCurrentNode2=agGraph.nodes().begin();

   while (lcCurrentNode2!=lcLastNode) {
    lcNode2=(*lcCurrentNode2).second;

    if (lcNode1==lcNode2)
     agDistanceX.insert(cpDistancePair(cpNodePair(lcNode1,lcNode1),0.0));
    else
     agDistanceX.insert(cpDistancePair(cpNodePair(lcNode1,lcNode2),realMax()));

    lcCurrentNode2++;
   }

   lcCurrentNode1++;
  }

  // Shortest Distances Computation //
  lcCurrentNode1=agGraph.nodes().begin();

  while (lcCurrentNode1!=lcLastNode) {
   lcNode1=(*lcCurrentNode1).second;
   lcCurrentNode2=agGraph.nodes().begin();

   // Incoming Arcs //
   while (lcCurrentNode2!=lcCurrentNode1) {
    lcNode2=(*lcCurrentNode2).second;
    lcMin=realMax();
    lcCurrentArc=lcNode1->outgoingArcs().begin();
    lcLastArc=lcNode1->outgoingArcs().end();

    while (lcCurrentArc!=lcLastArc) {
     lcArc=(*lcCurrentArc).second;
     lcValue1=lcArc->data().length()+agDistanceX[cpNodePair(lcArc->targetNode(),lcNode2)];
     if (lcValue1<lcMin) lcMin=lcValue1;
     lcCurrentArc++;
    }

    agDistanceX[cpNodePair(lcNode1,lcNode2)]=lcMin;
    lcCurrentNode2++;
   }

   // Outgoing Arcs //
   lcCurrentNode2=agGraph.nodes().begin();

   while (lcCurrentNode2!=lcCurrentNode1) {
    lcNode2=(*lcCurrentNode2).second;
    lcMin=realMax();
    lcCurrentArc=lcNode1->incomingArcs().begin();
    lcLastArc=lcNode1->incomingArcs().end();

    while (lcCurrentArc!=lcLastArc) {
     lcArc=(*lcCurrentArc).second;
     lcValue1=agDistanceX[cpNodePair(lcNode2,lcArc->sourceNode())]+lcArc->data().length();
     if (lcValue1<lcMin) lcMin=lcValue1;
     lcCurrentArc++;
    }

    agDistanceX[cpNodePair(lcNode2,lcNode1)]=lcMin;
    lcCurrentNode2++;
   }

   // All Pairs //
   lcCurrentNode2=agGraph.nodes().begin();

   while (lcCurrentNode2!=lcCurrentNode1) {
    lcNode2=(*lcCurrentNode2).second;
    lcCurrentNode3=agGraph.nodes().begin();

    while (lcCurrentNode3!=lcCurrentNode1) {
     lcNode3=(*lcCurrentNode3).second;
     lcValue1=agDistanceX[cpNodePair(lcNode2,lcNode1)];
     if (lcValue1!=realMax()) lcValue1+=agDistanceX[cpNodePair(lcNode1,lcNode3)];
     lcValue2=&(agDistanceX[cpNodePair(lcNode2,lcNode3)]);
     if (*lcValue2>lcValue1) *lcValue2=lcValue1;
     lcCurrentNode3++;
    }

    lcCurrentNode2++;
   }

   lcCurrentNode1++;
  }
 }
 //-------------------------------------------------------------------------------------------Length
 /*FUNCTION*/ /* Returns the length of a path. */
 template <tdGraph> tyReal length(const std_vector(clArc<tuGraph> *) & agPath) {
  typedef typename std_vector(clArc<tuGraph> *)::const_iterator cpArcIterator;

  cpArcIterator lcCurrentArc = agPath .begin();
  cpArcIterator lcLastArc    = agPath.end();
  tyReal        lcLength     = 0.0;

  if (agPath.size()==0) return (realMax());

  while (lcCurrentArc!=lcLastArc) {
   lcLength+=(*lcCurrentArc)->data().length();
   lcCurrentArc++;
  }

  return (lcLength);
 }
}

namespace private_area {}

// B e l l m a n A l g o  Implementation //---------------------------------------------------------
namespace public_area {
 //----------------------------------------------------------------------------------------------Run
 /*METHOD clBellmanAlgo */ /* Solves the shortest path problem between two nodes in a graph. */
 template <tdGraph>
 void clBellmanAlgo<tuGraph>::run(clGraph<tuGraph> & agGraph,tyNodeKey agSourceKey,
                                  tyNodeKey agTargetKey,
                                  std_vector(clArc<tuGraph> *) & agPath) const {
  typedef clArc<tuGraph>                     cpArc;
  typedef clNode<tuGraph>                    cpNode;
  typedef std_vector(cpNode *)               cpNodeS;
  typedef private_area::clNodeInfo1<tuGraph> cpInfo;

  typedef typename cpNode::cpArcX::const_iterator            cpArcIterator;
  typedef typename clGraph<tuGraph>::cpNodeX::const_iterator cpNodeIterator;

  tyMark    lcMark       = ++(agGraph.mark());
  cpNode *  lcSourceNode = &(agGraph.node(agSourceKey));
  cpNode *  lcTargetNode = &(agGraph.node(agTargetKey));

  cpArc *        lcArc;
  cpArcIterator  lcCurrentArc;
  cpNodeIterator lcCurrentNode;
  cpInfo *       lcInfo;
  cpArcIterator  lcLastArc;
  cpNodeIterator lcLastNode;
  cpNode *       lcNode1;
  cpNode *       lcNode2;
  cpNodeS        lcNodeS;
  tyReal         lcPotential;

  // Initialization //
  lcCurrentNode=agGraph.nodes().begin();
  lcLastNode=agGraph.nodes().end();

  while (lcCurrentNode!=lcLastNode) {
   lcNode1=(*lcCurrentNode).second;

   if (lcNode1==lcSourceNode) {
    lcNode1->work()=new_object(cpInfo(0,0.0,nil));
    lcNodeS.push_back(lcNode1);
    lcNode1->mark()=lcMark;
   }
   else {
    lcNode1->work()=new_object(cpInfo(lcNode1->incomingArcs().size(),realMax(),nil));

    if (lcNode1->incomingArcs().size()==0) {
     lcNodeS.push_back(lcNode1);
     lcNode1->mark()=lcMark;
    }
   }

   lcCurrentNode++;
  }

  // Path Search //
  while (lcNodeS.size()>0 and lcTargetNode->mark()<lcMark) {
   lcNode1=lcNodeS.back();
   lcNodeS.pop_back();
   lcPotential=static_cast<cpInfo *>(lcNode1->work())->potential();
   lcCurrentArc=lcNode1->outgoingArcs().begin();
   lcLastArc=lcNode1->outgoingArcs().end();

   while (lcCurrentArc!=lcLastArc) {
    lcArc=(*lcCurrentArc).second;
    lcNode2=lcArc->targetNode();
    lcInfo=static_cast<cpInfo *>(lcNode2->work());

    if (lcInfo->potential() > lcPotential+lcArc->data().length()) {
     lcInfo->potential()=lcPotential+lcArc->data().length();
     lcInfo->predecessor()=lcArc;
    }

    if (lcInfo->incoming()==1) {
     lcNodeS.push_back(lcNode2);
     lcNode2->mark()=lcMark;
    }

    lcInfo->incoming()--;
    lcCurrentArc++;
   }
  }

  // Path Construction //
  agPath.erase(agPath.begin(),agPath.end());

  if (lcTargetNode->mark()==lcMark) {
   lcArc=static_cast<cpInfo *>(lcTargetNode->work())->predecessor();

   while (lcArc!=nil) {
    agPath.push_back(lcArc);
    lcArc=static_cast<cpInfo *>(lcArc->sourceNode()->work())->predecessor();
   }
  }

  deleteNodeWorkspace(agGraph,cpInfo());
 }
}

// D i j k s t r a A l g o  Implementation //-------------------------------------------------------
namespace public_area {
 //----------------------------------------------------------------------------------------------Run
 /*METHOD clDijkstraAlgo */ /* Solves the shortest path problem between two nodes in a graph. */
 template <tdGraph>
 void clDijkstraAlgo<tuGraph>::run(clGraph<tuGraph> & agGraph,tyNodeKey agSourceKey,
                                   tyNodeKey agTargetKey,
                                   std_vector(clArc<tuGraph> *) & agPath) const {
  method_name("dijkstraAlgo::run");

  typedef clArc<tuGraph>                     cpArc;
  typedef private_area::clNodeInfo1<tuGraph> cpInfo;
  typedef clNode<tuGraph>                    cpNode;
  typedef std_multimap(tyReal,cpNode *)      cpNodeX;

  typedef typename cpNodeX::value_type            cpNodePair;
  typedef typename cpNode::cpArcX::const_iterator cpArcIterator;
  typedef typename cpNodeX::iterator              cpNodeIterator;

  tyMark   lcMark       = ++(agGraph.mark());
  cpNode * lcSourceNode = &(agGraph.node(agSourceKey));
  cpNode * lcTargetNode = &(agGraph.node(agTargetKey));

  cpArc *        lcArc;
  cpArcIterator  lcCurrentArc;
  cpInfo *       lcInfo;
  cpNodeIterator lcIterator;
  cpArcIterator  lcLastArc;
  cpNode *       lcNode1;
  cpNode *       lcNode2;
  cpNodeX        lcNodeX;
  tyReal         lcPotential1;
  tyReal         lcPotential2;

  // Path Search //
  lcNodeX.insert(cpNodePair(0.0,lcSourceNode));
  lcSourceNode->work()=new_object(cpInfo(0,0.0,nil));
  lcSourceNode->mark()=lcMark;

  while (lcNodeX.size()>0) {
   lcNode1=(*(lcNodeX.begin())).second;

   if (lcNode1==lcTargetNode) lcNodeX.erase(lcNodeX.begin(),lcNodeX.end());
   else {
    lcNodeX.erase(lcNodeX.begin());
    lcPotential1=static_cast<cpInfo *>(lcNode1->work())->potential();
    lcCurrentArc=lcNode1->outgoingArcs().begin();
    lcLastArc=lcNode1->outgoingArcs().end();

    while (lcCurrentArc!=lcLastArc) {
     lcArc=(*lcCurrentArc).second;
     lcNode2=lcArc->targetNode();
     if (lcArc->data().length()<0) send_error(erNegativeArcLength);

     if (lcNode2->mark()<lcMark) {
      lcPotential2=lcPotential1+lcArc->data().length();
      lcNodeX.insert(cpNodePair(lcPotential2,lcNode2));
      lcNode2->work()=new_object(cpInfo(0,lcPotential2,lcArc));
      lcNode2->mark()=lcMark;
     }
     else {
      lcInfo=static_cast<cpInfo *>(lcNode2->work());
      lcPotential2=lcInfo->potential();

      if (lcPotential2 > lcPotential1+lcArc->data().length()) {
       lcIterator=std_find(lcNodeX.lower_bound(lcPotential2),lcNodeX.upper_bound(lcPotential2),
                           cpNodePair(lcPotential2,lcNode2));

       lcNodeX.erase(lcIterator);
       lcPotential2=lcPotential1+lcArc->data().length();
       lcInfo->potential()=lcPotential2;
       lcInfo->predecessor()=lcArc;
       lcNodeX.insert(cpNodePair(lcPotential2,lcNode2));
      }
     }

     lcCurrentArc++;
    }
   }
  }

  // Path Construction //
  agPath.erase(agPath.begin(),agPath.end());

  if (lcTargetNode->mark()==lcMark) {
   lcArc=static_cast<cpInfo *>(lcTargetNode->work())->predecessor();

   while (lcArc!=nil) {
    agPath.push_back(lcArc);
    lcArc=static_cast<cpInfo *>(lcArc->sourceNode()->work())->predecessor();
   }
  }

  deleteNodeWorkspace(agGraph,cpInfo());
 }
}

// F o r d A l g o  Implementation //---------------------------------------------------------------
namespace public_area {
 //----------------------------------------------------------------------------------------------Run
 /*METHOD clFordAlgo */
 /* Solves the shortest path problem between two nodes in a graph. If there is a negative length
    cycle, an exception is thrown. */
 template <tdGraph>
 void clFordAlgo<tuGraph>::run(clGraph<tuGraph> & agGraph,tyNodeKey agSourceKey,
                               tyNodeKey agTargetKey,std_vector(clArc<tuGraph> *) & agPath) const {
  method_name("fordAlgo::run");

  typedef clArc<tuGraph>                     cpArc;
  typedef clNode<tuGraph>                    cpNode;
  typedef std_queue(cpNode *)                cpNodeS;
  typedef private_area::clNodeInfo2<tuGraph> cpInfo;

  typedef typename clGraph<tuGraph>::cpArcX::const_iterator  cpArcIterator1;
  typedef typename cpNode::cpArcX::const_iterator            cpArcIterator2;
  typedef typename clGraph<tuGraph>::cpNodeX::const_iterator cpNodeIterator;

  tyMark    lcMark       = ++(agGraph.mark());
  cpNode *  lcSourceNode = &(agGraph.node(agSourceKey));
  cpNode *  lcTargetNode = &(agGraph.node(agTargetKey));
  tyReal    lcBoundary   = 0.0;

  cpArcIterator1 lcCurrentArc1 = agGraph.arcs().begin();
  cpNodeIterator lcCurrentNode = agGraph.nodes().begin();
  cpArcIterator1 lcLastArc1    = agGraph.arcs().end();
  cpNodeIterator lcLastNode    = agGraph.nodes().end();

  cpArc *        lcArc;
  cpArcIterator2 lcCurrentArc2;
  cpInfo *       lcInfo;
  cpArcIterator2 lcLastArc2;
  cpNode *       lcNode;
  cpNode *       lcNode2;
  cpNodeS        lcNode1S;
  cpNodeS        lcNode2S;
  tyReal         lcPotential;

  // Initialization //
  while (lcCurrentNode!=lcLastNode) {
   lcNode=(*lcCurrentNode).second;

   if (lcNode==lcSourceNode) {
    lcNode->work()=new_object(cpInfo(true,0.0,nil));
    lcNode1S.push(lcNode);
    lcNode->mark()=lcMark;
   }
   else {
    lcNode->work()=new_object(cpInfo(false,realMax(),nil));

    if (lcNode->incomingArcs().size()==0) {
     static_cast<cpInfo *>(lcNode->work())->waiting()=true;
     lcNode1S.push(lcNode);
     lcNode->mark()=lcMark;
    }
   }

   lcCurrentNode++;
  }

  // Boundary Search For Negative Cycle //
  while (lcCurrentArc1!=lcLastArc1) {
   lcArc=(*lcCurrentArc1).second;

   if (lcArc->data().length()<0.0) lcBoundary=maxi(lcBoundary,-(lcArc->data().length()));
   else lcBoundary=maxi(lcBoundary,lcArc->data().length());

   ++lcCurrentArc1;
  }

  if (lcBoundary==realMax()) lcBoundary=realMin();
  else lcBoundary=-(lcBoundary*agGraph.nodes().size());

  // Path Search //
  while (lcNode1S.size()>0 or lcNode2S.size()>0) {
   if (lcNode2S.size()>0) {
    lcNode=lcNode2S.front();
    lcNode2S.pop();
   }
   else {
    lcNode=lcNode1S.front();
    lcNode1S.pop();
   }

   static_cast<cpInfo *>(lcNode->work())->waiting()=false;
   lcPotential=static_cast<cpInfo *>(lcNode->work())->potential();
   lcCurrentArc2=lcNode->outgoingArcs().begin();
   lcLastArc2=lcNode->outgoingArcs().end();

   while (lcCurrentArc2!=lcLastArc2) {
    lcArc=(*lcCurrentArc2).second;
    lcNode2=lcArc->targetNode();
    lcInfo=static_cast<cpInfo *>(lcNode2->work());

    if (lcInfo->potential() > lcPotential+lcArc->data().length()) {
     lcInfo->potential()=lcPotential+lcArc->data().length();

     if (lcInfo->potential()<lcBoundary) {
      if (not atKeepWorkspace) deleteNodeWorkspace(agGraph,cpInfo());
      send_error(erNegativeCycle);
     }

     lcInfo->predecessor()=lcArc;

     if (not lcInfo->waiting()) {
      if (lcNode2->mark()==lcMark) lcNode2S.push(lcNode2);
      else {
       lcNode1S.push(lcNode2);
       lcNode2->mark()=lcMark;
      }

      lcInfo->waiting()=true;
     }
    }

    ++lcCurrentArc2;
   }
  }

  // Path Construction //
  agPath.erase(agPath.begin(),agPath.end());

  if (lcTargetNode->mark()==lcMark) {
   lcArc=static_cast<cpInfo *>(lcTargetNode->work())->predecessor();

   while (lcArc!=nil) {
    agPath.push_back(lcArc);
    lcArc=static_cast<cpInfo *>(lcArc->sourceNode()->work())->predecessor();
   }
  }

  if (not atKeepWorkspace) deleteNodeWorkspace(agGraph,cpInfo());
 }
}

// S o l v e A l g o  Implementation //-------------------------------------------------------------
namespace public_area {
 //---------------------------------------------------------------------------------------DefaultRun
 /*METHOD clSolveAlgo */
 /* Solves the shortest path problem between two nodes in a graph using the default version of the
    algorithm (Dijkstra's method). Static method. */
 template <tdGraph> inline void
 clSolveAlgo<tuGraph>::defaultRun(clGraph<tuGraph> & agGraph,tyNodeKey agSourceKey,
                                  tyNodeKey agTargetKey,std_vector(clArc<tuGraph> *) & agPath)
 { clDijkstraAlgo<tuGraph>().run(agGraph,agSourceKey,agTargetKey,agPath); }
}

// End //-------------------------------------------------------------------------------------------
}
#undef dll_export
#undef public_area
#undef private_area
#undef tdGraph
#undef tuGraph
#endif
 
//==================================================================================================
// G r a p h _ p r o b l e m                                                         Implementation
// S h o r t e s t _ p a t h
//                                                                                By Bruno Bachelet
//==================================================================================================
// Copyright (c) 1999-2016
// Bruno Bachelet - bruno@nawouak.net - http://www.nawouak.net
//
// This file is part of the B++ Library. This library is free software; you can redistribute it
// and/or modify it under the terms of the GNU Library General Public License as published by the
// Free Software Foundation; either version 2 of the License, or (at your option) any later
// version.
//
// This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
// the GNU Library General Public License for more details (http://www.gnu.org).

// File Name //-------------------------------------------------------------------------------------
#line __LINE__ "graph_problem/shortest_path.cpp"

// DLL Belonging //---------------------------------------------------------------------------------
#define GRAPH_PROBLEM_DLL

// Headers //---------------------------------------------------------------------------------------
#include <bpp/graph_problem/shortest_path.hpp> /*INTERFACE*/

namespace bpp {

// Namespaces //------------------------------------------------------------------------------------
#define public_area  graphProblemShortestPath
#define private_area graphProblemShortestPath_private
#define dll_export   DLL_EXPORT

namespace public_area  {}
namespace private_area {}

static_module_name("Graph_problem/Shortest_path");

// Initialization //--------------------------------------------------------------------------------
#undef iniGraphProblemShortestPath
static_constant(private_area::clInitializer,goInitializer);

// Errors //----------------------------------------------------------------------------------------
namespace public_area {
 static_error erNegativeArcLength;
 static_error erNegativeCycle;
}

// Constants & Variables //-------------------------------------------------------------------------
dynamic_constant(clString,goDataLocation);

// Static Members //--------------------------------------------------------------------------------
namespace public_area  {}
namespace private_area {}

// Functions Implementation //----------------------------------------------------------------------
namespace public_area  {}
namespace private_area {}

// X X X  Implementation //-------------------------------------------------------------------------
namespace {}

// I n i t i a l i z e r  Implementation //---------------------------------------------------------
namespace private_area {
 //--------------------------------------------------------------------------------------------Start
 property void clInitializer::start(void) {
  if (atCounter++ == 0) {
   try {
    #include <bpp/modules.hpp> /*NEED*/
    registerStop(this);
    environment::informInitialization(goModuleName);

    erNegativeArcLength.create("Shortest Path - The length of an arc is negative.");
    erNegativeCycle.create("Shortest Path - A cycle is negative.");

    goDataLocation = new_object(clString(environment::dataLocation()+fileNameSeparator()
                     +"graph_problem"+fileNameSeparator()+"shortest_path"));
   }

   initializer_catch;
  }
 }
 //---------------------------------------------------------------------------------------------Stop
 property void clInitializer::stop(void) {
  try {
   environment::informTermination(goModuleName);

   delete_object(goDataLocation);
  }

  initializer_catch;
 }
}

// End //-------------------------------------------------------------------------------------------
}