FS-8875 Enable faster beep detection

Unnecessary computation of arc cosine is omitted
and convergence is checked on partial results.
Original frequency is easily retrieved when needed.

diff --git a/src/mod/applications/mod_avmd/Makefile.am b/src/mod/applications/mod_avmd/Makefile.am
index ed791ce208dc603f6ea2d7623c351924c4e0755a..885cb6be3c5c4704b5688c698f6c2f94ae878827 100644 (file)
--- a/src/mod/applications/mod_avmd/Makefile.am
+++ b/src/mod/applications/mod_avmd/Makefile.am
@@ -2,7 +2,7 @@ include $(top_srcdir)/build/modmake.rulesam
 MODNAME=mod_avmd
 
 mod_LTLIBRARIES = mod_avmd.la
-mod_avmd_la_SOURCES  = mod_avmd.c avmd_amplitude.c avmd_buffer.c avmd_desa2.c avmd_goertzel.c avmd_fast_acosf.c
+mod_avmd_la_SOURCES  = mod_avmd.c avmd_buffer.c avmd_desa2_tweaked.c avmd_fast_acosf.c
 mod_avmd_la_CFLAGS   = $(AM_CFLAGS) $(AM_MOD_AVMD_CXXFLAGS)
 mod_avmd_la_LIBADD   = $(switch_builddir)/libfreeswitch.la
 mod_avmd_la_LDFLAGS  = -avoid-version -module -no-undefined -shared

diff --git a/src/mod/applications/mod_avmd/avmd_amplitude.c b/src/mod/applications/mod_avmd/avmd_amplitude.c
index e9222de2668c3f28741086bdd4f7f9d1f73745cc..a54caef4fa416f989e5fa4db982db45f2fddfada 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_amplitude.c
+++ b/src/mod/applications/mod_avmd/avmd_amplitude.c
@@ -1,4 +1,6 @@
-#ifndef __AMPLITUDE_H__
+#ifndef __AVMD_AMPLITUDE_H__
+
+
 #include <math.h>
 #include "avmd_amplitude.h"
 #include "avmd_psi.h"
@@ -10,14 +12,13 @@
  * @param f Frequency estimate
  * @return The amplitude at position i 
  */
-extern double amplitude(circ_buffer_t *b, size_t i, double f)
+extern double avmd_amplitude(circ_buffer_t *b, size_t i, double f)
 {
     double result;
-
     result = sqrt(PSI(b, i) / sin(f * f));
-
     return result;
 }
 
-#endif
+
+#endif /* __AVMD_AMPLITUDE_H__ */
 

diff --git a/src/mod/applications/mod_avmd/avmd_amplitude.h b/src/mod/applications/mod_avmd/avmd_amplitude.h
index 04db43f048f52fb98354fe1a242bca3c89acb064..b059dfffba2855f2ed9ef7ff53b743133e921e44 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_amplitude.h
+++ b/src/mod/applications/mod_avmd/avmd_amplitude.h
@@ -1,10 +1,18 @@
-#ifndef __AMPLITUDE_H__
-#define __AMPLITUDE_H__
-#include "avmd_buffer.h"
+/*
+ * @brief   Estimation of amplitude using DESA-2 algorithm.
+ * @author Eric des Courtis
+ * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ */
+
+
+#ifndef __AVMD_AMPLITUDE_H__
+#define __AVMD_AMPLITUDE_H__
 
-extern double amplitude(circ_buffer_t *, size_t i, double f);
+
+#include "avmd_buffer.h"
 
 
-#endif
+extern double avmd_amplitude(circ_buffer_t *, size_t i, double f);
 
 
+#endif /* __AVMD_AMPLITUDE_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_buffer.h b/src/mod/applications/mod_avmd/avmd_buffer.h
index 46d1c07d4cb99122b838033b965d11dda8f067ce..61aff270b24fac7fb92744866097d104ffc090f7 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_buffer.h
+++ b/src/mod/applications/mod_avmd/avmd_buffer.h
@@ -1,5 +1,15 @@
-#ifndef __BUFFER_H__
-#define __BUFFER_H__
+/*
+ * @brief   Circular buffer.
+ *
+ * @author Eric des Courtis
+ * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ */
+
+
+#ifndef __AVMD_BUFFER_H__
+#define __AVMD_BUFFER_H__
+
+
 #include <stdlib.h>
 #include <assert.h>
 
@@ -55,6 +65,10 @@ extern size_t next_power_of_2(size_t v);
        if ((b)->backlog > (b)->buf_len) (b)->backlog = (b)->buf_len; \
     } while (0)
 
+
+/*                 ((f)[(b)->i] >= 0) ? \
+                   ((BUFF_TYPE)(f)[(b)->i] / (BUFF_TYPE)INT16_MAX): \
+                   (0.0 - ((BUFF_TYPE)(f)[(b)->i] / (BUFF_TYPE)INT16_MIN)) \ */
 #define INSERT_INT16_FRAME(b, f, l) \
     { \
        for ((b)->i = 0; (b)->i < (l); (b)->i++) { \
@@ -62,9 +76,7 @@ extern size_t next_power_of_2(size_t v);
                (b), \
                ((b)->i + (b)->pos), \
                ( \
-                   ((f)[(b)->i] >= 0) ? \
-                   ((BUFF_TYPE)(f)[(b)->i] / (BUFF_TYPE)INT16_MAX): \
-                   (0.0 - ((BUFF_TYPE)(f)[(b)->i] / (BUFF_TYPE)INT16_MIN)) \
+                   (BUFF_TYPE)(f)[(b)->i] \
                ) \
            ); \
        } \
