#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;
}