From 80ab8c86d93777354b2d780784ebb6e025d6147e Mon Sep 17 00:00:00 2001
From: Tilman Sauerbeck <tilman@code-monkey.de>
Date: Sun, 19 Jan 2020 19:57:42 +0100
Subject: [PATCH] common: Implement TimeAndPos::distance_cm().

Calculates the distance between two points in cm.
---
 src/common/gps.rs | 19 +++++++++++++++++++
 test/gps_test.rs  | 26 ++++++++++++++++++++++++++
 2 files changed, 45 insertions(+)

diff --git a/src/common/gps.rs b/src/common/gps.rs
index 7e635fc..85e1dd4 100644
--- a/src/common/gps.rs
+++ b/src/common/gps.rs
@@ -63,6 +63,25 @@ impl TimeAndPos {
             longitude_rad: Fixed::from_i64(0),
         }
     }
+
+    pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
+        let a_lat = self.latitude_rad;
+        let a_lon = self.longitude_rad;
+
+        let b_lat = other.latitude_rad;
+        let b_lon = other.longitude_rad;
+
+        let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos();
+        let y = b_lat - a_lat;
+
+        let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371);
+
+        let hundred = Fixed::from_i64(100);
+        let thousand = Fixed::from_i64(1000);
+
+        // Convert from km to cm.
+        (d_km * thousand * hundred).to_i64()
+    }
 }
 
 fn to_lower(c: u8) -> u8 {
diff --git a/test/gps_test.rs b/test/gps_test.rs
index 1cff088..7a71a76 100644
--- a/test/gps_test.rs
+++ b/test/gps_test.rs
@@ -21,8 +21,11 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
+use common::fixed15_49;
 use common::gps;
 
+type Fixed = fixed15_49::Fixed15_49;
+
 // Verifies that a single RMC message with position, date and time
 // does not suffice for getting a valid TimeAndPos result.
 #[test]
@@ -87,3 +90,26 @@ $GPGGA,110338.000,1234.5678,N,12345.6789,E,\
     assert_eq!(0.21949487565883447, tap.latitude_rad.to_f32());
     assert_eq!(2.160042433347846, tap.longitude_rad.to_f32());
 }
+
+#[test]
+fn distance_cm0() {
+    let tap0 = gps::TimeAndPos {
+        system_time: 0,
+        unix_time: 0,
+        latitude: 0,
+        longitude: 0,
+        latitude_rad: Fixed::from_f32(49.02541333).to_radians(),
+        longitude_rad: Fixed::from_f32(8.79440167).to_radians(),
+    };
+
+    let tap1 = gps::TimeAndPos {
+        system_time: 1,
+        unix_time: 1,
+        latitude: 0,
+        longitude: 0,
+        latitude_rad: Fixed::from_f32(49.02541000).to_radians(),
+        longitude_rad: Fixed::from_f32(8.79443667).to_radians(),
+    };
+
+    assert_eq!(349, tap1.distance_cm(&tap0));
+}
-- 
2.30.2