#include "BFM.h"
#include "Dijkstra.h"
#include "Graph.h"
#include "ShortestPath.h"
#include "SPFA.h"

#include <assert.h>
#include <iostream>
#include <vector>
#include <string>

void
PrintPath(std::vector<Edge> aPath)
{
  for (unsigned int i = 0 ; i < aPath.size() ; ++i) {
    Edge e = aPath[i];
    if (!i) { // First edge.
      std::cout << "(" << e.mFrom << ")-- ";
    }
    std::cout << e.mDistance << " -->(" << e.mTo << ")";
    if (i + 1 != aPath.size()) {
      std::cout << "-- ";
    }
  }
}

void ShowResults(const char* aName, ShortestPath& aSP)
{
  std::cout << "Shortest path by " << aName << " from " << aSP.Start() << std::endl;
  std::cout << "Vertex\tDistance\tPath" << std::endl;

  std::vector<int> distances = aSP.Distances();
  assert(!distances.empty());

  for (unsigned int i = 0 ; i < distances.size() ; ++i) {
    // Print the distances from start to each vertex.
    std::cout << i << "\t" <<
      (distances[i] == INF ? "*" : std::to_string(distances[i]));
    // Print the path from start to each vertex.
    std::cout << "\t\t";
    std::vector<Edge> path;
    try {
      path = aSP.GetPath(i);
    } catch(BFM::NegativeCycleException& e) {
      std::cout << e.what() << ": ";
      path = e.path();
    }
    PrintPath(path);
    std::cout << std::endl;
  }

  std::cout << std::endl;
}

void
CompareResults(std::vector<int> aDistances1, std::vector<int> aDistances2)
{
  assert(!aDistances1.empty() && !aDistances2.empty() &&
         aDistances1.size() == aDistances2.size());
  for (unsigned int i = 0 ; i < aDistances1.size(); ++i) {
    assert(aDistances1[i] == aDistances2[i]);
  }
}

