#include <iostream>

 #include <fstream>

 #include <lemon/smart_graph.h>

 #include <lemon/dijkstra.h>

 #include <lemon/maps.h>

 #include <lemon/xy.h>

 #include <lemon/graph_reader.h>

 #include <lemon/time_measure.h>

 #include <cmath>

 using namespace lemon;

 template <typename CoordMap>

 class PotentialMap {

 public:

   typedef double Value;

   typedef typename CoordMap::Key Key;

   PotentialMap(const CoordMap& _coord, const xy<double>& _target)

     : coord(_coord), target(_target) {}

   double operator[](const Key& node) const {

     return std::sqrt((coord[node].x - target.x) * (coord[node].x - target.x) + 

 		     (coord[node].y - target.y) * (coord[node].y - target.y));

   }

 private:

   const CoordMap& coord;

   xy<double> target;

 };

 template <typename Graph, typename LengthMap, typename PotentialMap>

 class ReducedLengthMap {

 public:

   typedef double Value;

   typedef typename LengthMap::Key Key;

   ReducedLengthMap(const Graph& _graph, const LengthMap& _length, 

 		   const PotentialMap& _pot) 

     : graph(_graph), length(_length), pot(_pot) {}

   Value operator[](const Key& edge) const {

     return length[edge] - (pot[graph.source(edge)] - pot[graph.target(edge)]);

   }

 private:

   const Graph& graph;

   const LengthMap& length;

   const PotentialMap& pot;

 };

 int main() {

   typedef SmartGraph Graph;

   typedef Graph::Edge Edge;

   typedef Graph::Node Node;

   typedef Graph::EdgeIt EdgeIt;

   typedef Graph::NodeIt NodeIt;

   typedef Graph::EdgeMap<double> LengthMap;

   typedef Graph::NodeMap<xy<double> > CoordMap;

   SmartGraph graph;

   std::ifstream is("route.lgf");

   GraphReader<Graph> reader(is, graph);

   CoordMap coord(graph);

   XMap<CoordMap> xcoord = xMap(coord);

   reader.addNodeMap("coordinates_x", xcoord);

   YMap<CoordMap> ycoord = yMap(coord);

   reader.addNodeMap("coordinates_y", ycoord);

   LengthMap length(graph);

   reader.addEdgeMap("length", length);

   Node source, target;

   reader.addNode("source", source);

   reader.addNode("target", target);

   reader.run();

   {

     Timer timer;

     Dijkstra<Graph, LengthMap> dijkstra(graph, length);

     dijkstra.init();

     dijkstra.addSource(source);

     dijkstra.start(target);

     std::cout << dijkstra.dist(target) << std::endl;

     std::cout << timer << std::endl;

   }

   {

     Timer timer;

     typedef PotentialMap<CoordMap> Potential;

     Potential potential(coord, coord[target]);

     typedef ReducedLengthMap<Graph, LengthMap, Potential> ReducedLength;

     ReducedLength reduced(graph, length, potential);

     Dijkstra<Graph, ReducedLength> dijkstra(graph, reduced);

     dijkstra.init();

     dijkstra.addSource(source);

     dijkstra.start(target);

     std::cout << dijkstra.dist(target) + potential[source] << std::endl;

     std::cout << timer << std::endl;

   }

   return 0;

 }