From b4a5814ef1db5e186e6ff3913b2515766944c298 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Thu, 3 Feb 2022 20:37:26 +0100
Subject: [PATCH] Points are now saveable in captureRoute

---
 lib/Navigation/navigation.cpp                    | 15 ++++++++++++++-
 src/driveModi/Modi/CaptureRoute/captureRoute.cpp |  7 ++++---
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp
index d00143b..415b191 100644
--- a/lib/Navigation/navigation.cpp
+++ b/lib/Navigation/navigation.cpp
@@ -78,9 +78,22 @@ CourseCorrection Navigation::getCourseCorrection() {
 }
 
 bool Navigation::addCurrentPosToRoute() {
-    Point p = currentLocation();
+    std::cout << "Navigation::addCurrentPosToRoute 1" << std::endl;
+    Point p = this->currentLocation();
+    std::cout << "Navigation::addCurrentPosToRoute " << p.lat << " " << p.lon << std::endl;
     if (!p.isValid()) { return false; }
+
+    // First Point
+    if (this->route->getRouteInfo().totalPoints == 0) {
+        std::cout << "Navigation::addCurrentPosToRoute 2" << std::endl;
+        this->route->addPointToRoute(p);
+        this->lastPoint = p;
+        return true;
+    }
+
+    // Ever Point after the first
     if (MIN_DISTANCE_BETWEEN_POINTS <= this->distanceBetwenn(p, this->lastPoint)) {
+        std::cout << "Navigation::addCurrentPosToRoute 3" << std::endl;
         this->route->addPointToRoute(p);
         this->lastPoint = p;
         return true;
diff --git a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
index afa198b..33ad13c 100644
--- a/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+++ b/src/driveModi/Modi/CaptureRoute/captureRoute.cpp
@@ -18,9 +18,10 @@ void CaptureRoute::loop() {
 
 void CaptureRoute::runCaptureRoute() {
     if (Ps3.data.button.triangle) {
-        this->navigation->addCurrentPosToRoute();
-        this->routeInfo.totalPoints++;
-        this->updateDisplay = true;
+        if (this->navigation->addCurrentPosToRoute()) {
+            this->routeInfo = navigation->getRouteInfo();
+            this->updateDisplay = true;
+        }
     }
 }