int main()
{
  // Directed Graph:
  //           3
  //      (1)--->(3)
  //   1 ^ |     /| \15
  //    /  |9   / |  v
  //  (0)  |   /  |  (5)
  //    \  |  /4  |  ^
  //  12 v v v    v /4
  //      (2)--->(4)
  //          5

  Graph dg(6, true);
  dg.AddEdge(0, 1, 1);
  dg.AddEdge(0, 2, 12);
  dg.AddEdge(1, 2, 9);
  dg.AddEdge(1, 3, 3);
  dg.AddEdge(2, 4, 5);
  dg.AddEdge(3, 2, 4);
  dg.AddEdge(3, 4, 13);
  dg.AddEdge(3, 5, 15);
  dg.AddEdge(4, 5, 4);

  // Undirected Graph:
  //           8      7
  //      (1)----(2)----(3)
  //   4 / |    2/ \     | \9
  //    /  |11  /   \  14|  \
  //  (0)  |  (8)    \4  |  (4)
  //    \  | 7/ \6    \  |  /
  //   8 \ | /   \     \ | /10
  //      (7)----(6)----(5)
  //          1      2

  Graph ug(9);
  ug.AddEdge(0, 1, 4);
  ug.AddEdge(0, 7, 8);
  ug.AddEdge(1, 2, 8);
  ug.AddEdge(1, 7, 11);
  ug.AddEdge(2, 3, 7);
  ug.AddEdge(2, 8, 2);
  ug.AddEdge(2, 5, 4);
  ug.AddEdge(3, 4, 9);
  ug.AddEdge(3, 5, 14);
  ug.AddEdge(4, 5, 10);
  ug.AddEdge(5, 6, 2);
  ug.AddEdge(6, 7, 1);
  ug.AddEdge(6, 8, 6);
  ug.AddEdge(7, 8, 7);

  std::vector<Graph> tests;
  tests.push_back(dg);
  tests.push_back(ug);

  for (auto g: tests) {
    for (unsigned int start = 0 ; start < g.mVertices ; ++start) {
      Dijkstra dij(g, start);
      ShowResults("Dijkstra", dij);

      BFM bfm(g, start);
      // If the graph has negative cycles, then it has no shortest path.
      assert(!bfm.NegativeCycleDetected());
      ShowResults("Bellman-Ford-Moore", bfm);

      SPFA spfa(g, start);
      ShowResults("Shortest-Path-Faster-Algorithm", spfa);

      // The shortest path might have several different solutions ,e.g.,
      // The shortest path from 7 -> 8 in the undirected graph above
      // could be 7 -> 8 or 7 -> 6 -> 8, so we only compare the distances here.
      CompareResults(dij.Distances(), bfm.Distances());
      CompareResults(bfm.Distances(), spfa.Distances());
    }
  }

  // Directed Graph with Negative Edges ?
  //          -1              -1
  //      (1)----(2)  => (1) ---> (2)
  //                      ^        |
  //                      |   -1   |
  //                      +--------+
  // If one undirected edge has negative weight,
  // then it must have a negative cycle.

  // Directed Graph with Negative Edges:
  //                (3) <--------- (5)
  //               /  ^      3      ^
  //           -3 /    \ 2          |
  //             /      \           | -4
  //       2    V    1   \     4    |
  // (0) ----> (1) ----> (2) ----> (4)

  Graph ndg(6, true);
  ndg.AddEdge(0, 1, 2);
  ndg.AddEdge(1, 2, 1);
  ndg.AddEdge(2, 3, 2);
  ndg.AddEdge(3, 1, -3);
  ndg.AddEdge(2, 4, 4);
  ndg.AddEdge(4, 5, -4);
  ndg.AddEdge(5, 3, 3);

  std::vector<Graph> nTests;
  nTests.push_back(ndg);

  for (auto g: nTests) {
    for (unsigned int start = 0 ; start < g.mVertices ; ++start) {
      BFM bfm(g, start);
      assert(!bfm.NegativeCycleDetected());
      ShowResults("Bellman-Ford-Moore", bfm);

      SPFA spfa(g, start);
      ShowResults("Shortest-Path-Faster-Algorithm", spfa);

      CompareResults(bfm.Distances(), spfa.Distances());
    }
  }

  // Directed Graph with Negative Cycles:
  // (6)            (3) <--------- (5)
  //  ^            /  ^      3      ^
  //  |        -4 /    \ 2          |
  //  | 6        /      \           | -5
  //  |    2    V    1   \     4    |
  // (0) ----> (1) ----> (2) ----> (4)

  Graph ncdg(7, true);
  ncdg.AddEdge(0, 1, 2);
  ncdg.AddEdge(1, 2, 1);
  ncdg.AddEdge(2, 3, 2);
  ncdg.AddEdge(3, 1, -4);
  ncdg.AddEdge(2, 4, 4);
  ncdg.AddEdge(4, 5, -5);
  ncdg.AddEdge(5, 3, 3);
  ncdg.AddEdge(0, 6, 6);

  std::vector<Graph> ncTests;
  ncTests.push_back(ncdg);

  for (auto g: ncTests) {
    for (unsigned int start = 0 ; start < g.mVertices ; ++start) {
      BFM bfm(g, start);
      assert(start == 6 // There is no negative cycle in any path from 6.
        ? !bfm.NegativeCycleDetected()
        : bfm.NegativeCycleDetected());
      ShowResults("Bellman-Ford-Moore", bfm);

      SPFA spfa(g, start);
      assert(start == 6 // There is no negative cycle in any path from 6.
        ? !spfa.NegativeCycleDetected()
        : spfa.NegativeCycleDetected());
      ShowResults("Shortest-Path-Faster-Algorithm", spfa);
    }
  }

  return 0;
}

CXX=g++
CFLAGS=-Wall -std=c++14

SOURCES=test.cpp\
        BFM.cpp\
        Dijkstra.cpp\
        SPFA.cpp
OBJECTS=$(SOURCES:.cpp=.o)

EXECUTABLE=run_test

