From b9840bccf8cfe730f3814f413e5abd6aaad22f6f Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 27 Dec 2022 13:11:03 +0100
Subject: [PATCH] refactor class route add a struct to hold coords

---
 lib/Navigation/route.cpp | 51 ++++++++++++++++++++++++++++++----------
 lib/Navigation/route.h   | 29 ++++++++++++++++-------
 2 files changed, 60 insertions(+), 20 deletions(-)

diff --git a/lib/Navigation/route.cpp b/lib/Navigation/route.cpp
index 614c929..8283a5d 100644
--- a/lib/Navigation/route.cpp
+++ b/lib/Navigation/route.cpp
@@ -12,36 +12,59 @@
 #include "route.h"
 
 Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
-    this->lat = lat;
-    this->lon = lon; 
+    this->coordinates.lat = lat;
+    this->coordinates.lon = lon; 
+    this->init(horizontalAccuracy);
+}
+
+Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
+    this->coordinates.lat = lat / 10000000.0;
+    this->coordinates.lon = lon / 10000000.0;
+    this->init(horizontalAccuracy);
+}
+
+Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
+    this->coordinates = coords;
     this->init(horizontalAccuracy);
 }
 
 Point::Point() {
-    this->lat = 0;
-    this->lon = 0; 
+    this->coordinates.lat = 0;
+    this->coordinates.lon = 0; 
     this->init(0);
 }
 
-double Point::distanceTo(const Point &point) const {
-    // distance = sqrt(dx * dx + dy * dy)
+bool Point::operator==(const Point& rhs) const {
+    return this->coordinates == rhs.getCoordinates();
+}
 
+    // distance = sqrt(dx * dx + dy * dy)
     // mit distance: Entfernung in km 
     // dx = 111.3 * cos(lat) * (lon1 - lon2)
     // lat = (lat1 + lat2) / 2 * 0.01745
     // dy = 111.3 * (lat1 - lat2)
     // lat1, lat2, lon1, lon2: Breite, Länge in Grad
+double Point::distanceTo(const Coordinates& point) const {
+    Coordinates begin = this->coordinates;
+    Coordinates end = point;
 
-    double lat = (this->lat + point.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
-    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (this->lat - point.lat);
-    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (this->lon - point.lon);
+    double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
+    double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
+    double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
 
     return sqrt(dx * dx + dy * dy);
 }
 
-double Point::courseTo(const Point &point) const {
-    double phi = log( tan(point.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(this->lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
-    double lon = (this->lon * ROUTE_DEGREE_TO_RADIANT - point.lon * ROUTE_DEGREE_TO_RADIANT);
+double Point::distanceTo(const Point &point) const {
+    return this->distanceTo(point.getCoordinates());
+}
+
+double Point::courseTo(const Coordinates& point) const {
+    Coordinates begin = this->coordinates;
+    Coordinates end = point;
+
+    double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
+    double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
 
     double res = atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
 
@@ -51,6 +74,10 @@ double Point::courseTo(const Point &point) const {
     return res;
 }
 
+double Point::courseTo(const Point &point) const {
+    return this->courseTo(point.getCoordinates());
+}
+
 void Point::init(uint32_t horizontalAccuracy) {
     this->creationTime = millis();
     if (horizontalAccuracy > 9999)
diff --git a/lib/Navigation/route.h b/lib/Navigation/route.h
index 2914d4f..42e72fd 100644
--- a/lib/Navigation/route.h
+++ b/lib/Navigation/route.h
@@ -30,6 +30,15 @@
  */
 class Point{
     public:
+        struct Coordinates {
+            double lon;
+            double lat;
+
+            bool operator==(const Coordinates rhs) const {
+                return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
+            }
+        };
+
         enum PointAccuracy {
             none,
             fourDigOfCM,
@@ -42,15 +51,14 @@ class Point{
         /**
          * @brief Construct a new Point object
          * 
-         * @param lat latidude
+         * @param lat latitude
          * @param lon longitude
          */
         Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
+        Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
+        Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
         Point();
 
-        double lat = 0;
-        double lon = 0;
-
         /**
          * @brief checks if to points are equal
          * 
@@ -58,9 +66,7 @@ class Point{
          * @return true 
          * @return false 
          */
-        bool operator==(const Point& rhs) const {
-            return this->lat == rhs.lat && this->lon == rhs.lon;
-        }
+        bool operator==(const Point& rhs) const;
 
         /**
          * @brief Checks if the Point is initalized.
@@ -68,18 +74,25 @@ class Point{
          * @return true 
          * @return false 
          */
-        bool isInit() const { return this->lat + this->lon; }
+        bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
         bool isValid() const { return (this->accuracy > 0) ? true : false; }
 
+        double distanceTo(const Coordinates& point) const;
         double distanceTo(const Point& point) const;
+        double courseTo(const Coordinates& point) const;
         double courseTo(const Point& point) const;
 
+        double getLongitude() const { return this->coordinates.lon; }
+        double getLatitude() const { return this->coordinates.lat; }
+        Coordinates getCoordinates() const { return this->coordinates; }
+
         PointAccuracy getAccuracy() { return this->accuracy; }
 
     private:
         void init(uint32_t horizontalAccuracy);
 
         PointAccuracy accuracy = PointAccuracy::none;
+        Coordinates coordinates;
 
         uint32_t creationTime = 0;
 };