From 46c936b48e6ce6b2fb164b881041e31e7aa027b7 Mon Sep 17 00:00:00 2001
From: Moritz Kobitzsch <moritz@mapbox.com>
Date: Mon, 17 Oct 2016 13:24:42 +0200
Subject: [PATCH] handle trips with identical coordinates

---
 features/testbot/trip.feature                   | 13 +++++++++++++
 include/engine/trip/trip_farthest_insertion.hpp | 14 ++++++++++----
 2 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/features/testbot/trip.feature b/features/testbot/trip.feature
index aae9691f7..0e1a141cf 100644
--- a/features/testbot/trip.feature
+++ b/features/testbot/trip.feature
@@ -108,4 +108,17 @@ Feature: Basic trip planning
             | waypoints | trips |
             | 1,2       |       |
 
+    Scenario: Testbot - Repeated Coordinate
+        Given the node map
+            """
+            a   b
+            """
+
+        And the ways
+            | nodes |
+            | ab    |
+
+        When I plan a trip I should get
+            | waypoints                                         | trips |
+            | a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a,a |       |
 
diff --git a/include/engine/trip/trip_farthest_insertion.hpp b/include/engine/trip/trip_farthest_insertion.hpp
index b0a6fc427..6a7c40088 100644
--- a/include/engine/trip/trip_farthest_insertion.hpp
+++ b/include/engine/trip/trip_farthest_insertion.hpp
@@ -168,9 +168,11 @@ std::vector<NodeID> FarthestInsertionTrip(const NodeIDIterator &start,
     if (static_cast<std::size_t>(component_size) == number_of_locations)
     {
         // find the pair of location with the biggest distance and make the pair the initial start
-        // trip
-        const auto index = std::distance(
-            std::begin(dist_table), std::max_element(std::begin(dist_table), std::end(dist_table)));
+        // trip. Skipping over the very first element (0,0), we make sure not to end up with the
+        // same start/end in the special case where all entries are the same.
+        const auto index =
+            std::distance(std::begin(dist_table),
+                          std::max_element(std::begin(dist_table) + 1, std::end(dist_table)));
         max_from = index / number_of_locations;
         max_to = index % number_of_locations;
     }
@@ -182,11 +184,15 @@ std::vector<NodeID> FarthestInsertionTrip(const NodeIDIterator &start,
         {
             for (auto y = start; y != end; ++y)
             {
+                // don't repeat coordinates
+                if (*x == *y)
+                    continue;
+
                 const auto xy_dist = dist_table(*x, *y);
                 // SCC decomposition done correctly?
                 BOOST_ASSERT(xy_dist != INVALID_EDGE_WEIGHT);
 
-                if (xy_dist > max_dist)
+                if (xy_dist >= max_dist)
                 {
                     max_dist = xy_dist;
                     max_from = *x;