lasp/src/lasp/c/lasp_firfilterbank.c

// lasp_filterbank.c
//
// Author: J.A. de Jong -ASCEE
// 
// Description:
// Firfilterbank implementation.
//////////////////////////////////////////////////////////////////////
#define TRACERPLUS (-5)
#include "lasp_firfilterbank.h"
#include "lasp_fft.h"
#include "lasp_dfifo.h"
#include "lasp_tracer.h"
#include "lasp_alg.h"
#define FIFO_SIZE_MULT 2

typedef struct Firfilterbank_s {
    us nfft;

    us P_m_1;                   /**< Filter length minus one */

    cmat filters;               /* Frequency-domain filter
                                 * coefficients */
    dFifo* output_fifo;
    dFifo* input_fifo;

    Fft* fft;                   /* Handle to internal FFT-function */

} Firfilterbank;

Firfilterbank* Firfilterbank_create(const dmat* h,
                              const us nfft) {

    fsTRACE(15);
    dbgassert(h,NULLPTRDEREF);
    const us P = h->n_rows;
    const us nfilters = h->n_cols;
    
    if(P > nfft/2) {
        WARN("Filter order should be <= nfft/2");
        return NULL;
    }

    Fft* fft = Fft_create(nfft);
    if(!fft) {
        WARN("Fft allocation failed");
        return NULL;
    }

    Firfilterbank* fb = a_malloc(sizeof(Firfilterbank));

    fb->nfft = nfft;
    fb->P_m_1 = P-1;
    fb->fft = fft;
    fb->filters = cmat_alloc(nfft/2+1,nfilters);

    fb->output_fifo = dFifo_create(nfilters,FIFO_SIZE_MULT*nfft);
    fb->input_fifo = dFifo_create(1,FIFO_SIZE_MULT*nfft);

    // Initialize the input fifo with zeros.
    // dmat init_zero = dmat_alloc(nfft - P, 1);
    // dmat_set(&init_zero,0);
    // dFifo_push(fb->input_fifo, &init_zero);
    // dmat_free(&init_zero);

    /* Create a temporary buffer which is going to be FFT'th to
     * contain the filter transfer functions.
     */
    dmat temp = dmat_alloc(nfft,nfilters);
    dmat_set(&temp,0);
    dmat_copy_rows(&temp,h,0,0,h->n_rows);

    /* Fft the FIR impulse responses */
    Fft_fft(fb->fft,&temp,&fb->filters);

    dmat_free(&temp);

    feTRACE(15);
    return fb;
}
void Firfilterbank_free(Firfilterbank* fb) {
    fsTRACE(15);
    dbgassert(fb,NULLPTRDEREF);
    cmat_free(&fb->filters);
    dFifo_free(fb->input_fifo);
    dFifo_free(fb->output_fifo);
    Fft_free(fb->fft);
    a_free(fb);
    feTRACE(15);
}
dmat Firfilterbank_filter(Firfilterbank* fb,
                       const vd* x) {

    fsTRACE(15);
    dbgassert(fb && x ,NULLPTRDEREF);

    dFifo* input_fifo = fb->input_fifo;
    dFifo* output_fifo = fb->output_fifo;
 
    const us nfft = fb->nfft;
    const us nfilters = fb->filters.n_cols;

    /* Push samples to the input fifo */
    dFifo_push(fb->input_fifo,x);

    dmat input_block = dmat_alloc(nfft,1);

    /* FFT'th filter coefficients */
    cmat input_fft = cmat_alloc(nfft/2+1,1);

    /* Output of the fast convolution */
    cmat output_fft = cmat_alloc(nfft/2+1,nfilters);

    /* Inverse FFT'th output */
    dmat output_block = dmat_alloc(nfft,nfilters);

    while (dFifo_size(input_fifo) >= nfft) {

        us nsamples = dFifo_pop(input_fifo,
                                &input_block,
                                fb->P_m_1 /* save P-1 samples */
            );
        
        dbgassert(nsamples == nfft,"BUG in dFifo");

        Fft_fft(fb->fft,&input_block,&input_fft);

        vc input_fft_col = cmat_column(&input_fft,0);

        for(us col=0;col<nfilters;col++) {
            vc output_fft_col = cmat_column(&output_fft,col);
            vc filter_col = cmat_column(&fb->filters,col);

            vc_hadamard(&output_fft_col,
                        &input_fft_col,
                        &filter_col);
                        
            vc_free(&output_fft_col);
            vc_free(&filter_col);
            
        }
        
        vc_free(&input_fft_col);

        Fft_ifft(fb->fft,&output_fft,&output_block);

        dmat valid_samples = dmat_submat(&output_block,
                                         fb->P_m_1,0, /* startrow, startcol */
                                         nfft-fb->P_m_1, /* Number of rows */
                                         output_block.n_cols);

        /* Push the valid samples to the output FIFO */
        dFifo_push(fb->output_fifo,&valid_samples);
        dmat_free(&valid_samples);

    }

    dmat_free(&input_block);
    cmat_free(&input_fft);
    cmat_free(&output_fft);
    dmat_free(&output_block);

    us samples_done = dFifo_size(output_fifo);
    uVARTRACE(15,samples_done);
    dmat filtered_result = dmat_alloc(samples_done,nfilters);
    if(samples_done) {
        us samples_done2 = dFifo_pop(output_fifo,&filtered_result,0);
        dbgassert(samples_done2 == samples_done,"BUG in dFifo");
    }
    feTRACE(15);
    return filtered_result;
}


//////////////////////////////////////////////////////////////////////