@@ -102,5 +114,4 @@ extern size_t next_power_of_2(size_t v);
        SET_SAMPLE((b), GET_CURRENT_LPOS((b)), (s)); \
     } while (0)
 
-#endif
-
+#endif /* __AVMD_BUFFER_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_desa2.c b/src/mod/applications/mod_avmd/avmd_desa2.c
index e96f4b013bdea31b3562384054c7c6223f33931a..abecf6ee644e22bfaaf15d9f4dd1df828dd29d4c 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_desa2.c
+++ b/src/mod/applications/mod_avmd/avmd_desa2.c
@@ -1,4 +1,6 @@
-#ifndef __DESA2_H__
+#ifndef __AVMD_DESA2_H__
+
+
 #include <stdio.h>
 #ifdef WIN32
 #include <float.h>
@@ -14,7 +16,7 @@
 #include "avmd_fast_acosf.h"
 #endif
 
-extern double desa2(circ_buffer_t *b, size_t i)
+extern double avmd_desa2(circ_buffer_t *b, size_t i)
 {
     double d;
     double n;
@@ -33,13 +35,10 @@ extern double desa2(circ_buffer_t *b, size_t i)
     x4 = GET_SAMPLE((b), ((i) + 4));
 
     x2sq = x2 * x2;
-
     d = 2.0 * ((x2sq) - (x1 * x3));
     if (d == 0.0) return 0.0;
-
     n = ((x2sq) - (x0 * x4)) - ((x1 * x1) - (x0 * x2)) - ((x3 * x3) - (x2 * x4));
 
-
 #ifdef AVMD_FAST_MATH
     result = 0.5 * (double)fast_acosf((float)n/d);
 #else
@@ -52,4 +51,4 @@ extern double desa2(circ_buffer_t *b, size_t i)
 
 }
 
-#endif
+#endif /* __AVMD_DESA2_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_desa2.h b/src/mod/applications/mod_avmd/avmd_desa2.h
index a22af5e72cdf6ee170fc1bbad1a0ec68f6938b1b..f6488e2ad8b67bca2b1b03e690a4a9b1e19f1f80 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_desa2.h
+++ b/src/mod/applications/mod_avmd/avmd_desa2.h
@@ -1,8 +1,19 @@
-#ifndef __DESA2_H__
-#define __DESA2_H__
+/*
+ * @brief  DESA-2 algorithm implementation.
+ * @author Eric des Courtis
+ * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ */
+
+
+#ifndef __AVMD_DESA2_H__
+#define __AVMD_DESA2_H__
+
+
 #include <math.h>
 #include "avmd_buffer.h"
 
-extern double desa2(circ_buffer_t *b, size_t i);
-#endif
+/* Returns digital frequency estimation. */
+extern double avmd_desa2(circ_buffer_t *b, size_t i);
+
 