all: $(EXECUTABLE)

$(EXECUTABLE): $(OBJECTS)
	$(CXX) $(CFLAGS) $(OBJECTS) -o $(EXECUTABLE)

.cpp.o:
	$(CXX) -c $(CFLAGS) $< -o $@

clean:
	rm $(EXECUTABLE) *.o

#if !defined(SHORTEST_PATH_H)
#define SHORTEST_PATH_H

#include "Graph.h"

#include <vector>

const int INF = ((unsigned int) ~0) >> 1; // Infinity: The max value of int.
const unsigned int NIL = ~0;              // Nonexistent vertex.

// Base class for shortest path's implementation.
class ShortestPath
{
public:
  explicit ShortestPath(Graph aGraph, unsigned int aStart)
    : mGraph(aGraph)
    , mStart(aStart)
  {
    assert(mGraph.mVertices && mStart < mGraph.mVertices);
  };

  virtual ~ShortestPath() = default;

  // Return the beginning vertex.
  unsigned int Start() { return mStart; }

  // Return the the shortest distance from mStart to each vertex.
  virtual std::vector<int> Distances() { return mDistances; }

  // Return the shortest path from mStart to aDestination.
  virtual std::vector<Edge> GetPath(unsigned int aDestination) = 0;

protected:
  Graph mGraph;
  unsigned int mStart;

  // Store the shortest distance from mStart to each vertex.
  std::vector<int> mDistances;

  // Store the last edges of the shortest path from mStart to each vertex.
  // They should be all initialized to be nonexistent edges at first.
  std::vector<Edge> mLastEdges;
};

#endif // SHORTEST_PATH_H

#if !defined(SPFA_H)
#define SPFA_H

#include "BFM.h"

#include <queue>

// Shortest Path Faster Algorithm: An improvement of the Bellman–Ford algorithm.
// https://en.wikipedia.org/wiki/Shortest_Path_Faster_Algorithm
class SPFA final: public BFM
{
public:
  SPFA(Graph aGraph, unsigned int aStart);

  ~SPFA();

protected:
  // Find the minimal distance to each vertex in the graph.
  virtual void Compute() override;

private:
  // Convert to the adjacent edges of all the vertice in the graph.
  void ConvertGraph();

  // The adjacent edge allows us to find all the connected edges from one vertex
  // in O(1). All the connected edges of vertex u will be stored in
  // mAdjacentEdges[u]. All the edges stored are directed.
  //
  // Undirected edge(u, v) = w will be stored as two directed edges:
  // edge(u -> v) = w:
  //   adjacent[u][x].mFrom = u, adjacent[u][x].mTo = v,
  //   adjacent[u][x].mWeight = w,
  // edge(v -> u) = w:
  //   adjacent[v][y].mFrom = v, adjacent[v][y].mTo = u,
  //   adjacent[v][y].mWeight = w,
  // where x, y are the indices of vector adjacent[u] and adjacent[v].
  std::vector< std::vector<Edge> > mAdjacentEdges;

  // The queue stores the reachable vertices, so we can go through all the graph
  // vertex-by-vertex to calculate the shortest path. Its usage is very similar
  // to BFS(Breadth-first search) algorithm.
  std::queue<unsigned int> mQueue;
};

#endif // SPFA_H

#include "SPFA.h"

#include <assert.h>

SPFA::SPFA(Graph aGraph, unsigned int aStart)
  : BFM(aGraph, aStart)
{
}

SPFA::~SPFA()
{
}

void
SPFA::ConvertGraph()
{
  assert(mAdjacentEdges.empty());

  mAdjacentEdges.resize(mGraph.mVertices);
  for (auto e: mGraph.mEdges) { // e: edge(from, to) = w
    assert(mGraph.mDirected == e.mDirected);
    // Put edge(from -> to) = w into mAdjacentEdges[mFrom].
    mAdjacentEdges[e.mFrom].push_back(Edge(e.mFrom, e.mTo, e.mDistance, true));
    if (!e.mDirected) {
      // Put edge(to -> from) = w into mAdjacentEdges[mTo].
      mAdjacentEdges[e.mTo].push_back(Edge(e.mTo, e.mFrom, e.mDistance, true));
    }
  }
}

