diff --git a/src/Tour.cpp b/src/Tour.cpp
index 0139e2f..1b93323 100755
--- a/src/Tour.cpp
+++ b/src/Tour.cpp
@@ -52,7 +52,7 @@ void Tour::draw(QGraphicsScene *scene) const {
         return;
 
     Node *node = m_startNode;
-    do{
+    do {
         node->point.draw(scene);
         node->point.drawTo(node->next->point, scene);
         node = node->next;
@@ -116,6 +116,31 @@ void Tour::insertNearest(Point p) {
     nearest->next = newNode;
 }
 
+Node *findbestNodeTSP(Node *const start, Point &point) {
+    auto calcHeuristic = [&point](Node *const insertAt) -> double {
+        double distanceWithNewPoint = insertAt->point.distanceTo(point) +
+                                      point.distanceTo(insertAt->next->point);
+        double originalDistance = insertAt->point.distanceTo(insertAt->next->point);
+
+        return distanceWithNewPoint - originalDistance;
+    };
+
+    double optimalH = Q_INFINITY;
+    Node *optimalNode;
+
+    Node *node = start;
+    do {
+        double h = calcHeuristic(node);
+        if (h < optimalH) {
+            optimalH = h;
+            optimalNode = node;
+        }
+        node = node->next;
+    } while (node != start);
+
+    return optimalNode;
+}
+
 void Tour::insertSmallest(Point p) {
     if (m_startNode == nullptr) {
         m_startNode = new Node(p);
@@ -128,21 +153,7 @@ void Tour::insertSmallest(Point p) {
         return;
     }
 
-    Node *bestNode = m_startNode;
-    double minIncrease = Q_INFINITY;
-
-    Node *node = m_startNode;
-    do {
-        double increase = node->point.distanceTo(p) +
-                          p.distanceTo(node->next->point) -
-                          node->point.distanceTo(node->next->point);
-        if (increase < minIncrease) {
-            minIncrease = increase;
-            bestNode = node;
-        }
-        node = node->next;
-    } while (node != m_startNode);
-
+    Node *bestNode = findbestNodeTSP(m_startNode, p);
     auto *newNode = new Node(p, bestNode->next);
     bestNode->next = newNode;
 }
diff --git a/src/tsp.cpp b/src/tsp.cpp
index 0241663..a2601ce 100755
--- a/src/tsp.cpp
+++ b/src/tsp.cpp
@@ -49,7 +49,7 @@ int main(int argc, char *argv[]) {
     auto timeStart = std::chrono::high_resolution_clock::now();
     while (input >> x >> y) {
         Point p(x, y);
-        tour.insertNearest(p);
+        tour.insertSmallest(p);
         //uncomment the 4 lines below to animate
         //tour.draw(scene);
         //std::chrono::milliseconds dura(50);