+#endif  /* __AVMD_DESA2_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_desa2_tweaked.c b/src/mod/applications/mod_avmd/avmd_desa2_tweaked.c
new file mode 100644 (file)
index 0000000..e7665a5
--- /dev/null
+++ b/src/mod/applications/mod_avmd/avmd_desa2_tweaked.c
@@ -0,0 +1,64 @@
+#ifndef __AVMD_DESA2_TWEAKED_H__
+
+
+#include <stdio.h>
+#ifdef WIN32
+#include <float.h>
+#define ISNAN(x) (!!(_isnan(x)))
+#else
+#define ISNAN(x) (isnan(x))
+#endif
+#include "avmd_buffer.h"
+#include "avmd_desa2_tweaked.h"
+#include "avmd_options.h"
+
+#ifdef AVMD_FAST_MATH
+#include "avmd_fast_acosf.h"
+#endif
+
+#include <switch.h>
+
+
+double
+avmd_desa2_tweaked(circ_buffer_t *b, size_t i,
+        switch_core_session_t *session)
+{
+    double d;
+    double n;
+    double x0;
+    double x1;
+    double x2;
+    double x3;
+    double x4;
+    double x2sq;
+    double result;
+
+    x0 = GET_SAMPLE((b), (i));
+    x1 = GET_SAMPLE((b), ((i) + 1));
+    x2 = GET_SAMPLE((b), ((i) + 2));
+    x3 = GET_SAMPLE((b), ((i) + 3));
+    x4 = GET_SAMPLE((b), ((i) + 4));
+    x2sq = x2 * x2;
+    d = 2.0 * ((x2sq) - (x1 * x3));
+    n = ((x2sq) - (x0 * x4)) - ((x1 * x1) 
+            - (x0 * x2)) - ((x3 * x3) - (x2 * x4));
+
+/* instead of
+#ifdef FASTMATH
+    result = 0.5 * (double)fast_acosf((float)n/d);
+#else
+    result = 0.5 * acos(n/d);
+#endif
+ we do simplified, modified for speed version : */
+
+    result = n/d;
+    if (isinf(result)) {
+        if (n < 0.0)
+            return -10.0;
+        else
+            return 10.0;
+    }
+    return result;
+}
+
+#endif  /* __AVMD_DESA2_TWEAKED_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_desa2_tweaked.h b/src/mod/applications/mod_avmd/avmd_desa2_tweaked.h
new file mode 100644 (file)
index 0000000..3013f34
--- /dev/null
+++ b/src/mod/applications/mod_avmd/avmd_desa2_tweaked.h
@@ -0,0 +1,42 @@
+/*
+ * @brief   Estimator of cosine of digital frequency.
+ * @details It is tweaked DESA implementation which
+ *          returns partial product of DESA-2 estimation
+ *          so that arc cosine transform can be ommited
+ *          on all computations, but these values can
+ *          be checked for convergence in the same time.
+ *          If the partial results converge then frequency
+ *          converges too.
+ * @author  Piotr Gregor < piotrek.gregor gmail.com >
+ * @date    20 Mar 2016
+ */
+
+
+#ifndef __AVMD_DESA2_TWEAKED_H__
+#define __AVMD_DESA2_TWEAKED_H__
+
+
+#include <math.h>
+#include "avmd_buffer.h"
+#include <switch.h>
+
+
+/* Instead of returning digital frequency estimation using
+ *      result = 0.5 * acos(n/d),
+ * which involves expensive computation of arc cosine on
+ * each new sample, this function returns only (n/d) factor.
+ * The series of these partial DESA-2 results can be still
+ * checked for convergence, though measures and thresholds
+ * used to assess this will differ from those used for
+ * assessment of convergence of instantaneous frequency
+ * estimates since transformation of tweaked results
+ * to corresponding frequencies is nonlinear.
+ * The actual frequency estimation can be retrieved later
+ * from this partial result using
+ *      0.5 * acos(n/d)
+ */
+double avmd_desa2_tweaked(circ_buffer_t *b, size_t i,
+                            switch_core_session_t *session);
+
+
+#endif  /* __AVMD_DESA2_TWEAKED_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_fast_acosf.h b/src/mod/applications/mod_avmd/avmd_fast_acosf.h
index e70c8f936b179748fc5e1a0febd3ec13fbf10793..3c404039feef6292694a9edb12323077ec5f93a2 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_fast_acosf.h
+++ b/src/mod/applications/mod_avmd/avmd_fast_acosf.h
@@ -1,9 +1,17 @@
-#ifndef __FAST_ACOSF_H__
-#define __FAST_ACOSF_H__
+/*
+ * @brief   Fast arithmetic using precomputed arc cosine table.
+ * @author  Eric des Courtis
+ * @par     Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ */
+
+
+#ifndef __AVMD_FAST_ACOSF_H__
+#define __AVMD_FAST_ACOSF_H__
 
 
 #define ACOS_TABLE_FILENAME "/tmp/acos_table.dat"
 
+
 /*! \brief Arc cosine table initialization.
  *
  * @author Eric des Courtis
@@ -42,5 +50,4 @@ extern float fast_acosf(float x);
  */
 extern int compute_table(void);
 
-#endif
-
+#endif /* __AVMD_FAST_ACOSF_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_fir.h b/src/mod/applications/mod_avmd/avmd_fir.h
new file mode 100644 (file)
index 0000000..76888a7
--- /dev/null
+++ b/src/mod/applications/mod_avmd/avmd_fir.h
@@ -0,0 +1,17 @@
+/*
+ * @brief   Filters.
+ * @author  Piotr Gregor < piotrek.gregor gmail.com >
+ * @date    23 Mar 2016
+ */
+
+
+#ifndef __AVMD_FIR_H__
+#define __AVMD_FIR_H__
+
+
+#define DESA_MAX(a, b) (a) > (b) ? (a) : (b)
+#define MEDIAN_FILTER(a, b, c) (a) > (b) ? ((a) > (c) ? \
+                DESA_MAX((b), (c)) : a) : ((b) > (c) ? DESA_MAX((a), (c)) : (b))
+
+
+#endif

diff --git a/src/mod/applications/mod_avmd/avmd_goertzel.c b/src/mod/applications/mod_avmd/avmd_goertzel.c
index ab52e8d256b78825dab4d62c229104991896aa65..6fdfafc0c85e43e5de837d6a8025c3c0643352b5 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_goertzel.c
+++ b/src/mod/applications/mod_avmd/avmd_goertzel.c
@@ -1,17 +1,12 @@
-#ifndef __GOERTZEL_H__
+#ifndef __AVMD_GOERTZEL_H__
+
+
 #include <math.h>
 #include "avmd_goertzel.h"
 #include "avmd_buffer.h"
 
