diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp
index a2b4201283..9b50426657 100644
--- a/routing/astar_router.cpp
+++ b/routing/astar_router.cpp
@@ -14,10 +14,10 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector<RoadPos> const & start
   m_algo.SetGraph(graph);
 
   // TODO (@gorshenin): switch to FindPathBidirectional.
-  Algo::Result result = m_algo.FindPath(startPos, finalPos, route);
+  TAlgorithm::Result result = m_algo.FindPath(startPos, finalPos, route);
   switch (result)
   {
-    case Algo::RESULT_OK:
+    case TAlgorithm::Result::OK:
       // Following hack is used because operator== checks for
       // equivalience, not identity, since it doesn't test
       // RoadPos::m_segEndpoint. Thus, start and final positions
@@ -34,9 +34,9 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector<RoadPos> const & start
           route.back() = fp;
       }
       return IRouter::NoError;
-    case Algo::RESULT_NO_PATH:
+    case TAlgorithm::Result::NoPath:
       return IRouter::RouteNotFound;
-    case Algo::RESULT_CANCELLED:
+    case TAlgorithm::Result::Cancelled:
       return IRouter::Cancelled;
   }
 }
diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp
index 9e9baf621c..353916c82f 100644
--- a/routing/astar_router.hpp
+++ b/routing/astar_router.hpp
@@ -8,11 +8,9 @@ namespace routing
 {
 class AStarRouter : public RoadGraphRouter
 {
-  typedef RoadGraphRouter BaseT;
-
 public:
   AStarRouter(Index const * pIndex = 0)
-      : BaseT(pIndex, unique_ptr<IVehicleModel>(new PedestrianModel()))
+      : RoadGraphRouter(pIndex, unique_ptr<IVehicleModel>(new PedestrianModel()))
   {
   }
 
@@ -30,7 +28,7 @@ public:
   bool IsCancelled() const override { return m_algo.IsCancelled(); }
 
 private:
-  using Algo = AStarAlgorithm<RoadGraph>;
-  Algo m_algo;
+  using TAlgorithm = AStarAlgorithm<RoadGraph>;
+  TAlgorithm m_algo;
 };
 }  // namespace routing
diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp
index 06c793fd5a..5305021fa7 100644
--- a/routing/base/astar_algorithm.hpp
+++ b/routing/base/astar_algorithm.hpp
@@ -5,6 +5,8 @@
 #include "base/logging.hpp"
 #include "routing/base/graph.hpp"
 #include "std/algorithm.hpp"
+#include "std/functional.hpp"
+#include "std/iostream.hpp"
 #include "std/map.hpp"
 #include "std/queue.hpp"
 #include "std/vector.hpp"
@@ -19,16 +21,34 @@ public:
   using TVertexType = typename TGraphType::TVertexType;
   using TEdgeType = typename TGraphType::TEdgeType;
 
-  static uint32_t const kCancelledPollPeriod = 128;
-  static double constexpr kEpsilon = 1e-6;
+  static uint32_t const kCancelledPollPeriod;
+  static uint32_t const kQueueSwtichPeriod;
+  static double const kEpsilon;
 
-  enum Result
+  enum class Result
   {
-    RESULT_OK,
-    RESULT_NO_PATH,
-    RESULT_CANCELLED
+    OK,
+    NoPath,
+    Cancelled
   };
 
