#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.readNodeMap("coordinates_x", xcoord);

  YMap<CoordMap> ycoord = yMap(coord);

  reader.readNodeMap("coordinates_y", ycoord);

  LengthMap length(graph);

  reader.readEdgeMap("length", length);

  Node source, target;

  reader.readNode("source", source);

  reader.readNode("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;

}