From 281d4bf9212c22a3b64902808bc15782a83a4ae9 Mon Sep 17 00:00:00 2001
From: "J.A. de Jong - Redu-Sone B.V., ASCEE V.O.F" <j.a.dejong@ascee.nl>
Date: Fri, 21 Feb 2020 15:11:11 +0100
Subject: [PATCH] Added wrapper for equalizer.

---
 lasp/c/CMakeLists.txt |  1 +
 lasp/c/lasp_eq.c      |  8 +++++--
 lasp/c/lasp_eq.h      |  2 +-
 lasp/wrappers.pyx     | 55 +++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 63 insertions(+), 3 deletions(-)

diff --git a/lasp/c/CMakeLists.txt b/lasp/c/CMakeLists.txt
index ef8caf4..7eb9403 100644
--- a/lasp/c/CMakeLists.txt
+++ b/lasp/c/CMakeLists.txt
@@ -1,6 +1,7 @@
 if(!LASP_DEBUG)
 SET_SOURCE_FILES_PROPERTIES(lasp_sosfilterbank.c PROPERTIES COMPILE_FLAGS -O3)
 SET_SOURCE_FILES_PROPERTIES(lasp_slm.c PROPERTIES COMPILE_FLAGS -O3)
+SET_SOURCE_FILES_PROPERTIES(lasp_eq.c PROPERTIES COMPILE_FLAGS -O3)
 endif(!LASP_DEBUG)
 
 add_library(lasp_lib
diff --git a/lasp/c/lasp_eq.c b/lasp/c/lasp_eq.c
index dfc84d0..7c90344 100644
--- a/lasp/c/lasp_eq.c
+++ b/lasp/c/lasp_eq.c
@@ -28,6 +28,7 @@ vd Eq_equalize(Eq* eq,const vd* input_data) {
 
     for(us filter=0;filter<eq->nfilters;filter++) {
         d ampl = *getvdval(&(eq->ampl_values), filter);
+        /// TODO: Replace this code with something more fast from BLAS.
         for(us sample=0;sample<filtered.n_rows;sample++) {
             d* res = getvdval(&result, sample);
             *res = *res + *getdmatval(&filtered, sample, filter) * ampl;
@@ -39,12 +40,15 @@ vd Eq_equalize(Eq* eq,const vd* input_data) {
     return result;
 }
 
-vd Eq_setLevels(Eq* eq,const vd* levels) {
+void Eq_setLevels(Eq* eq,const vd* levels) {
     fsTRACE(15);
     assertvalidptr(eq);
     assert_vx(levels);
     dbgassert(levels->n_rows == eq->nfilters, "Invalid levels size");
-
+    for(us ch=0;ch<eq->nfilters;ch++){
+        d level = *getvdval(levels, ch);
+        *getvdval(&(eq->ampl_values), ch) = d_pow(10, level/20);
+    }
 
     feTRACE(15);
 }
diff --git a/lasp/c/lasp_eq.h b/lasp/c/lasp_eq.h
index 92573ce..48eb231 100644
--- a/lasp/c/lasp_eq.h
+++ b/lasp/c/lasp_eq.h
@@ -24,7 +24,7 @@ Eq* Eq_create(Sosfilterbank* fb);
  *
  * @param[in] eq Equalizer handle
  * @param[in] input_data Input data to equalize
- * @return Equalized data
+ * @return Equalized data. Newly allocated vector. Ownership is transferred.
  * */
 vd Eq_equalize(Eq* eq,const vd* input_data);
 
diff --git a/lasp/wrappers.pyx b/lasp/wrappers.pyx
index 91af69b..0d34719 100644
--- a/lasp/wrappers.pyx
+++ b/lasp/wrappers.pyx
@@ -674,3 +674,58 @@ cdef class Siggen:
         siggen = Siggen()
         siggen._siggen = c_siggen
         return siggen
+
+cdef extern from "lasp_eq.h" nogil:
+    ctypedef struct c_Eq "Eq"
+    c_Eq* Eq_create(c_Sosfilterbank* fb)
+    vd Eq_equalize(c_Eq* eq,const vd* input_data) 
+    us Eq_getNLevels(const c_Eq* eq)
+    void Eq_setLevels(c_Eq* eq, const vd* levels)
+    void Eq_free(c_Eq* eq)
+
+cdef class Equalizer:
+    cdef:
+        c_Eq* ceq
+
+    def __cinit__(self, SosFilterBank cdef_fb):
+        """
+        Initialize equalizer using given filterbank. Note: Steals pointer of
+        underlying c_Sosfilterbank!!
+        """
+        self.ceq = Eq_create(cdef_fb.fb)
+        # Set this pointer to NULL, such that the underlying c_SosfilterBank is
+        # not deallocated.
+        cdef_fb.fb = NULL
+
+    def getNLevels(self):
+        return Eq_getNLevels(self.ceq)
+
+    def setLevels(self,d[:] new_levels):
+        cdef dmat dmat_new_levels = dmat_foreign_data(new_levels.shape[0],
+                                        1,
+                                        &new_levels[0],
+                                        False)
+        Eq_setLevels(self.ceq, &dmat_new_levels) 
+        dmat_free(&dmat_new_levels)
+
+    def equalize(self, d[::1, :] input_data):
+        assert input_data.shape[1] == 1
+        cdef:
+            vd res
+        cdef dmat input_dmat = dmat_foreign_data(input_data.shape[0],
+                                        1,
+                                        &input_data[0,0],
+                                        False)
+        with nogil:
+            res = Eq_equalize(self.ceq, &input_dmat)
+
+        # Steal the pointer from output
+        py_res = dmat_to_ndarray(&res,True)
+
+        dmat_free(&res)
+        vd_free(&input_dmat)
+        return py_res
+
+    def __dealloc__(self):
+        if self.ceq:
+            Eq_free(self.ceq)