+  friend ostream & operator<<(ostream & os, Result const & result)
+  {
+    switch (result)
+    {
+      case Result::OK:
+        os << "OK";
+        break;
+      case Result::NoPath:
+        os << "NoPath";
+        break;
+      case Result::Cancelled:
+        os << "Cancelled";
+        break;
+    }
+    return os;
+  }
+
   AStarAlgorithm() : m_graph(nullptr) {}
 
   Result FindPath(vector<TVertexType> const & startPos, vector<TVertexType> const & finalPos,
@@ -46,7 +66,7 @@ private:
   {
     State(TVertexType const & vertex, double distance) : vertex(vertex), distance(distance) {}
 
-    bool operator<(State const & rhs) const { return distance > rhs.distance; }
+    inline bool operator>(State const & rhs) const { return distance > rhs.distance; }
 
     TVertexType vertex;
     double distance;
@@ -68,9 +88,7 @@ private:
     double TopDistance() const
     {
       ASSERT(!queue.empty(), ());
-      auto it = bestDistance.find(queue.top().vertex);
-      CHECK(it != bestDistance.end(), ());
-      return it->second;
+      return bestDistance.at(queue.top().vertex);
     }
 
     // p_f(v) = 0.5*(π_f(v) - π_r(v)) + 0.5*π_r(t)
@@ -101,7 +119,7 @@ private:
     vector<TVertexType> const & finalPos;
     TGraph const & graph;
 
-    priority_queue<State> queue;
+    priority_queue<State, vector<State>, greater<State>> queue;
     map<TVertexType, double> bestDistance;
     map<TVertexType, TVertexType> parent;
     TVertexType bestVertex;
@@ -119,6 +137,18 @@ private:
   TGraphType const * m_graph;
 };
 
+// static
+template <typename TGraph>
+uint32_t const AStarAlgorithm<TGraph>::kCancelledPollPeriod = 128;
+
+// static
+template <typename TGraph>
+uint32_t const AStarAlgorithm<TGraph>::kQueueSwtichPeriod = 128;
+
+// static
+template <typename TGraph>
+double const AStarAlgorithm<TGraph>::kEpsilon = 1e-6;
+
 // This implementation is based on the view that the A* algorithm
 // is equivalent to Dijkstra's algorithm that is run on a reweighted
 // version of the graph. If an edge (v, w) has length l(v, w), its reduced
@@ -139,22 +169,16 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
   ASSERT(m_graph, ());
   ASSERT(!startPos.empty(), ());
   ASSERT(!finalPos.empty(), ());
-#if defined(DEBUG)
-  for (auto const & v : startPos)
-    LOG(LDEBUG, ("AStarAlgorithm::FindPath(): startPos:", v));
-  for (auto const & v : finalPos)
-    LOG(LDEBUG, ("AStarAlgorithm::FindPath(): finalPos:", v));
-#endif  // defined(DEBUG)
 
   vector<TVertexType> sortedStartPos(startPos.begin(), startPos.end());
   sort(sortedStartPos.begin(), sortedStartPos.end());
 
   map<TVertexType, double> bestDistance;
-  priority_queue<State> queue;
+  priority_queue<State, vector<State>, greater<State>> queue;
   map<TVertexType, TVertexType> parent;
   for (auto const & rp : finalPos)
   {
-    VERIFY(bestDistance.insert({rp, 0.0}).second, ());
+    VERIFY(bestDistance.emplace(rp, 0.0).second, ());
     queue.push(State(rp, 0.0));
   }
 
@@ -163,11 +187,8 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
   while (!queue.empty())
   {
     ++steps;
-    if (steps % kCancelledPollPeriod == 0)
-    {
-      if (IsCancelled())
-        return RESULT_CANCELLED;
-    }
+    if (steps % kCancelledPollPeriod == 0 && IsCancelled())
+      return Result::Cancelled;
 
     State const stateV = queue.top();
     queue.pop();
@@ -175,13 +196,10 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
     if (stateV.distance > bestDistance[stateV.vertex])
       continue;
 
-    /// We need the original start position because it contains the projection point to the road
-    /// feature.
-    auto it = lower_bound(sortedStartPos.begin(), sortedStartPos.end(), stateV.vertex);
-    if (it != sortedStartPos.end() && *it == stateV.vertex)
+    if (binary_search(sortedStartPos.begin(), sortedStartPos.end(), stateV.vertex))
     {
       ReconstructPath(stateV.vertex, parent, path);
-      return RESULT_OK;
+      return Result::OK;
     }
 
     vector<TEdgeType> adj;
@@ -196,7 +214,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
       double const piW = m_graph->HeuristicCostEstimate(stateW.vertex, sortedStartPos[0]);
       double const reducedLen = len + piW - piV;
 
-      ASSERT(reducedLen >= -kEpsilon, ("Invariant violation!"));
+      CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon));
       double const newReducedDist = stateV.distance + max(reducedLen, 0.0);
 
       auto t = bestDistance.find(stateW.vertex);
@@ -210,7 +228,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
     }
   }
 
-  return RESULT_NO_PATH;
+  return Result::NoPath;
 }
 
 /// @todo This may work incorrectly if (startPos.size() > 1) or (finalPos.size() > 1).