void
SPFA::Compute()
{
  assert(!mGraph.mEdges.empty() && mDistances.empty() && mLastEdges.empty() &&
         mQueue.empty());

  ConvertGraph();

  // To track how many times the distance from mStart to one vertex has updated.
  std::vector<unsigned int> count(mGraph.mVertices, 0);

  mDistances.resize(mGraph.mVertices, INF);
  mDistances[mStart] = 0;
  //                         start vertex
  //                              v
  //          +---+---+---+-   -+---+-   -+---+
  //   vertex | 0 | 1 | 2 | ... | k | ... | N |
  //          +---+---+---+-   -+---+-   -+---+
  // distance | * | * | * | ... | 0 | ... | * |
  //          +---+---+---+-   -+---+-   -+---+

  // Indicating one vertex is already in queue or not.
  std::vector<bool> queuing(mGraph.mVertices, false);
  mQueue.push(mStart);
  queuing[mStart] = true;

  // Initialize all last edge to be nonexistent edge at first.
  for (unsigned int i = 0 ; i < mGraph.mVertices ; ++i) {
    mLastEdges.push_back(Edge(NIL, i, INF, true));
  }

  while (!mQueue.empty()) {
    unsigned int current = mQueue.front();
    mQueue.pop();
    queuing[current] = false;

    for (auto e: mAdjacentEdges[current]) {
      assert(current == e.mFrom && mDistances[e.mFrom] < INF);
      if (mDistances[e.mTo] > mDistances[e.mFrom] + e.mDistance) {
        ++count[e.mTo];
        // One vertex has pointed by (V - 1) edges at most,
        // where the V is the number of vertices in the graph,
        // so the distance from mStart to it will be updated
        // (V - 1) times at most.
        // If the distance is updated over (V - 1) times, it implies that
        // it must be in a negative cycle
        // (or the distance is impossible to be updated).
        if (count[e.mTo] >= mGraph.mVertices) {
          mNegativeCycleDetected = true;
          return;
        }
        mDistances[e.mTo] = mDistances[e.mFrom] + e.mDistance;
        mLastEdges[e.mTo] = e;
        if (!queuing[e.mTo]) {
          mQueue.push(e.mTo);
          queuing[e.mTo] = true;
        }
      }
    }
  }
}

#if !defined(GRAPH_H)
#define GRAPH_H

#include <assert.h> // for assert
#include <vector>   // for std::vector

// The path is a combination of successive graph's edges,
// whose distance is the sum of all edges' distance.
typedef struct Path // it's directed.
{
  Path(unsigned int aFrom, unsigned int aTo, int aDistance)
    : mFrom(aFrom)
    , mTo(aTo)
    , mDistance(aDistance)
  {
  }

  ~Path() {}

  // Used in std::greater
  bool operator>(const Path& aOther) const
  {
    return mDistance > aOther.mDistance;
  }

  // bool operator<(const Path& aOther) const
  // {
  //   return mDistance < aOther.mDistance;
  // }

  unsigned int mFrom;
  unsigned int mTo;
  int mDistance;
} Path;

typedef struct Edge: public Path
{
  Edge(unsigned int aFrom, unsigned int aTo, int aDistance, bool aDirected)
    : Path(aFrom, aTo, aDistance)
    , mDirected(aDirected)
  {
  }

  ~Edge() {}

  // Used in std::greater
  bool operator>(const Edge& aOther) const
  {
    assert(mDirected == aOther.mDirected);
    return mDistance > aOther.mDistance;
  }

  // bool operator<(const Edge& aOther) const
  // {
  //   assert(mDirected == aOther.mDirected);
  //   return mDistance < aOther.mDistance;
  // }

  // If edge is directed, then the edge is from mFrom to mTo.
  // Otherwise, the edge is bi-directional.
  // It can start from mFrom to mTo and from mTo to mFrom.
  bool mDirected;
} Edge;