-/*! \brief Identify frequency components of a signal
- * @author Eric des Courtis
- * @param b A circular buffer
- * @param pos Position in the buffer
- * @param f Frequency to look at
- * @param num Number of samples to look at
- * @return A power estimate for frequency f at position pos in the stream
- */
-extern double goertzel(circ_buffer_t *b, size_t pos, double f, size_t num)
+
+extern double avmd_goertzel(circ_buffer_t *b, size_t pos, double f, size_t num)
 {
     double s = 0.0;
     double p = 0.0;
@@ -22,15 +17,14 @@ extern double goertzel(circ_buffer_t *b, size_t pos, double f, size_t num)
     coeff = 2.0 * cos(2.0 * M_PI * f);
 
     for (i = 0; i < num; i++) {
-           /* TODO: optimize to avoid GET_SAMPLE when possible */
-           s = GET_SAMPLE(b, i + pos) + (coeff * p) - p2;
-           p2 = p;
-           p = s;
+        /* TODO: optimize to avoid GET_SAMPLE when possible */
+        s = GET_SAMPLE(b, i + pos) + (coeff * p) - p2;
+        p2 = p;
+        p = s;
     }
 
     return (p2 * p2) + (p * p) - (coeff * p2 * p);
 }
 
 
-#endif
-
+#endif /* __AVMD_GOERTZEL_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_goertzel.h b/src/mod/applications/mod_avmd/avmd_goertzel.h
index a2bc032189131f69fd409c8eebef35d8663882f0..fefb74271b8c85f513d1a244993b2c22c6e1d65e 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_goertzel.h
+++ b/src/mod/applications/mod_avmd/avmd_goertzel.h
@@ -1,5 +1,12 @@
-#ifndef __GOERTZEL_H__
-#define __GOERTZEL_H__
+/*
+ * @brief   Goertzel algorithm.
+ * @author  Eric des Courtis
+ */
+
+
+#ifndef __AVMD_GOERTZEL_H__
+#define __AVMD_GOERTZEL_H__
+
 
 #ifndef _MSC_VER
 #include <stdint.h>
@@ -11,8 +18,16 @@
 #define M_PI 3.14159265358979323846264338327
 #endif
 
-extern double goertzel(circ_buffer_t *b, size_t pos, double f, size_t num);
 
-#endif
+/*! \brief Identify frequency components of a signal
+ * @author Eric des Courtis
+ * @param b A circular buffer
+ * @param pos Position in the buffer
+ * @param f Frequency to look at
+ * @param num Number of samples to look at
+ * @return A power estimate for frequency f at position pos in the stream
+ */
+extern double avmd_goertzel(circ_buffer_t *b, size_t pos, double f, size_t num);
 
 
+#endif /* __AVMD_GOERTZEL_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_options.h b/src/mod/applications/mod_avmd/avmd_options.h
index 33a32381d7a1c22c98fec5b718e2e1a30310c6e5..4c2216cfd2c11b5600e518bd81de933e2d472774 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_options.h
+++ b/src/mod/applications/mod_avmd/avmd_options.h
@@ -10,11 +10,13 @@
 #define __AVMD_OPTIONS_H__
 
 
-/* #define AVMD_DEBUG 1 */
+/* define/undefine this to enable/disable printing of avmd
+ * intermediate computations to log */
+/*#define AVMD_DEBUG */
 
 /* define/undef this to enable/disable reporting of beep
  * detection status after session ended */
-#define AVMD_REPORT_STATUS 1
+#define AVMD_REPORT_STATUS
 
 /* define/undefine this to enable/disable faster computation
  * of arcus cosine - table will be created mapping floats
@@ -25,7 +27,16 @@
 /* define/undefine this to classify avmd beep detection as valid
  * only when there is required number of consecutive elements
  * in the SMA buffer without reset */
-#define AVMD_REQUIRE_CONTINUOUS_STREAK 5
+#define AVMD_REQUIRE_CONTINUOUS_STREAK
+
+/* define number of samples to skip starting from the beginning
+ *  of frame and after reset */
+#define AVMD_SAMLPE_TO_SKIP_N 6
+
+/* define/undefine this to enable/disable simplified estimation
+ * of frequency based on approximation of sin(x) with (x)
+ * in the range x=[0,PI/2] */
+#define AVMD_SIMPLIFIED_ESTIMATION
 
 /* define/undefine to enable/disable avmd on incoming audio */
 #define AVMD_INBOUND_CHANNEL

diff --git a/src/mod/applications/mod_avmd/avmd_psi.h b/src/mod/applications/mod_avmd/avmd_psi.h
index 341540eb141f222857a35e1d581d67b0ddd620c3..1764897d65b647cca24abbcac6df682870bf3023 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_psi.h
+++ b/src/mod/applications/mod_avmd/avmd_psi.h
@@ -1,9 +1,9 @@
-#ifndef __PSI_H__
-#define __PSI_H__
-#include "avmd_buffer.h"
+#ifndef __AVMD_PSI_H__
+#define __AVMD_PSI_H__
 
-#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1))*GET_SAMPLE((b), ((i) + 1))-GET_SAMPLE((b), ((i) + 2))*GET_SAMPLE((b), ((i) + 0)))
 
