From cdd524b9f36cc239cca25cbfcad5ef27c876e843 Mon Sep 17 00:00:00 2001
From: Mark Thompson <129641948+NotherNgineer@users.noreply.github.com>
Date: Fri, 30 Jun 2023 00:37:43 -0500
Subject: [PATCH] Use a common function for finding Tone Key index from
 received Tone Frequency (#1218)

* Common function for finding CTCSS tone index from freq
---
 .../application/apps/analog_audio_app.cpp     | 19 ++-------
 firmware/application/apps/ui_level.cpp        | 35 ++++++----------
 firmware/application/apps/ui_recon.cpp        | 36 ++++++----------
 firmware/application/apps/ui_recon.hpp        |  2 +-
 firmware/application/tone_key.cpp             | 41 +++++++++++++++----
 firmware/application/tone_key.hpp             |  6 +--
 6 files changed, 64 insertions(+), 75 deletions(-)

diff --git a/firmware/application/apps/analog_audio_app.cpp b/firmware/application/apps/analog_audio_app.cpp
index 0a2dfbe70..85d7bd9d4 100644
--- a/firmware/application/apps/analog_audio_app.cpp
+++ b/firmware/application/apps/analog_audio_app.cpp
@@ -404,22 +404,9 @@ void AnalogAudioView::update_modulation(ReceiverModel::Mode modulation) {
 }
 
 void AnalogAudioView::handle_coded_squelch(uint32_t value) {
-    float diff, min_diff = value;
-    size_t min_idx{0};
-    size_t c;
-
-    // Find nearest match
-    for (c = 0; c < tone_keys.size(); c++) {
-        diff = abs(((float)value / 100.0) - tone_keys[c].second);
-        if (diff < min_diff) {
-            min_idx = c;
-            min_diff = diff;
-        }
-    }
-
-    // Arbitrary confidence threshold
-    if (min_diff < 40)
-        text_ctcss.set("CTCSS " + tone_keys[min_idx].first);
+    tone_index idx = tone_key_index_by_value(value);
+    if (idx >= 0)
+        text_ctcss.set("CTCSS " + tone_key_string(idx));
     else
         text_ctcss.set("???");
 }
diff --git a/firmware/application/apps/ui_level.cpp b/firmware/application/apps/ui_level.cpp
index 50df2884b..d0dd3c761 100644
--- a/firmware/application/apps/ui_level.cpp
+++ b/firmware/application/apps/ui_level.cpp
@@ -216,34 +216,23 @@ size_t LevelView::change_mode(freqman_index_t new_mod) {
 }
 
 void LevelView::handle_coded_squelch(const uint32_t value) {
-    static int32_t last_idx = -1;
+    static tone_index last_squelch_index = -1;
 
-    float diff, min_diff = value;
-    size_t min_idx{0};
-    size_t c;
+    if (field_mode.selected_index() == NFM_MODULATION) {
+        tone_index idx = tone_key_index_by_value(value);
 
-    if (field_mode.selected_index() != NFM_MODULATION) {
-        text_ctcss.set("             ");
-        return;
-    }
-
-    // Find nearest match
-    for (c = 0; c < tone_keys.size(); c++) {
-        diff = abs(((float)value / 100.0) - tone_keys[c].second);
-        if (diff < min_diff) {
-            min_idx = c;
-            min_diff = diff;
+        if ((last_squelch_index < 0) || (last_squelch_index != idx)) {
+            last_squelch_index = idx;
+            if (idx >= 0) {
+                text_ctcss.set("T: " + tone_key_string(idx));
+                return;
+            }
+        } else {
+            return;
         }
     }
 
-    // Arbitrary confidence threshold
-    if (last_idx < 0 || (unsigned)last_idx != min_idx) {
-        last_idx = min_idx;
-        if (min_diff < 40)
-            text_ctcss.set("T: " + tone_keys[min_idx].first);
-        else
-            text_ctcss.set("             ");
-    }
+    text_ctcss.set("             ");
 }
 
 } /* namespace ui */
diff --git a/firmware/application/apps/ui_recon.cpp b/firmware/application/apps/ui_recon.cpp
index 6b509db34..4fce52465 100644
--- a/firmware/application/apps/ui_recon.cpp
+++ b/firmware/application/apps/ui_recon.cpp
@@ -1426,32 +1426,20 @@ size_t ReconView::change_mode(freqman_index_t new_mod) {
 }
 
 void ReconView::handle_coded_squelch(const uint32_t value) {
-    float diff{0.0};
-    float min_diff{(float)value};
-    size_t min_idx{0};
-    size_t c{0};
+    if (field_mode.selected_index() == NFM_MODULATION) {
+        tone_index idx = tone_key_index_by_value(value);
 
-    if (field_mode.selected_index() != NFM_MODULATION) {
-        text_ctcss.set("        ");
-        return;
-    }
-
-    // Find nearest match
-    for (c = 0; c < tone_keys.size(); c++) {
-        diff = abs(((float)value / 100.0) - tone_keys[c].second);
-        if (diff < min_diff) {
-            min_idx = c;
-            min_diff = diff;
+        if ((last_squelch_index < 0) || (last_squelch_index != idx)) {
+            last_squelch_index = idx;
+            if (idx >= 0) {
+                text_ctcss.set("T: " + tone_key_string(idx));
+                return;
+            }
+        } else {
+            return;
         }
     }
-
-    // Arbitrary confidence threshold
-    if (last_squelch_index < 0 || (unsigned)last_squelch_index != min_idx) {
-        last_squelch_index = min_idx;
-        if (min_diff < 40)
-            text_ctcss.set("T: " + tone_keys[min_idx].first);
-        else
-            text_ctcss.set("        ");
-    }
+    text_ctcss.set("        ");
 }
+
 } /* namespace ui */
diff --git a/firmware/application/apps/ui_recon.hpp b/firmware/application/apps/ui_recon.hpp
index 7bc3d27c9..b1d25f3ec 100644
--- a/firmware/application/apps/ui_recon.hpp
+++ b/firmware/application/apps/ui_recon.hpp
@@ -139,7 +139,7 @@ class ReconView : public View {
     int8_t last_rssi_med{-127};
     int8_t last_rssi_max{-127};
     int32_t last_index{-1};
-    int32_t last_squelch_index{-1};
+    tone_index last_squelch_index{-1};
     int64_t last_freq{0};
     std::string freq_file_path{};
     systime_t chrono_start{};
diff --git a/firmware/application/tone_key.cpp b/firmware/application/tone_key.cpp
index 1774c86fb..138cd407b 100644
--- a/firmware/application/tone_key.cpp
+++ b/firmware/application/tone_key.cpp
@@ -73,9 +73,9 @@ const tone_key_t tone_keys = {
     {"34 M3", 218.100},
     {"35 M4", 225.700},
     {"49 9Z", 229.100},
-    {"36 --", 233.600},
-    {"37 --", 241.800},
-    {"38 --", 250.300},
+    {"36 M5", 233.600},
+    {"37 M6", 241.800},
+    {"38 M7", 250.300},
     {"50 0Z", 254.100},
     {"Axient 28kHz", 28000.0},
     {"Senn. 32.768k", 32768.0},
@@ -113,6 +113,36 @@ std::string tone_key_string(tone_index index) {
     return tone_keys[index].first;
 }
 
+std::string tone_key_string_by_value(uint32_t value) {
+    return tone_key_string(tone_key_index_by_value(value));
+}
+
+tone_index tone_key_index_by_value(uint32_t value) {
+    float diff;
+    float min_diff{(float)value};
+    float fvalue{(float)((min_diff + 50.0) / 100.0)};
+    tone_index min_idx{-1};
+    tone_index idx;
+
+    // Find nearest match
+    for (idx = 0; idx < (tone_index)tone_keys.size(); idx++) {
+        diff = abs(fvalue - tone_keys[idx].second);
+        if (diff < min_diff) {
+            min_idx = idx;
+            min_diff = diff;
+        } else {
+            // list is sorted in frequency order; if diff is getting larger than we've passed it
+            break;
+        }
+    }
+
+    // Arbitrary confidence threshold
+    if (min_diff < 40.0)
+        return min_idx;
+    else
+        return -1;
+}
+
 tone_index tone_key_index_by_string(char* str) {
     if (!str)
         return -1;
@@ -123,9 +153,4 @@ tone_index tone_key_index_by_string(char* str) {
     return -1;
 }
 
-/* tone_index tone_key_index_by_value( int32_t freq )
-{
-        return -1 ;
-} */
-
 }  // namespace tonekey
diff --git a/firmware/application/tone_key.hpp b/firmware/application/tone_key.hpp
index 88808cd8d..fa26046de 100644
--- a/firmware/application/tone_key.hpp
+++ b/firmware/application/tone_key.hpp
@@ -30,7 +30,7 @@ using namespace ui;
 
 namespace tonekey {
 
-typedef int16_t tone_index;
+typedef int32_t tone_index;
 
 using tone_key_t = std::vector<std::pair<std::string, float>>;
 
@@ -40,8 +40,8 @@ void tone_keys_populate(OptionsField& field);
 float tone_key_frequency(const tone_index index);
 
 std::string tone_key_string(const tone_index index);
-tone_index tone_key_index_by_string(char* str);
-// tone_index tone_key_index_by_value( int32_t freq );
+std::string tone_key_string_by_value(uint32_t value);
+tone_index tone_key_index_by_value(uint32_t value);
 
 }  // namespace tonekey