kfr

commit 641d691bbc50ddec3f17676ccc98017af29c3e22
parent ce2f289e6a86245d194da54efcb5244d01edf234
Author: [email protected] <[email protected]>
Date:   Mon, 16 Oct 2023 09:17:28 +0100

Fix Goertzel

Diffstat:
M
include/kfr/dsp/goertzel.hpp
 | 
29
+++++++++++++++++++++--------
M
tests/dsp_test.cpp
 | 
21
+++++++++++++++++++++

2 files changed, 42 insertions(+), 8 deletions(-)
diff --git a/include/kfr/dsp/goertzel.hpp b/include/kfr/dsp/goertzel.hpp
@@ -38,10 +38,15 @@ inline namespace CMT_ARCH_NAME
 template <typename T>
 struct expression_goertzel : expression_traits_defaults
 {
-    using value_type = accepts_any;
+    constexpr static size_t dims = 1;    
+
+    using value_type = T;
+    
+    constexpr static shape<1> get_shape(const expression_goertzel&) { return shape<1>(infinite_size); }
+    constexpr static shape<1> get_shape() { return shape<1>(infinite_size); }
 
     expression_goertzel(complex<T>& result, T omega)
-        : result(result), omega(omega), coeff(2 * cos(omega)), q0(), q1(), q2()
+        : result(result), omega(omega), coeff(2 * cos(omega)), q0(0), q1(0), q2(0)
     {
     }
     ~expression_goertzel()
@@ -49,8 +54,10 @@ struct expression_goertzel : expression_traits_defaults
         result.real(q1 - q2 * cos(omega));
         result.imag(q2 * sin(omega));
     }
-    template <typename U, size_t N>
-    KFR_INTRINSIC friend void set_elements(expression_goertzel& self, shape<1>, const vec<U, N>& x)
+    
+    template <size_t N, index_t VecAxis>
+    friend KFR_INTRINSIC void set_elements(expression_goertzel& self, shape<1>, axis_params<VecAxis, N>,
+                                           const identity<vec<T, N>>& x)
     {
         vec<T, N> in = x;
         CMT_LOOP_UNROLL
@@ -72,10 +79,15 @@ struct expression_goertzel : expression_traits_defaults
 template <typename T, size_t width>
 struct expression_parallel_goertzel : expression_traits_defaults
 {
-    using value_type = accepts_any;
+    constexpr static size_t dims = 1;    
+
+    using value_type = T;
+    
+    constexpr static shape<1> get_shape(const expression_parallel_goertzel&) { return shape<1>(infinite_size); }
+    constexpr static shape<1> get_shape() { return shape<1>(infinite_size); }
 
     expression_parallel_goertzel(complex<T> result[], vec<T, width> omega)
-        : result(result), omega(omega), coeff(cos(omega)), q0(), q1(), q2()
+        : result(result), omega(omega), coeff(2 * cos(omega)), q0(T(0)), q1(T(0)), q2(T(0))
     {
     }
     ~expression_parallel_goertzel()
@@ -88,8 +100,9 @@ struct expression_parallel_goertzel : expression_traits_defaults
             result[i].imag(im[i]);
         }
     }
-    template <typename U, size_t N>
-    KFR_INTRINSIC friend void set_elements(expression_parallel_goertzel& self, shape<1>, const vec<U, N>& x)
+    template <size_t N, index_t VecAxis>
+    friend KFR_INTRINSIC void set_elements(expression_parallel_goertzel& self, shape<1>, axis_params<VecAxis, N>,
+                                           const identity<vec<T, N>>& x)
     {
         const vec<T, N> in = x;
         CMT_LOOP_UNROLL
diff --git a/tests/dsp_test.cpp b/tests/dsp_test.cpp
@@ -18,6 +18,27 @@ using namespace kfr;
 namespace CMT_ARCH_NAME
 {
 
+TEST(goertzel)
+{
+    testo::eplison_scope<float> e(100);
+    univector<float, 16> a;
+    a = sinenorm(phasor(0.125f));
+
+    float omega = c_pi<float, 2> * 0.125f;
+
+    complex<float> c;
+    process(goertzel(c, omega), a);
+    CHECK(cabs(c) == 8.f);
+
+    complex<float> cs[3];
+    float omegas[3] = { omega, omega, omega };
+    process(goertzel(cs, omegas), a);
+    println(cs[0]);
+    CHECK(cabs(cs[0]) == 8.f);
+    CHECK(cabs(cs[1]) == 8.f);
+    CHECK(cabs(cs[2]) == 8.f);
+}
+
 TEST(delay)
 {
     const univector<float, 33> v1 = counter() + 100;