-#endif
+#include "avmd_buffer.h"
 
+#define PSI(b, i) (GET_SAMPLE((b), ((i) + 1))*GET_SAMPLE((b), ((i) + 1))-GET_SAMPLE((b), ((i) + 2))*GET_SAMPLE((b), ((i) + 0)))
 
+#endif /* __AVMD_PSI_H__ */

diff --git a/src/mod/applications/mod_avmd/avmd_sma_buf.h b/src/mod/applications/mod_avmd/avmd_sma_buf.h
index 08790e6e69c588f9f25d4e559da9714b3eb9606c..0fcc451dab6fdaa9a20220c39fa997c0401e23b1 100644 (file)
--- a/src/mod/applications/mod_avmd/avmd_sma_buf.h
+++ b/src/mod/applications/mod_avmd/avmd_sma_buf.h
@@ -1,5 +1,15 @@
-#ifndef __SMA_BUFFER_H__
-#define __SMA_BUFFER_H__
+/*
+ * @brief   SMA buffer.
+ *
+ * @author Eric des Courtis
+ * @par    Modifications: Piotr Gregor < piotrek.gregor gmail.com >
+ */
+
+
+#ifndef __AVMD_SMA_BUFFER_H__
+#define __AVMD_SMA_BUFFER_H__
+
+
 #include <stdio.h>
 #include <stdlib.h>
 #ifndef _MSC_VER
@@ -64,7 +74,11 @@ typedef struct {
     }while(0);
 
 */
-#endif
+
+
+#endif /* __AVMD_SMA_BUFFER_H__ */
+
+
 /*
 
 int main(void)

diff --git a/src/mod/applications/mod_avmd/mod_avmd.2015.vcxproj b/src/mod/applications/mod_avmd/mod_avmd.2015.vcxproj
index 046667c95b25e78771065c775c488c35dc753a67..5c38bf2a217d9c057df5dc874f5b4345faa634c4 100644 (file)
--- a/src/mod/applications/mod_avmd/mod_avmd.2015.vcxproj
+++ b/src/mod/applications/mod_avmd/mod_avmd.2015.vcxproj
@@ -154,4 +154,4 @@
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />\r
   <ImportGroup Label="ExtensionTargets">\r
   </ImportGroup>\r
-</Project>
\ No newline at end of file
+</Project>

diff --git a/src/mod/applications/mod_avmd/mod_avmd.c b/src/mod/applications/mod_avmd/mod_avmd.c
index 357cb30a5c98085005d619382d1e4f0ebc949278..a92736bbfdb7a4e42dbc920e570da00295c148cf 100644 (file)
--- a/src/mod/applications/mod_avmd/mod_avmd.c
+++ b/src/mod/applications/mod_avmd/mod_avmd.c
@@ -27,10 +27,12 @@
  *
  * Modifications:
  *      Piotr Gregor <piotrek.gregor gmail.com>:
- *      FS-8808, FS-8809, FS-8810, FS-8852, FS-8853, FS-8854, FS-8855, FS-8860, FS-8861
+ *      FS-8808, FS-8809, FS-8810, FS-8852, FS-8853, FS-8854, FS-8855,
+ *      FS-8860, FS-8861, FS-8875
  */
 
 #include <switch.h>
+#include <g711.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -43,11 +45,8 @@
 #endif
 
 
-#include "avmd_amplitude.h"
 #include "avmd_buffer.h"
-#include "avmd_desa2.h"
-//#include "avmd_goertzel.h"
-#include "avmd_psi.h"
+#include "avmd_desa2_tweaked.h"
 #include "avmd_sma_buf.h"
 #include "avmd_options.h"
 
@@ -59,9 +58,9 @@
 /*! Calculate how many audio samples per ms based on the rate */
 #define SAMPLES_PER_MS(r, m) ((r) / (1000/(m)))
 /*! Minimum beep length */
-#define BEEP_TIME (100)
+#define BEEP_TIME (2)
 /*! How often to evaluate the output of desa2 in ms */
-#define SINE_TIME (10)
+#define SINE_TIME (2*0.125)
 /*! How long in samples does desa2 results get evaluated */
 #define SINE_LEN(r) SAMPLES_PER_MS((r), SINE_TIME)
 /*! How long in samples is the minimum beep length */
@@ -92,11 +91,11 @@
 /*! Maximum frequency as digital normalized frequency */
 #define MAX_FREQUENCY_R(r) ((2.0 * M_PI * MAX_FREQUENCY) / (r))
 /* decrease this value to eliminate false positives */
-#define VARIANCE_THRESHOLD (0.0001)
+#define VARIANCE_THRESHOLD (0.00025)
 
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
     /* increase this value to eliminate false positives */
-    #define SAMPLES_CONSECUTIVE_STREAK 3
+    #define SAMPLES_CONSECUTIVE_STREAK 15
 #endif
 
 /*! Syntax of the API call. */
@@ -108,6 +107,9 @@
 /*! FreeSWITCH CUSTOM event type. */
 #define AVMD_EVENT_BEEP "avmd::beep"
 