typedef struct Graph
{
  Graph(unsigned int aVertices, bool aDirected = false)
    : mVertices(aVertices)
    , mDirected(aDirected)
  {
    assert(mVertices);
  }

  ~Graph() {}

  void AddEdge(unsigned int aFrom, unsigned int aTo, int aWeight)
  {
    assert(aFrom < mVertices && aTo < mVertices);
    mEdges.push_back(Edge(aFrom, aTo, aWeight, mDirected));
  }

  unsigned int mVertices;   // number of vertices.
  std::vector<Edge> mEdges; // edges between vertices.
  bool mDirected;           // Indicating this is directed graph or not.
} Graph;

#endif // GRAPH_H

#if !defined(DIJKSTRA_H)
#define DIJKSTRA_H

#include "ShortestPath.h"

#include <functional> // for std::greater
#include <queue>      // for std::priority_queue

class Dijkstra final: public ShortestPath
{
public:
  Dijkstra(Graph aGraph, unsigned int aStart);

  ~Dijkstra();

  virtual std::vector<Edge> GetPath(unsigned int aDestination) override;

private:
  // Find the minimal distance to each vertex in the graph.
  void Compute();

  // Convert to the adjacent edges of all the vertice in the graph.
  void ConvertGraph();

  // The adjacent edge allows us to find all the connected edges from one vertex
  // in O(1). All the connected edges of vertex u will be stored in
  // mAdjacentEdges[u]. All the edges stored are directed.
  //
  // Undirected edge(u, v) = w will be stored as two directed edges:
  // edge(u -> v) = w:
  //   adjacent[u][x].mFrom = u, adjacent[u][x].mTo = v,
  //   adjacent[u][x].mWeight = w,
  // edge(v -> u) = w:
  //   adjacent[v][y].mFrom = v, adjacent[v][y].mTo = u,
  //   adjacent[v][y].mWeight = w,
  // where x, y are the indices of vector adjacent[u] and adjacent[v].
  std::vector< std::vector<Edge> > mAdjacentEdges;

  // The priority queue allows us to get the vertex with minimal distance
  // from mStart in O(1), and update the distance in O(log V).
  // The std::priority_queue is in decreasing order by default,
  // so we need to use std::greater to sort the elements in increasing order.
  std::priority_queue<Path,
                      std::vector<Path>,
                      std::greater<Path>> mQueue;
};

#endif // DIJKSTRA_H

#include "Dijkstra.h"

#include <assert.h>

Dijkstra::Dijkstra(Graph aGraph, unsigned int aStart)
  : ShortestPath(aGraph, aStart)
{
  Compute();
}

Dijkstra::~Dijkstra()
{
}

/*
std::vector<Edge>
Dijkstra::GetPath(unsigned int aDestination)
{
  std::vector<Edge> path;
  std::vector<bool> visited(mGraph.mVertices, false);

  unsigned int v = aDestination;
  visited[aDestination] = true;
  while(v != mStart) {
    Edge e = mLastEdges[v];
    assert(e.mTo == v);
    if (e.mFrom == NIL) {
      // aDestination is source vertex or it's unreachable vertex.
      assert(e.mDistance == INF);
      assert((v == mStart) ? !mDistances[v] : mDistances[v] == INF);
      break;
    }

    path.insert(path.begin(), e);

    v = e.mFrom;
    assert(!visited[v]);
    visited[v] = true;
  }

  // For debugging.
  int sum = 0;
  for (unsigned int i = 0 ; i < path.size() ; ++i) {
    if (i + 1 < path.size()) { // Not the last element.
     assert(path[i].mTo == path[i + 1].mFrom);
    }
    sum += path[i].mDistance;
    assert(visited[path[i].mFrom] && visited[path[i].mTo]);
  }
  assert(mDistances[aDestination] == INF || sum == mDistances[aDestination]);

  return path;
};
*/

