From 333e542c49ee62f686be32735e0439dd202bf1e5 Mon Sep 17 00:00:00 2001
From: Silvano Seva <silseva@fastwebnet.it>
Date: Fri, 27 Oct 2023 12:17:00 +0200
Subject: [PATCH] wip agc on codec2 output

---
 openrtx/src/core/audio_codec.c | 26 +++++++++++++++++++++++---
 1 file changed, 23 insertions(+), 3 deletions(-)

diff --git a/openrtx/src/core/audio_codec.c b/openrtx/src/core/audio_codec.c
index fecd9ca0..2f4020e9 100644
--- a/openrtx/src/core/audio_codec.c
+++ b/openrtx/src/core/audio_codec.c
@@ -284,6 +284,8 @@ static void *decodeFunc(void *arg)
         return NULL;
     }
 
+    float agcPeakAverage = 2000.0f;
+    float agcGain = 1.0f;
     codec2 = codec2_create(CODEC2_MODE_3200);
 
     // Ensure that thread start is correctly synchronized with the output
@@ -325,15 +327,33 @@ static void *decodeFunc(void *arg)
         {
             codec2_decode(codec2, audioBuf, ((uint8_t *) &frame));
 
-            #ifdef PLATFORM_MD3x0
+            // #ifdef PLATFORM_MD3x0
             // Bump up volume a little bit, as on MD3x0 is quite low
-            for(size_t i = 0; i < 160; i++) audioBuf[i] *= 2;
-            #endif
+            // for(size_t i = 0; i < 160; i++) audioBuf[i] *= 2;
+            // #endif
 
+            agcGain = 2000.0f / agcPeakAverage;
+            agcGain *= 6.0f;
+
+            uint16_t maxPeak = 0;
+            for(size_t i = 0; i < 160; i++)
+            {
+                int16_t sample = abs(audioBuf[i]);
+                audioBuf[i] *= agcGain;
+                if(sample > maxPeak)
+                    maxPeak = sample;
+            }
+
+            if(maxPeak > 200)
+            {
+                agcPeakAverage -= agcPeakAverage / 100.0f;
+                agcPeakAverage += ((float) maxPeak) / 100.0f;
+            }
         }
         else
         {
             memset(audioBuf, 0x00, 160 * sizeof(stream_sample_t));
+            agcPeakAverage = 2000.0f;
         }
 
         outputStream_sync(oStream, true);