+#define AVMD_CHAR_BUF_LEN 10
+#define AVMD_BUF_LINEAR_LEN 160
+
 
 /* Prototypes */
 SWITCH_MODULE_SHUTDOWN_FUNCTION(mod_avmd_shutdown);
@@ -144,13 +146,16 @@ typedef struct {
        switch_time_t start_time;
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
     size_t samples_streak; /* number of DESA samples in single streak without reset
-                              needed to validate SMA estimator, half the size of SMA buffer */
+                              needed to validate SMA estimator */
 #endif
+    size_t sample_count;
 } avmd_session_t;
 
 static void avmd_process(avmd_session_t *session, switch_frame_t *frame);
-static switch_bool_t avmd_callback(switch_media_bug_t * bug, void *user_data, switch_abc_type_t type);
-static void init_avmd_session_data(avmd_session_t *avmd_session,  switch_core_session_t *fs_session);
+static switch_bool_t avmd_callback(switch_media_bug_t * bug,
+                                    void *user_data, switch_abc_type_t type);
+static void init_avmd_session_data(avmd_session_t *avmd_session,
+                                    switch_core_session_t *fs_session);
 
 
 /*! \brief The avmd session data initialization function.
@@ -158,7 +163,8 @@ static void init_avmd_session_data(avmd_session_t *avmd_session,  switch_core_se
  * @param avmd_session A reference to a avmd session.
  * @param fs_session A reference to a FreeSWITCH session.
  */