/*
std::vector<Edge>
Dijkstra::GetPath(unsigned int aDestination)
{
  std::vector<Edge> path;
  std::vector<bool> visited(mGraph.mVertices, false);

  unsigned int v = aDestination;
  while(v != mStart) {
    assert(!visited[v]);
    visited[v] = true;

    Edge e = mLastEdges[v];
    assert(e.mTo == v);
    if (e.mFrom == NIL) {
      // aDestination is source vertex or it's unreachable vertex.
      assert(e.mDistance == INF);
      assert((v == mStart) ? !mDistances[v] : mDistances[v] == INF);
      break;
    }

    path.insert(path.begin(), e);

    v = e.mFrom;
  }
  if (!visited[v]) {
    assert(v == mStart);
    visited[v] = true;
  }

  // For debugging.
  int sum = 0;
  for (unsigned int i = 0 ; i < path.size() ; ++i) {
    if (i + 1 < path.size()) { // Not the last element.
     assert(path[i].mTo == path[i + 1].mFrom);
    }
    sum += path[i].mDistance;
    assert(visited[path[i].mFrom] && visited[path[i].mTo]);
  }
  assert(mDistances[aDestination] == INF || sum == mDistances[aDestination]);

  return path;
};
*/

std::vector<Edge>
Dijkstra::GetPath(unsigned int aDestination)
{
  std::vector<Edge> path;
  std::vector<bool> visited(mGraph.mVertices, false);

  unsigned int v = aDestination;
  do {
    Edge e = mLastEdges[v];
    assert(e.mTo == v);
    assert(!visited[v]);
    visited[v] = true;

    if (e.mFrom == NIL) {
      // aDestination is source vertex or it's unreachable vertex.
      assert(e.mDistance == INF);
      assert((v == mStart) ? !mDistances[v] : mDistances[v] == INF);
      break;
    }

    path.insert(path.begin(), e);

    v = e.mFrom;
  } while(1);

  // For debugging.
  int sum = 0;
  for (unsigned int i = 0 ; i < path.size() ; ++i) {
    if (i + 1 < path.size()) { // Not the last element.
     assert(path[i].mTo == path[i + 1].mFrom);
    }
    sum += path[i].mDistance;
    assert(visited[path[i].mFrom] && visited[path[i].mTo]);
  }
  assert(mDistances[aDestination] == INF || sum == mDistances[aDestination]);

  return path;
};

void
Dijkstra::ConvertGraph()
{
  assert(mAdjacentEdges.empty());

  mAdjacentEdges.resize(mGraph.mVertices);
  for (auto e: mGraph.mEdges) { // e: edge(from, to) = w
    assert(mGraph.mDirected == e.mDirected);
    // Put edge(from -> to) = w into mAdjacentEdges[mFrom].
    mAdjacentEdges[e.mFrom].push_back(Edge(e.mFrom, e.mTo, e.mDistance, true));
    if (!e.mDirected) {
      // Put edge(to -> from) = w into mAdjacentEdges[mTo].
      mAdjacentEdges[e.mTo].push_back(Edge(e.mTo, e.mFrom, e.mDistance, true));
    }
  }
}

