From 35d2fb71aed001cbeeb760361648320d888ceee1 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: Mon, 2 Mar 2020 22:03:35 +0100
Subject: [PATCH] First work on noise coloring filter implementations

---
 lasp/c/lasp_siggen.c        |  2 +-
 lasp/c/lasp_siggen.h        | 20 ++++++-------
 lasp/filter/__init__.py     |  1 +
 lasp/filter/colorednoise.py | 57 +++++++++++++++++++++++++++++++++++++
 lasp/wrappers.pyx           |  2 ++
 5 files changed, 71 insertions(+), 11 deletions(-)
 create mode 100644 lasp/filter/colorednoise.py

diff --git a/lasp/c/lasp_siggen.c b/lasp/c/lasp_siggen.c
index ee8c0e5..5068007 100644
--- a/lasp/c/lasp_siggen.c
+++ b/lasp/c/lasp_siggen.c
@@ -78,7 +78,7 @@ Siggen* Siggen_Sinewave_create(const d fs, const d freq,const d level_dB) {
     return sine;
 }
 
-Siggen* Siggen_Whitenoise_create(const d fs, const d level_dB) {
+Siggen* Siggen_Noise_create(const d fs, const d level_dB, Sosfilterbank* colorfilter) {
     fsTRACE(15);
 
     Siggen* whitenoise = Siggen_create(WHITENOISE, fs, level_dB);
diff --git a/lasp/c/lasp_siggen.h b/lasp/c/lasp_siggen.h
index 553087e..e9ed786 100644
--- a/lasp/c/lasp_siggen.h
+++ b/lasp/c/lasp_siggen.h
@@ -10,6 +10,7 @@
 #ifndef LASP_SIGGEN_H
 #define LASP_SIGGEN_H
 #include "lasp_mat.h"
+#include "lasp_sosfilterbank.h"
 
 typedef struct Siggen Siggen;
 
@@ -23,19 +24,18 @@ typedef struct Siggen Siggen;
 Siggen* Siggen_Sinewave_create(const d fs,const d freq,const d level_dB);
 
 /**
- * Create a white noise signal generator
- * 
- * @return Siggen* handle
- */
-Siggen* Siggen_Whitenoise_create(const d fs, const d level_dB);
-
-/**
- * Create a pink (1/f) noise signal generator
- * 
+ * Create a Noise signal generator. If no Sosfilterbank is provided, it will
+ * create white noise. Otherwise, the noise is 'colored' using the filterbank
+ * given in the constructor. Note that the pointer to this filterbank is
+ * *STOLEN*!.
+ *
  * @param[in] fs: Sampling frequency [Hz]
+ * @param[in] level_dB: Relative level [dB]
+ * @param[in]
+ * 
  * @return Siggen* handle
  */
-Siggen* Siggen_Pinknoise_create(const us fs,const d level_dB);
+Siggen* Siggen_Noise_create(const d fs, const d level_dB, Sosfilterbank* colorfilter);
 
 // Define this flag to repeat a forward sweep only, or backward only. If not
 // set, we do a continuous sweep
diff --git a/lasp/filter/__init__.py b/lasp/filter/__init__.py
index df9b2dd..f354314 100644
--- a/lasp/filter/__init__.py
+++ b/lasp/filter/__init__.py
@@ -1,4 +1,5 @@
 from .soundpressureweighting import *
 from .filterbank_design import *
 from .fir_design import *
+from .colorednoise import *
 
diff --git a/lasp/filter/colorednoise.py b/lasp/filter/colorednoise.py
new file mode 100644
index 0000000..57ddc48
--- /dev/null
+++ b/lasp/filter/colorednoise.py
@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""!
+Author: J.A. de Jong - ASCEE
+
+Description: Implementation of SOS filters to color a signal. Typically used to
+color white noise to i.e. Pink noise, or Brownian noise.
+"""
+from scipy.signal import bilinear_zpk, zpk2sos, freqz_zpk
+import numpy as np
+
+__all__ = ['PinkNoise'] #, 'BrownianNoise', 'BlueNoise']
+
+def PinkNoise(fs, fstart=10, fend=None, N=3):
+    """
+    Creates SOS filter for pink noise. The filter has a flat response below
+    fstart, and rolls of close to Nyquist, or flattens out above fend. The
+    ripple (i.e.) closeness the filter rolls of depends on the number of
+    sections. The default, N=3 results in
+
+    Args:
+        fs: Sampling frequency [Hz]
+        fstart: Frequency of first pole of the filter
+        fend: Frequency of last pole of the filter, if not given, set to 5/6th
+        of the Nyquist frequency.
+        N: Number of sections.
+
+    Returns:
+        sos: Array of digital filter coefficients
+
+    """
+    order = N*2
+    if fend is None:
+        fend = 5*fs/6/2
+    # Not the fastest implementation, but this will not be the bottleneck
+    # anyhow.
+    fpoles = np.array([fstart*(fend/fstart)**(n/(order-1)) 
+                       for n in range(order)])
+
+    # Put zeros inbetween poles
+    fzeros = np.sqrt(fpoles[1:]*fpoles[:-1])
+
+    poles = -2*np.pi*fpoles
+    zeros = -2*np.pi*fzeros
+
+    z,p,k = bilinear_zpk(zeros, poles,1, fs=fs)
+
+    # Compute the frequency response and search for the gain at fstart, we
+    # normalize such that this gain is ~ 0 dB. Rounded to an integer frequency
+    # in Hz.
+    Omg, h = freqz_zpk(z, p, k, worN = int(fs/2), fs=fs)
+    h_fstart = np.abs(h[int(fstart)])
+    k *= 1/h_fstart
+    
+    sos = zpk2sos(z,p,k)
+
+    return sos
diff --git a/lasp/wrappers.pyx b/lasp/wrappers.pyx
index bd6c53e..8e5124f 100644
--- a/lasp/wrappers.pyx
+++ b/lasp/wrappers.pyx
@@ -637,6 +637,8 @@ sweep_flag_linear = SWEEP_FLAG_LINEAR
 sweep_flag_exponential = SWEEP_FLAG_EXPONENTIAL
 sweep_flag_hyperbolic = SWEEP_FLAG_HYPERBOLIC
 
+from .filter import PinkNoise
+
 cdef class Siggen:
 
     cdef c_Siggen *_siggen