-static void init_avmd_session_data(avmd_session_t *avmd_session,  switch_core_session_t *fs_session)
+static void init_avmd_session_data(avmd_session_t *avmd_session,
+                                    switch_core_session_t *fs_session)
 {
        /*! This is a worst case sample rate estimate */
        avmd_session->rate = 48000;
@@ -175,6 +181,7 @@ static void init_avmd_session_data(avmd_session_t *avmd_session,  switch_core_se
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
     avmd_session->samples_streak = SAMPLES_CONSECUTIVE_STREAK;
 #endif
+    avmd_session->sample_count = 0;
 
        INIT_SMA_BUFFER(
                &avmd_session->sma_b,
@@ -198,7 +205,8 @@ static void init_avmd_session_data(avmd_session_t *avmd_session,  switch_core_se
  * @param type The switch callback type.
  * @return The success or failure of the function.
  */
-static switch_bool_t avmd_callback(switch_media_bug_t * bug, void *user_data, switch_abc_type_t type)
+static switch_bool_t avmd_callback(switch_media_bug_t * bug,
+                                        void *user_data, switch_abc_type_t type)
 {
        avmd_session_t *avmd_session;
        switch_codec_t *read_codec;
@@ -216,7 +224,8 @@ static switch_bool_t avmd_callback(switch_media_bug_t * bug, void *user_data, sw
                read_codec = switch_core_session_get_read_codec(avmd_session->session);
                avmd_session->rate = read_codec->implementation->samples_per_second;
                avmd_session->start_time = switch_micro_time_now();
-               /* avmd_session->vmd_codec.channels = read_codec->implementation->number_of_channels; */
+               /* avmd_session->vmd_codec.channels = 
+         *                  read_codec->implementation->number_of_channels; */
                break;
 
        case SWITCH_ABC_TYPE_READ_REPLACE:
@@ -255,13 +264,11 @@ SWITCH_MODULE_LOAD_FUNCTION(mod_avmd_load)
        /* connect my internal structure to the blank pointer passed to me */
        *module_interface = switch_loadable_module_create_module_interface(pool, modname);
 
-
        if (switch_event_reserve_subclass(AVMD_EVENT_BEEP) != SWITCH_STATUS_SUCCESS) {
                switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_ERROR,
                 "Couldn't register subclass [%s]!\n", AVMD_EVENT_BEEP);
                return SWITCH_STATUS_TERM;
        }
-
        
        switch_log_printf(
                SWITCH_CHANNEL_LOG,
@@ -340,7 +347,8 @@ SWITCH_MODULE_LOAD_FUNCTION(mod_avmd_load)
                SAF_NONE
        );
 
-       SWITCH_ADD_API(api_interface, "avmd", "Voicemail beep detection", avmd_api_main, AVMD_SYNTAX);
+       SWITCH_ADD_API(api_interface, "avmd", "Voicemail beep detection",
+                                            avmd_api_main, AVMD_SYNTAX);
 
        /* indicate that the module should continue to be loaded */
        return SWITCH_STATUS_SUCCESS;
@@ -386,7 +394,8 @@ SWITCH_STANDARD_APP(avmd_start_function)
                return;
        }
 
-       avmd_session = (avmd_session_t *)switch_core_session_alloc(session, sizeof(avmd_session_t));
+       avmd_session = (avmd_session_t *)switch_core_session_alloc(
+                                            session, sizeof(avmd_session_t));
 
        init_avmd_session_data(avmd_session, session);
 
@@ -554,7 +563,8 @@ SWITCH_STANDARD_API(avmd_api_main)
 
        /* Allocate memory attached to this FreeSWITCH session for
        * use in the callback routine and to store state information */
-       avmd_session = (avmd_session_t *) switch_core_session_alloc(fs_session, sizeof(avmd_session_t));
+       avmd_session = (avmd_session_t *) switch_core_session_alloc(
+                                            fs_session, sizeof(avmd_session_t));
 
        init_avmd_session_data(avmd_session, fs_session);
 
@@ -624,18 +634,16 @@ static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
 
        circ_buffer_t *b;
        size_t pos;
-       double f;
+       double omega;
+#ifdef AVMD_DEBUG
+    double f;
+#endif
        double v;
-       //    double error = 0.0;
-       //    double success = 0.0;
-       //    double amp = 0.0;
-       //    double s_rate;
-       //    double e_rate;
-       //    double avg_a;
-       //double sine_len;
+    double sma_digital_freq;
        uint32_t sine_len_i;
-       //uint32_t beep_len_i;
-       //    int valid;
+    char buf[AVMD_CHAR_BUF_LEN];
+    int sample_to_skip_n = AVMD_SAMLPE_TO_SKIP_N;
+    size_t sample_n = 0;
 
        b = &session->b;
 
@@ -651,50 +659,109 @@ static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
 
        /* Insert frame of 16 bit samples into buffer */
        INSERT_INT16_FRAME(b, (int16_t *)(frame->data), frame->samples);
+    session->sample_count += frame->samples;
 
-       //switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_INFO, "<<< AVMD sine_len_i=%d >>>\n", sine_len_i);
-
-       /* INNER LOOP -- OPTIMIZATION TARGET */
-       for (pos = session->pos; pos < (GET_CURRENT_POS(b) - P); pos++) {
-               if ((pos % sine_len_i) == 0) {
+    /* INNER LOOP -- OPTIMIZATION TARGET */
+    pos = session->pos;
+    while (sample_n < (frame->samples - P)) {
+       /*for (pos = session->pos; pos < (GET_CURRENT_POS(b) - P); pos++) { */
+               if ((sample_n % sine_len_i) == 0) {
                        /* Get a desa2 frequency estimate every sine len */
-                       f = desa2(b, pos);
+                       omega = avmd_desa2_tweaked(b, pos + sample_n, session->session);
 
-                       if (f < MIN_FREQUENCY_R(session->rate) || f > MAX_FREQUENCY_R(session->rate)) {
+                       if (omega < -0.999999 || omega > 0.999999) {
+#ifdef AVMD_DEBUG
+                               switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session),
+                        SWITCH_LOG_DEBUG, "<<< AVMD RESET >>>\n");
+#endif
                                v = 99999.0;
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
                                RESET_SMA_BUFFER(&session->sma_b);
                                RESET_SMA_BUFFER(&session->sqa_b);
                 session->samples_streak = SAMPLES_CONSECUTIVE_STREAK;
+                sample_to_skip_n = AVMD_SAMLPE_TO_SKIP_N;
 #endif
                        } else {
-                               APPEND_SMA_VAL(&session->sma_b, f);
-                               APPEND_SMA_VAL(&session->sqa_b, f * f);
+                if (isnan(omega)) {
+#ifdef AVMD_DEBUG
+                       switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session),
+                            SWITCH_LOG_DEBUG, "<<< AVMD, SKIP NaN >>>\n");
+#endif
+                    sample_to_skip_n = AVMD_SAMLPE_TO_SKIP_N;
+                    goto loop_continue;
+                }
+                if (session->sma_b.pos > 0 && 
+                        (fabs(omega - session->sma_b.data[session->sma_b.pos - 1]) < 0.00000001)) {
+#ifdef AVMD_DEBUG
+                       switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_DEBUG,
+                        "<<< AVMD, SKIP >>>\n");
+#endif
+                    goto loop_continue;
+                }
+#ifdef AVMD_DEBUG
+                   switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session),
+                        SWITCH_LOG_DEBUG, "<<< AVMD omega [%f] >>>\n", omega);
+#endif
+                if (sample_to_skip_n > 0) {
+                    sample_to_skip_n--;
+                    goto loop_continue;
+                }
+
+                /* saturate */
+                if (omega < -0.9999)
+                    omega = -0.9999;
+                if (omega > 0.9999)
+                    omega = 0.9999;
+
+                /* append */
+                               APPEND_SMA_VAL(&session->sma_b, omega);
+                               APPEND_SMA_VAL(&session->sqa_b, omega * omega);
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
                 if (session->samples_streak > 0)
                     --session->samples_streak;
 #endif
-                               /* calculate variance */
+                               /* calculate variance (biased estimator) */
                                v = session->sqa_b.sma - (session->sma_b.sma * session->sma_b.sma);
 #ifdef AVMD_DEBUG
-#ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
+    #ifdef  AVMD_FAST_MATH
+                f =  0.5 * (double) fast_acosf((float)omega);
+                sma_digital_freq =  0.5 * (double) fast_acosf((float)session->sma_b.sma);
+    #else
+                f = 0.5 * acos(omega);
+                sma_digital_freq =  0.5 * acos(session->sma_b.sma);
+    #endif /* AVMD_FAST_MATH */
+    #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
                                switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_DEBUG,
-                    "<<< AVMD v[%f] f[%f] [%f]Hz\tsma[%f][%f]Hz\tsqa[%f]\tstreak[%zu] pos[%zu] >>>\n", v, f, TO_HZ(session->rate, f),
-                    session->sma_b.sma, TO_HZ(session->rate, session->sma_b.sma), session->sqa_b.sma, session->samples_streak, session->sma_b.pos);
-#else
+                    "<<< AVMD v[%.10f]\tomega[%f]\tf[%f] [%f]Hz\t\tsma[%f][%f]Hz\t\tsqa[%f]\t"
+                    "streak[%zu] pos[%zu] sample_n[%zu] lpos[%zu] s[%zu]>>>\n",
+                    v, omega, f, TO_HZ(session->rate, f), session->sma_b.sma,
+                    TO_HZ(session->rate, sma_digital_freq), session->sqa_b.sma, session->samples_streak,
+                    session->sma_b.pos, sample_n, session->sma_b.lpos, pos);
+    #else
                                switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_DEBUG,
-                    "<<< AVMD v[%f] f[%f] [%f]Hz\tsma[%f][%f]Hz\tsqa[%f]\tpos[%zu] >>>\n", v, f, TO_HZ(session->rate, f),
-                    session->sma_b.sma, TO_HZ(session->rate, session->sma_b.sma), session->sqa_b.sma, session->sma_b.pos);
-#endif
-#endif
+                    "<<< AVMD v[%.10f]\tomega[%f]\tf[%f] [%f]Hz\t\tsma[%f][%f]Hz\t\tsqa[%f]\tpos[%zu]"
+                    " sample_n[%zu] lpos[%zu] s[%zu]>>>\n", v, omega, f,
+                    TO_HZ(session->rate, f), session->sma_b.sma, TO_HZ(session->rate, sma_digital_freq),
+                    session->sqa_b.sma, session->sma_b.pos, sample_n, session->sma_b.lpos, pos);
+    #endif  /* AVMD_REQUIRE_CONTINUOUS_STREAK */
+#endif  /* AVMD_DEBUG */
                        }
 