void
Dijkstra::Compute()
{
  assert(!mGraph.mEdges.empty() && mDistances.empty() && mLastEdges.empty() &&
         mQueue.empty());

  ConvertGraph();

  std::vector<bool> visitied(mGraph.mVertices, false);

  mDistances.resize(mGraph.mVertices, INF);
  mDistances[mStart] = 0;
  //                         start vertex
  //                              v
  //          +---+---+---+-   -+---+-   -+---+
  //   vertex | 0 | 1 | 2 | ... | k | ... | N |
  //          +---+---+---+-   -+---+-   -+---+
  // distance | * | * | * | ... | 0 | ... | * |
  //          +---+---+---+-   -+---+-   -+---+

  // The first selected vertex must be the start vertex,
  // so we fake an edge connected to it.
  // Therefore, it must be picked at first.
  mQueue.push(Path(mStart, mStart, 0));

  // Initialize all last edge to be nonexistent edge at first.
  for (unsigned int i = 0 ; i < mGraph.mVertices ; ++i) {
    mLastEdges.push_back(Edge(NIL, i, INF, true));
  }

  while (!mQueue.empty()) {
    // Pick the vertex with minimal distance from mStart.
    unsigned int nearest = mQueue.top().mTo;

    if (!visitied[nearest]) {
      visitied[nearest] = true;
    } /*else {
      // Some longer paths might be pushed to the queue before the shortest one
      // is pushed, so we could skip them.
      mQueue.pop();
      continue;
    }*/
    // However, the shortest path must be picked before the longer paths
    // are selected, so even the longer paths are picked, they are unable
    // to update the shortest path. Thus, we could ignore them.

    // Go to the nearest vertex and update the distances of all vertice
    // to the source vertex via the nearest vertex.
    mQueue.pop();
    for (auto e: mAdjacentEdges[nearest]) {
      assert(e.mDistance >= 0); // Dijkstra cannot handle negative edge.
      assert(e.mFrom == nearest); // e is a directed edge from the nearest.
      if (mDistances[e.mTo] > mDistances[nearest] + e.mDistance) {
        assert(!visitied[e.mTo]);
        mDistances[e.mTo] = mDistances[nearest] + e.mDistance;
        mQueue.push(Path(mStart, e.mTo, mDistances[e.mTo]));
        // Update the last edge of the shortest path from mStart to e.mTo.
        mLastEdges[e.mTo] = e;
      }
    }
  }
}

#if !defined(BFM_H)
#define BFM_H

#include "ShortestPath.h"

#include <exception>

class BFM: public ShortestPath // Bellman-Ford-Moore
{
public:
  struct NegativeCycleException : public std::exception {
    NegativeCycleException(std::vector<Edge> aPath)
      : mPath(aPath)
    {}

    const char* what() const throw()
    {
      return "Negative Cycle Exception";
    }

    std::vector<Edge>& path()
    {
      return mPath;
    }

  private:
    std::vector<Edge> mPath;
  };

  BFM(Graph aGraph, unsigned int aStart);

  ~BFM();

  virtual std::vector<int> Distances() override
  {
    if (!Computed()) {
      Compute();
    }
    return mDistances;
  }

  // It returns the shortest distance if aDestination is NOT in a negative cycle,
  // Otherwise, it throws a NegativeCycleException error.
  virtual std::vector<Edge> GetPath(unsigned int aDestination) override;

  bool NegativeCycleDetected()
  {
    if (!Computed()) {
      Compute();
    }
    return mNegativeCycleDetected;
  }

protected:
  // Find the minimal distance to each vertex in the graph.
  virtual void Compute();

  virtual bool Computed()
  {
    return !mDistances.empty();
  }

  bool mNegativeCycleDetected;

private:
  // Try relaxing the edge.
  // Return true if the edge is relaxed. Otherwise, return false.
  // If aNegativeCycleDetecting is true, then the distances and its
  // coming edge won't be updated.
  bool Relax(Edge& aEdge, bool aNegativeCycleDetecting = false);
};

#endif // BFM_H

#include "BFM.h"

#include <assert.h>

BFM::BFM(Graph aGraph, unsigned int aStart)
  : ShortestPath(aGraph, aStart)
  , mNegativeCycleDetected(false)
{
}

BFM::~BFM()
{
}

