From da1a75ec39e3f6131fe8fcc462835a9886ef50c5 Mon Sep 17 00:00:00 2001
From: Carles Fernandez <carles.fernandez@gmail.com>
Date: Sat, 18 Mar 2023 08:47:35 +0100
Subject: [PATCH] Fix geohash decoding

---
 src/algorithms/PVT/libs/geohash.cc            |  8 ++++++--
 .../pvt/geohash_test.cc                       | 19 +++++++++++++------
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/algorithms/PVT/libs/geohash.cc b/src/algorithms/PVT/libs/geohash.cc
index 0021a752d..33f14c705 100644
--- a/src/algorithms/PVT/libs/geohash.cc
+++ b/src/algorithms/PVT/libs/geohash.cc
@@ -122,8 +122,12 @@ std::array<double, 2> Geohash::decode(std::string geohash) const
 
     // round to close to centre without excessive precision: ⌊2-log10(Δ°)⌋ decimal places
     std::array<double, 2> latlon{};
-    latlon[0] = std::floor(lat * std::pow(10, std::floor(2.0 - std::log10(latMax - latMin))));
-    latlon[1] = std::floor(lon * std::pow(10, std::floor(2.0 - std::log10(lonMax - lonMin))));
+    int decimalPlaces = std::floor(2.0 - std::log10(latMax - latMin));
+    double factor = std::pow(10, decimalPlaces);
+    latlon[0] = std::round(lat * factor) / factor;
+    int decimalPlaces2 = std::floor(2.0 - std::log10(lonMax - lonMin));
+    double factor2 = std::pow(10, decimalPlaces2);
+    latlon[1] = std::round(lon * factor2) / factor2;
 
     return latlon;
 }
diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc
index bb1168a08..9eb0080ad 100644
--- a/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc
+++ b/src/tests/unit-tests/signal-processing-blocks/pvt/geohash_test.cc
@@ -19,15 +19,22 @@
 TEST(Geohash_Test, Encode)
 {
     Geohash gh = Geohash();
-    std::string hash;
-    EXPECT_NO_THROW(hash = gh.encode(52.205, 0.119, 7));
-
-    EXPECT_EQ(0, hash.compare("u120fxw"));
-
+    std::string geohash;
+    EXPECT_NO_THROW(geohash = gh.encode(52.205, 0.119, 7));
+    EXPECT_EQ(0, geohash.compare("u120fxw"));
     EXPECT_THROW(gh.encode(52.205, 0.119, 0), std::invalid_argument);
 }
 
-TEST(Geohash_Test, precision)
+TEST(Geohash_Test, Decode)
+{
+    Geohash gh = Geohash();
+    auto latlon = gh.decode("sp36v1zk0e2g");
+    EXPECT_NEAR(41.274966141209006, latlon[0], 1e-8);
+    EXPECT_NEAR(1.9875180535018444, latlon[1], 1e-8);
+    EXPECT_THROW(gh.decode(""), std::runtime_error);
+}
+
+TEST(Geohash_Test, Precision)
 {
     Geohash gh = Geohash();
     std::string hash;