-                       /* If variance is less than threshold then we have detection */
+            /* DECISION */
+                       /* If variance is less than threshold
+             * and we have at least two estimates
+             * then we have detection */
 #ifdef AVMD_REQUIRE_CONTINUOUS_STREAK
-                       if (v < VARIANCE_THRESHOLD && (session->sma_b.pos > 1) && (session->samples_streak == 0)) {
+                       if (v < VARIANCE_THRESHOLD && (session->sma_b.lpos > 1) && (session->samples_streak == 0)) {
 #else
-                       if (v < VARIANCE_THRESHOLD && (session->sma_b.pos > 1)) {
+                       if (v < VARIANCE_THRESHOLD && (session->sma_b.lpos > 1)) {
 #endif
+    #ifdef  AVMD_FAST_MATH
+                sma_digital_freq =  0.5 * (double) fast_acosf((float)session->sma_b.sma);
+    #else
+                sma_digital_freq =  0.5 * acos(session->sma_b.sma);
+    #endif /* AVMD_FAST_MATH */
+                snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", TO_HZ(session->rate, sma_digital_freq));
                                switch_channel_set_variable_printf(channel, "avmd_total_time",
                         "[%d]", (int)(switch_micro_time_now() - session->start_time) / 1000);
                                switch_channel_execute_on(channel, "execute_on_avmd_beep");
@@ -707,6 +774,9 @@ static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
                                switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "Unique-ID",
                     switch_core_session_get_uuid(session->session));
                                switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "call-command", "avmd");
+                               switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "frequency", buf);
+                snprintf(buf, AVMD_CHAR_BUF_LEN, "%f", v);
+                               switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, "variance", buf);
 
                                if ((switch_event_dup(&event_copy, event)) != SWITCH_STATUS_SUCCESS) return;
 
@@ -714,22 +784,25 @@ static void avmd_process(avmd_session_t *session, switch_frame_t *frame)
                                switch_event_fire(&event_copy);
 
 #ifdef AVMD_REPORT_STATUS
-                               switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_DEBUG,
-                        "<<< AVMD - Beep Detected f = [%f] >>>\n", TO_HZ(session->rate, session->sma_b.sma));
+                               switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(session->session), SWITCH_LOG_NOTICE,
+                        "<<< AVMD - Beep Detected: f = [%f], variance = [%f] >>>\n",
+                        TO_HZ(session->rate, sma_digital_freq), v);
 #endif
                                switch_channel_set_variable(channel, "avmd_detect", "TRUE");
                                RESET_SMA_BUFFER(&session->sma_b);
                                RESET_SMA_BUFFER(&session->sqa_b);
                                session->state.beep_state = BEEP_DETECTED;
 
-                               return;
+                               goto done;
             }
-                       //amp = 0.0;
-                       //success = 0.0;
-                       //error = 0.0;
                }
+loop_continue:
+        ++sample_n;
        }
-       session->pos = pos;
+
+done:
+       session->pos += sample_n;
+    session->pos &= b->mask;
 
     return;
 }