From ced45c7638ac622b309bda982e62c3e3dcee475e 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: Wed, 12 Feb 2020 12:36:11 +0100
Subject: [PATCH] First work on equalizer

---
 lasp/c/CMakeLists.txt |  1 +
 lasp/c/lasp_eq.c      | 66 +++++++++++++++++++++++++++++++++++++++++++
 lasp/c/lasp_eq.h      | 60 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 127 insertions(+)
 create mode 100644 lasp/c/lasp_eq.c
 create mode 100644 lasp/c/lasp_eq.h

diff --git a/lasp/c/CMakeLists.txt b/lasp/c/CMakeLists.txt
index 00bb27f..ef8caf4 100644
--- a/lasp/c/CMakeLists.txt
+++ b/lasp/c/CMakeLists.txt
@@ -21,6 +21,7 @@ add_library(lasp_lib
   lasp_sosfilterbank.c
   lasp_decimation.c
   lasp_slm.c
+  lasp_eq.c
   )
 
 
diff --git a/lasp/c/lasp_eq.c b/lasp/c/lasp_eq.c
new file mode 100644
index 0000000..dfc84d0
--- /dev/null
+++ b/lasp/c/lasp_eq.c
@@ -0,0 +1,66 @@
+#include "lasp_eq.h"
+#include "lasp_assert.h"
+
+typedef struct Eq {
+    Sosfilterbank* fb;
+    us nfilters;
+    vd ampl_values;
+} Eq;
+
+Eq* Eq_create(Sosfilterbank* fb) {
+    fsTRACE(15);
+    assertvalidptr(fb);
+    Eq* eq = a_malloc(sizeof(Eq));
+    eq->fb = fb;
+    eq->nfilters = Sosfilterbank_getFilterbankSize(fb);
+    eq->ampl_values = vd_alloc(eq->nfilters);
+    vd_set(&(eq->ampl_values), 1.0);
+    feTRACE(15);
+    return eq;
+}
+vd Eq_equalize(Eq* eq,const vd* input_data) {
+    fsTRACE(15);
+    assertvalidptr(eq);
+    assert_vx(input_data);
+    vd result = vd_alloc(input_data->n_rows);
+    dmat filtered = Sosfilterbank_filter(eq->fb, input_data);
+    dmat_set(&filtered, 0);
+
+    for(us filter=0;filter<eq->nfilters;filter++) {
+        d ampl = *getvdval(&(eq->ampl_values), filter);
+        for(us sample=0;sample<filtered.n_rows;sample++) {
+            d* res = getvdval(&result, sample);
+            *res = *res + *getdmatval(&filtered, sample, filter) * ampl;
+        }
+    }
+    dmat_free(&filtered);
+
+    feTRACE(15);
+    return result;
+}
+
+vd Eq_setLevels(Eq* eq,const vd* levels) {
+    fsTRACE(15);
+    assertvalidptr(eq);
+    assert_vx(levels);
+    dbgassert(levels->n_rows == eq->nfilters, "Invalid levels size");
+
+
+    feTRACE(15);
+}
+
+us Eq_getNLevels(const Eq* eq) {
+    fsTRACE(15);
+    assertvalidptr(eq);
+    feTRACE(15);
+    return eq->nfilters;
+}
+
+void Eq_free(Eq* eq) {
+    fsTRACE(15);
+    assertvalidptr(eq);
+    assertvalidptr(eq->fb);
+    Sosfilterbank_free(eq->fb);
+    vd_free(&(eq->ampl_values));
+    feTRACE(15);
+}
diff --git a/lasp/c/lasp_eq.h b/lasp/c/lasp_eq.h
new file mode 100644
index 0000000..92573ce
--- /dev/null
+++ b/lasp/c/lasp_eq.h
@@ -0,0 +1,60 @@
+// lasp_eq.h
+//
+// Author: J.A. de Jong - ASCEE
+//
+// Description: Implementation of an equalizer using the Second Order Sections
+// filter bank implementation. 
+#pragma once
+#ifndef LASP_EQ_H
+#define LASP_EQ_H
+#include "lasp_sosfilterbank.h"
+typedef struct Eq Eq;
+
+/**
+ * Initialize an equalizer using the given Filterbank. Note: takes over pointer
+ * ownership of fb! Sets levels of all filterbanks initially to 0 dB.
+ *
+ * @param[in] fb Filterbank to be used in equalizer
+ * @return Equalizer handle, NULL on error
+ * */
+Eq* Eq_create(Sosfilterbank* fb);
+
+/**
+ * Equalize a given piece of data using current settings of the levels.
+ *
+ * @param[in] eq Equalizer handle
+ * @param[in] input_data Input data to equalize
+ * @return Equalized data
+ * */
+vd Eq_equalize(Eq* eq,const vd* input_data);
+
+/**
+ * Returns number of channels of the equalizer. Note: takes over pointer
+ * ownership of fb!
+ *
+ * @param[in] eq Equalizer handle
+ * */
+us Eq_getNLevels(const Eq* eq);
+
+/**
+ * Set amplification values for each filter in the equalizer.
+ *
+ * @param[in] eq Equalizer handle
+ * @param[in] levels: Vector with level values for each channel. Should have
+ * length equal to the number of filters in the filterbank.
+ * */
+void Eq_setLevels(Eq* eq, const vd* levels);
+
+/** 
+ * Cleans up an existing Equalizer
+ *
+ * @param[in] eq Equalizer handle
+ */
+void Eq_free(Eq* eq);
+
+    
+        
+
+#endif // LASP_EQ_H
+// //////////////////////////////////////////////////////////////////////
+