std::vector<Edge>
BFM::GetPath(unsigned int aDestination)
{
  if (!Computed()) {
    Compute();
  }

  assert(!mLastEdges.empty());

  std::vector<Edge> path;
  std::vector<bool> visited(mGraph.mVertices, false);
  bool inCycle = false;

  unsigned int v = aDestination;
  do {
    Edge e = mLastEdges[v];
    assert(e.mTo == v);

    if (visited[v]) {
      assert(mNegativeCycleDetected);
      inCycle = true;
      // break;
      throw NegativeCycleException(path);
    }

    visited[v] = true;

    if (e.mFrom == NIL) {
      // aDestination is source vertex or it's unreachable vertex.
      assert(e.mDistance == INF);
      assert((v == mStart) ? !mDistances[v] : mDistances[v] == INF);
      break;
    }

    path.insert(path.begin(), e);

    v = e.mFrom;
  } while(1);

  // For debugging.
  int sum = 0;
  for (unsigned int i = 0 ; i < path.size() ; ++i) {
    if (i + 1 < path.size()) { // Not the last element.
     assert(path[i].mTo == path[i + 1].mFrom);
    }
    sum += path[i].mDistance;
    assert(visited[path[i].mFrom] && visited[path[i].mTo]);
  }
  //  mNegativeCycleDetected  inCycle  behavior
  //  -------------------------------------------------------
  //                   false    false  return shortest path
  //                   false     true  !! impossible case !!
  //                    true    false  return shortest path
  //                    true     true  return path with cycle
  if (inCycle) {
    assert(mNegativeCycleDetected);
  } else {
    // mNegativeCycleDetected can be true or false.
    assert(mDistances[aDestination] == INF || sum == mDistances[aDestination]);
  }

  return path;
};

void
BFM::Compute()
{
  assert(!mGraph.mEdges.empty() && mDistances.empty() && mLastEdges.empty());

  mDistances.resize(mGraph.mVertices, INF);
  mDistances[mStart] = 0;
  //                         start vertex
  //                              v
  //          +---+---+---+-   -+---+-   -+---+
  //   vertex | 0 | 1 | 2 | ... | k | ... | N |
  //          +---+---+---+-   -+---+-   -+---+
  // distance | * | * | * | ... | 0 | ... | * |
  //          +---+---+---+-   -+---+-   -+---+

  // Initialize all last edge to be nonexistent edge at first.
  for (unsigned int i = 0 ; i < mGraph.mVertices ; ++i) {
    mLastEdges.push_back(Edge(NIL, i, INF, true));
  }

  // The longest path of the graph with N vertices is N - 1 edges,
  // so the distances will be updated N - 1 times at most.
  // In the first round, the distances from mSatrt to the one-edge-away vertice
  // will be updated(at least).
  // In the second round, the distances from mSatrt to the two-edge-away vertice
  // will be updated(at least).
  // ...
  // In the (N - 1)th round, the distances from mSatrt to the
  // (N - 1)-edge-away vertice will be updated.
  for (unsigned int i = 0 ; i < mGraph.mVertices - 1 ; ++i) {
    for (auto e: mGraph.mEdges) {
      Relax(e);
    }
  }

  // Detect whether the graph has negative cycles.
  assert(!mNegativeCycleDetected);
  for (auto e: mGraph.mEdges) {
     if (Relax(e, true)) {
       mNegativeCycleDetected = true;
       break;
     }
  }
}

bool
RelaxOneWay(Edge& aEdge, std::vector<int>& aDistances,
            std::vector<Edge>& aLastEdges, bool aNegativeCycleDetecting)
{
  if (aDistances[aEdge.mFrom] < INF &&
      aDistances[aEdge.mTo] > aDistances[aEdge.mFrom] + aEdge.mDistance) {
    if (!aNegativeCycleDetecting) {
      aDistances[aEdge.mTo] = aDistances[aEdge.mFrom] + aEdge.mDistance;
      aLastEdges[aEdge.mTo] = aEdge;
    }
    return true;
  }
  return false;
}

bool
BFM::Relax(Edge& aEdge, bool aNegativeCycleDetecting)
{
  bool r = RelaxOneWay(aEdge, this->mDistances, this->mLastEdges,
                       aNegativeCycleDetecting);
  if (!aEdge.mDirected) {
    Edge e = Edge(aEdge.mTo, aEdge.mFrom, aEdge.mDistance, true);
    r |= RelaxOneWay(e, this->mDistances, this->mLastEdges,
                     aNegativeCycleDetecting);
  }
  return r;
}