@@ -221,12 +239,6 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
 {
   ASSERT(!startPos.empty(), ());
   ASSERT(!finalPos.empty(), ());
-#if defined(DEBUG)
-  for (auto const & roadPos : startPos)
-    LOG(LDEBUG, ("AStarAlgorithm::FindPathBidirectional(): startPos:", roadPos));
-  for (auto const & roadPos : finalPos)
-    LOG(LDEBUG, ("AStarAlgorithm::FindPathBidirectional(): finalPos:", roadPos));
-#endif  // defined(DEBUG)
 
   BidirectionalStepContext forward(true /* forward */, startPos, finalPos, *m_graph);
   BidirectionalStepContext backward(false /* forward */, startPos, finalPos, *m_graph);
@@ -236,12 +248,12 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
 
   for (auto const & rp : startPos)
   {
-    VERIFY(forward.bestDistance.insert({rp, 0.0}).second, ());
+    VERIFY(forward.bestDistance.emplace(rp, 0.0).second, ());
     forward.queue.push(State(rp, 0.0 /* distance */));
   }
   for (auto const & rp : finalPos)
   {
-    VERIFY(backward.bestDistance.insert({rp, 0.0}).second, ());
+    VERIFY(backward.bestDistance.emplace(rp, 0.0).second, ());
     backward.queue.push(State(rp, 0.0 /* distance */));
   }
 
@@ -260,16 +272,11 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
   {
     ++steps;
 
-    if (steps % kCancelledPollPeriod == 0)
-    {
-      if (IsCancelled())
-        return RESULT_CANCELLED;
-    }
+    if (steps % kCancelledPollPeriod == 0 && IsCancelled())
+      return Result::Cancelled;
 
-    if (steps % kCancelledPollPeriod == 0)
-    {
+    if (steps % kQueueSwtichPeriod == 0)
       swap(cur, nxt);
-    }
 
     double const curTop = cur->TopDistance();
     double const nxtTop = nxt->TopDistance();
@@ -288,7 +295,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
       CHECK(!path.empty(), ());
       if (!cur->forward)
         reverse(path.begin(), path.end());
-      return RESULT_OK;
+      return Result::OK;
     }
 
     State const stateV = cur->queue.top();
@@ -309,7 +316,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
       double const pW = cur->ConsistentHeuristic(stateW.vertex);
       double const reducedLen = len + pW - pV;
       double const pRW = nxt->ConsistentHeuristic(stateW.vertex);
-      CHECK(reducedLen >= -kEpsilon, ("Invariant failed:", reducedLen, "<", -kEpsilon));
+      CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon));
       double newReducedDist = stateV.distance + max(reducedLen, 0.0);
 
       typename map<TVertexType, double>::const_iterator t = cur->bestDistance.find(stateW.vertex);
@@ -340,7 +347,7 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
     }
   }
 
-  return RESULT_NO_PATH;
+  return Result::NoPath;
 }
 
 // static
diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp
index beeac149da..3ed2e35903 100644
--- a/routing/road_graph.cpp
+++ b/routing/road_graph.cpp
@@ -8,6 +8,15 @@
 
 namespace routing
 {
+namespace
+{
+double TimeBetweenSec(m2::PointD const & v, m2::PointD const & w)
+{
+  static double const kMaxSpeedMPS = 5000.0 / 3600;
+  return MercatorBounds::DistanceOnEarth(v, w) / kMaxSpeedMPS;
+}
+}  // namespace
+
 RoadPos::RoadPos(uint32_t featureId, bool forward, size_t segId, m2::PointD const & p)
     : m_featureId((featureId << 1) + (forward ? 1 : 0)), m_segId(segId), m_segEndpoint(p)
 {
@@ -34,16 +43,13 @@ void RoadGraph::GetAdjacencyListImpl(RoadPos const & v, vector<RoadEdge> & adj)
   for (PossibleTurn const & turn : turns)
   {
     RoadPos const & w = turn.m_pos;
-    adj.push_back(RoadEdge(w, HeuristicCostEstimate(v, w)));
+    adj.emplace_back(w, TimeBetweenSec(v.GetSegEndpoint(), w.GetSegEndpoint()));
   }
 }
 
 double RoadGraph::HeuristicCostEstimateImpl(RoadPos const & v, RoadPos const & w) const
 {
-  static double const kMaxSpeedMPS = 5000.0 / 3600;
-  m2::PointD const & ve = v.GetSegEndpoint();
-  m2::PointD const & we = w.GetSegEndpoint();
-  return MercatorBounds::DistanceOnEarth(ve, we) / kMaxSpeedMPS;
+  return TimeBetweenSec(v.GetSegEndpoint(), w.GetSegEndpoint());
 }
 
 }  // namespace routing
diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp
index 038aaddd38..0c5c677441 100644
--- a/routing/road_graph.hpp
+++ b/routing/road_graph.hpp
@@ -110,7 +110,7 @@ struct RoadEdge
   double const weight;
 };
 
-// A wrapper around IGraph, which make it's possible to use IRoadGraph
+// A wrapper around IGraph, which makes it possible to use IRoadGraph
 // with routing algorithms.
 class RoadGraph : public Graph<RoadPos, RoadEdge, RoadGraph>
 {
diff --git a/routing/routing_tests/astar_algorithm_test.cpp b/routing/routing_tests/astar_algorithm_test.cpp
index 9872aab5c6..0f7f6e03f8 100644
--- a/routing/routing_tests/astar_algorithm_test.cpp
+++ b/routing/routing_tests/astar_algorithm_test.cpp
@@ -19,17 +19,17 @@ struct Edge
   double w;
 };
 
-class MemGraph : public Graph<unsigned, Edge, MemGraph>
+class UndirectedGraph : public Graph<unsigned, Edge, UndirectedGraph>
 {
 public:
-  void AddUndirectedEdge(unsigned u, unsigned v, unsigned w)
+  void AddEdge(unsigned u, unsigned v, unsigned w)
   {
     m_adjs[u].push_back(Edge(v, w));
     m_adjs[v].push_back(Edge(u, w));
   }
 
 private:
-  friend class Graph<unsigned, Edge, MemGraph>;
+  friend class Graph<unsigned, Edge, UndirectedGraph>;
 
   void GetAdjacencyListImpl(unsigned v, vector<Edge> & adj) const
   {
@@ -44,33 +44,33 @@ private:
   map<unsigned, vector<Edge>> m_adjs;
 };
 
-void TestAStar(MemGraph const & graph, vector<unsigned> const & expectedRoute)
+void TestAStar(UndirectedGraph const & graph, vector<unsigned> const & expectedRoute)
 {
-  using Algo = AStarAlgorithm<MemGraph>;
+  using TAlgorithm = AStarAlgorithm<UndirectedGraph>;
 
-  Algo algo;
+  TAlgorithm algo;
   algo.SetGraph(graph);
   vector<unsigned> actualRoute;
-  TEST_EQUAL(Algo::RESULT_OK, algo.FindPath(vector<unsigned>{0}, vector<unsigned>{4}, actualRoute),
-             ());
+  TEST_EQUAL(TAlgorithm::Result::OK,
+             algo.FindPath(vector<unsigned>{0}, vector<unsigned>{4}, actualRoute), ());
   TEST_EQUAL(expectedRoute, actualRoute, ());
 
   actualRoute.clear();
-  TEST_EQUAL(Algo::RESULT_OK,
+  TEST_EQUAL(TAlgorithm::Result::OK,
              algo.FindPathBidirectional(vector<unsigned>{0}, vector<unsigned>{4}, actualRoute), ());
   TEST_EQUAL(expectedRoute, actualRoute, ());
 }
 
 UNIT_TEST(AStarAlgorithm_Sample)
 {
-  MemGraph graph;
+  UndirectedGraph graph;
 
   // Inserts edges in a format: <source, target, weight>.
-  graph.AddUndirectedEdge(0, 1, 10);
-  graph.AddUndirectedEdge(1, 2, 5);
-  graph.AddUndirectedEdge(2, 3, 5);
-  graph.AddUndirectedEdge(2, 4, 10);
-  graph.AddUndirectedEdge(3, 4, 3);
+  graph.AddEdge(0, 1, 10);
+  graph.AddEdge(1, 2, 5);
+  graph.AddEdge(2, 3, 5);
+  graph.AddEdge(2, 4, 10);
+  graph.AddEdge(3, 4, 3);
 
   vector<unsigned> expectedRoute = {0, 1, 2, 3, 4};
 
diff --git a/std/function.hpp b/std/function.hpp
index d43499e7bc..ba4b1e9cd3 100644
--- a/std/function.hpp
+++ b/std/function.hpp
@@ -9,6 +9,7 @@
 
   #include <functional>
   using std::function;
+  using std::greater;
 
 #else