#include <iostream>

#include <vector>

#include <cmath>

#include <cstdlib>

#include <lemon/smart_graph.h>

#include <lemon/dijkstra.h>

#include <lemon/floyd_warshall.h>

#include <lemon/johnson.h>

#include <lemon/fib_heap.h>

#include <lemon/time_measure.h>

#include "test_tools.h"

using namespace lemon;

using namespace std;

int main(int argc, const char *argv[]) {

  srand(time(0));

  typedef SmartGraph Graph;

  typedef Graph::Node Node;

  typedef Graph::Edge Edge;

  typedef Graph::NodeIt NodeIt;

  typedef Graph::EdgeIt EdgeIt;

  typedef Graph::EdgeMap<double> LengthMap;

  typedef Graph::NodeMap<double> DistMap;

  const int n = argc > 1 ? atoi(argv[1]) : 20;

  const int e = argc > 2 ? atoi(argv[2]) : (int)(n * log((double)n));

  const double m = argc > 3 ? (double)atoi(argv[3]) : 100.0;

  Graph graph;

  LengthMap length(graph);

  vector<Node> nodes;

  DistMap shift(graph);

  for (int i = 0; i < n; ++i) {

    Node node = graph.addNode();

    nodes.push_back(node);

    shift[node] = m * (double)rand() / (RAND_MAX + 1.0);

  }

  for (int i = 0; i < e; ++i) {

    int s = (int)(n * (double)rand() / (RAND_MAX + 1.0));

    int t = (int)(n * (double)rand() / (RAND_MAX + 1.0));

    double c = m * (double)rand() / (RAND_MAX + 1.0);

    Edge edge = graph.addEdge(nodes[s], nodes[t]);

    length[edge] = c - shift[nodes[s]] + shift[nodes[t]];

  }

  Johnson<Graph, LengthMap> johnson(graph, length);

  {

    Timer timer;

    johnson.run();

    cout << "Johnson: " << timer << endl;

  }

  typedef FibHeap<Node, double, Graph::NodeMap<int> > DoubleFibHeap;

  Johnson<Graph, LengthMap>::DefStandardHeap<DoubleFibHeap>

    ::Create fibJohnson(graph, length);

  {

    Timer timer;

    fibJohnson.run();

    cout << "Johnson with fibonacci heap: " << timer << endl;

  }

  FloydWarshall<Graph, LengthMap> floyd(graph, length);

  {

    Timer timer;

    floyd.run();

    cout << "FloydWarshall: " << timer << endl;

  }    

  for (NodeIt it(graph); it != INVALID; ++it) {

    for (NodeIt jt(graph); jt != INVALID; ++jt) {

      check(johnson.connected(it, jt) == floyd.connected(it, jt),

	    "Wrong connection in all pairs shortest path");

      check(johnson.connected(it, jt) == fibJohnson.connected(it, jt),

	    "Wrong connection in all pairs shortest path");

      if (johnson.connected(it, jt)) {

	check(johnson.dist(it, jt) == floyd.dist(it, jt),

	      "Wrong distance in all pairs shortest path");

	check(johnson.dist(it, jt) == fibJohnson.dist(it, jt),

	      "Wrong distance in all pairs shortest path");

	if (it != jt) {

 	  check(johnson.dist(it, jt) == 

		johnson.dist(it, johnson.predNode(it, jt)) +

		length[johnson.pred(it, jt)],

		"Wrong edge in all pairs shortest path");

 	  check(fibJohnson.dist(it, jt) == 

		fibJohnson.dist(it, fibJohnson.predNode(it, jt)) +

		length[fibJohnson.pred(it, jt)],

		"Wrong edge in all pairs shortest path");

	  check(floyd.dist(it, jt) == 

		floyd.dist(it, floyd.predNode(it, jt)) +

		length[floyd.pred(it, jt)],

		"Wrong edge in all pairs shortest path");

	}

      }

    }

  }

  return 0;

}