/*
 * Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/>
 *           (C) 2020 Vladimir Sadovnikov <sadko4u@gmail.com>
 *
 * This file is part of lsp-dsp-units
 * Created on: 28 июня 2016 г.
 *
 * lsp-dsp-units is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * lsp-dsp-units is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with lsp-dsp-units. If not, see <https://www.gnu.org/licenses/>.
 */

#include <lsp-plug.in/dsp-units/filters/Filter.h>
#include <lsp-plug.in/dsp-units/const.h>
#include <lsp-plug.in/stdlib/math.h>
#include <lsp-plug.in/common/alloc.h>
#include <lsp-plug.in/dsp/dsp.h>

#define MIN_APO_Q           0.1f        /* Minimum Q for APO cannot be 0 */
#define STACK_BUF_SIZE      0x100U

namespace lsp
{
    namespace dspu
    {
        Filter::Filter()
        {
            construct();
        }

        Filter::~Filter()
        {
            destroy();
        }

        void Filter::construct()
        {
            pBank               = NULL;
            sParams.nType       = FLT_NONE;
            sParams.fFreq       = 0;
            sParams.fFreq2      = 0;
            sParams.fGain       = 0.0f;
            sParams.nSlope      = 0;
            sParams.fQuality    = 0.0f;

            nSampleRate         = 0;
            nMode               = FM_BYPASS;
            nLatency            = 0;
            nItems              = 0;

            vItems              = NULL;
            vData               = NULL;
            nFlags              = FF_REBUILD | FF_CLEAR;
        }

        bool Filter::init(FilterBank *fb)
        {
            filter_params_t fp;

            fp.nType            = FLT_NONE;
            fp.fFreq            = 1000.0f;
            fp.fFreq2           = 1000.0f;
            fp.fGain            = 1.0f;
            fp.fQuality         = 0;
            fp.nSlope           = 1;

            if (fb != NULL)
                pBank           = fb;
            else
            {
                // Try to allocate filter bank
                pBank           = new FilterBank();
                if (pBank == NULL)
                    return false;

                // Update flags that we hawe own filter bank
                nFlags         |= FF_OWN_BANK;

                // Try to initialize own filter bank
                if (!pBank->init(FILTER_CHAINS_MAX))
                    return false;
            }

            if (vData == NULL)
            {
                size_t cascade_size = align_size(sizeof(dsp::f_cascade_t) * FILTER_CHAINS_MAX, DEFAULT_ALIGN);

                size_t allocate     = cascade_size + DEFAULT_ALIGN; // + filters_size;
                vData               = new uint8_t[allocate];
                if (vData == NULL)
                    return false;

                uint8_t *ptr        = align_ptr(vData, DEFAULT_ALIGN);
                vItems              = reinterpret_cast<dsp::f_cascade_t *>(ptr);
                ptr                += cascade_size;
            }

            update(48000, &fp);
            nFlags             |= FF_REBUILD | FF_CLEAR;

            return true;
        }

        void Filter::destroy()
        {
            if (vData != NULL)
            {
                delete  [] vData;
                vItems      = NULL;
                vData       = NULL;
            }

            if (pBank != NULL)
            {
                // Destroy filter bank if it's our own filter bank
                if (nFlags & FF_OWN_BANK)
                {
                    pBank->destroy();
                    delete pBank;
                }

                pBank       = NULL;
            }

            nFlags      = 0;
        }

        void Filter::update(size_t sr, const filter_params_t *params)
        {
            // Init direct filter chain
            filter_params_t *fp     = &sParams;
            size_t type             = fp->nType;
            size_t slope            = fp->nSlope;

            // Reset parameters
            nSampleRate             = sr;
            nMode                   = FM_BYPASS;
            nLatency                = 0;

            // Copy and limit parameters
            *fp                     = *params;
            if (fp->nSlope < 1)
                fp->nSlope              = 1;
            else if (fp->nSlope > FILTER_CHAINS_MAX)
                fp->nSlope              = FILTER_CHAINS_MAX;
            if (fp->fFreq < LSP_DSP_UNITS_SPEC_FREQ_MIN)
                fp->fFreq               = LSP_DSP_UNITS_SPEC_FREQ_MIN;
            else if (fp->fFreq > LSP_DSP_UNITS_SPEC_FREQ_MAX)
                fp->fFreq               = LSP_DSP_UNITS_SPEC_FREQ_MAX;
            if (fp->fFreq >= (0.49f * nSampleRate))
                fp->fFreq               = 0.49f * nSampleRate;
            if (fp->fFreq2 < LSP_DSP_UNITS_SPEC_FREQ_MIN)
                fp->fFreq2              = LSP_DSP_UNITS_SPEC_FREQ_MIN;
            else if (fp->fFreq2 > LSP_DSP_UNITS_SPEC_FREQ_MAX)
                fp->fFreq2              = LSP_DSP_UNITS_SPEC_FREQ_MAX;
            if (fp->fFreq2 >= (0.49f * nSampleRate))
                fp->fFreq2              = 0.49f * nSampleRate;

            nFlags                 |= FF_REBUILD;
            if ((type != fp->nType) || (slope != fp->nSlope))
                nFlags                 |= FF_CLEAR;
        }

        void Filter::set_sample_rate(size_t sr)
        {
            update(sr, &sParams);
        }

        void Filter::get_params(filter_params_t *params)
        {
            if (params != NULL)
                *params =    sParams;
        }

        dsp::f_cascade_t *Filter::add_cascade()
        {
            // Get cascade
            dsp::f_cascade_t *c = (nItems >= FILTER_CHAINS_MAX) ?
                &vItems[FILTER_CHAINS_MAX-1] :
                &vItems[nItems];

            // Increment number of chains
            if (nItems < FILTER_CHAINS_MAX)
                nItems++;

            // Initialize cascade
            for (size_t i=0; i<4; ++i)
            {
                c->t[i] = 0.0;
                c->b[i] = 0.0;
            }

            // Return cascade
            return c;
        }

        float Filter::bilinear_relative(float f1, float f2)
        {
            float nf    = M_PI / float(nSampleRate);
            return tanf(f1 * nf) / tanf(f2 * nf);
        }

        void Filter::rebuild()
        {
            // Clear bank if it is internal bank
            if (nFlags & FF_OWN_BANK)
                pBank->begin();

            // Reset number of cascades
            nItems                  = 0;

            filter_params_t fp      = sParams;

            // Calculate filter
            switch (sParams.nType)
            {
                case FLT_BT_AMPLIFIER:
                case FLT_BT_RLC_LOPASS:
                case FLT_BT_RLC_HIPASS:
                case FLT_BT_RLC_LOSHELF:
                case FLT_BT_RLC_HISHELF:
                case FLT_BT_RLC_BELL:
                case FLT_BT_RLC_RESONANCE:
                case FLT_BT_RLC_NOTCH:
                case FLT_BT_RLC_ALLPASS:
                case FLT_BT_RLC_ALLPASS2:
                case FLT_BT_RLC_LADDERPASS:
                case FLT_BT_RLC_LADDERREJ:
                case FLT_BT_RLC_BANDPASS:
                case FLT_BT_RLC_ENVELOPE:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = bilinear_relative(fp.fFreq, fp.fFreq2);    // Normalize frequency
                    calc_rlc_filter(sParams.nType, &fp);
                    nMode               = FM_BILINEAR;
                    break;
                }

                case FLT_MT_AMPLIFIER:
                case FLT_MT_RLC_LOPASS:
                case FLT_MT_RLC_HIPASS:
                case FLT_MT_RLC_LOSHELF:
                case FLT_MT_RLC_HISHELF:
                case FLT_MT_RLC_BELL:
                case FLT_MT_RLC_RESONANCE:
                case FLT_MT_RLC_NOTCH:
                case FLT_MT_RLC_ALLPASS:
                case FLT_MT_RLC_ALLPASS2:
                case FLT_MT_RLC_LADDERPASS:
                case FLT_MT_RLC_LADDERREJ:
                case FLT_MT_RLC_BANDPASS:
                case FLT_MT_RLC_ENVELOPE:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = fp.fFreq / fp.fFreq2;    // Normalize frequency
                    calc_rlc_filter(sParams.nType - 1, &fp);
                    nMode               = FM_MATCHED;
                    break;
                }

                case FLT_BT_BWC_LOPASS:
                case FLT_BT_BWC_HIPASS:
                case FLT_BT_BWC_LOSHELF:
                case FLT_BT_BWC_HISHELF:
                case FLT_BT_BWC_BELL:
                case FLT_BT_BWC_LADDERPASS:
                case FLT_BT_BWC_LADDERREJ:
                case FLT_BT_BWC_BANDPASS:
                case FLT_BT_BWC_ALLPASS:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = bilinear_relative(fp.fFreq, fp.fFreq2);    // Normalize frequency
                    calc_bwc_filter(sParams.nType, &fp);
                    nMode               = FM_BILINEAR;
                    break;
                }

                case FLT_MT_BWC_LOPASS:
                case FLT_MT_BWC_HIPASS:
                case FLT_MT_BWC_LOSHELF:
                case FLT_MT_BWC_HISHELF:
                case FLT_MT_BWC_BELL:
                case FLT_MT_BWC_LADDERPASS:
                case FLT_MT_BWC_LADDERREJ:
                case FLT_MT_BWC_BANDPASS:
                case FLT_MT_BWC_ALLPASS:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = fp.fFreq / fp.fFreq2;    // Normalize frequency
                    calc_bwc_filter(sParams.nType - 1, &fp);
                    nMode               = FM_MATCHED;
                    break;
                }

                case FLT_BT_LRX_LOPASS:
                case FLT_BT_LRX_HIPASS:
                case FLT_BT_LRX_LOSHELF:
                case FLT_BT_LRX_HISHELF:
                case FLT_BT_LRX_BELL:
                case FLT_BT_LRX_LADDERPASS:
                case FLT_BT_LRX_LADDERREJ:
                case FLT_BT_LRX_BANDPASS:
                case FLT_BT_LRX_ALLPASS:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = bilinear_relative(fp.fFreq, fp.fFreq2);    // Normalize frequency
                    calc_lrx_filter(sParams.nType, &fp);
                    nMode               = FM_BILINEAR;
                    break;
                }

                case FLT_MT_LRX_LOPASS:
                case FLT_MT_LRX_HIPASS:
                case FLT_MT_LRX_LOSHELF:
                case FLT_MT_LRX_HISHELF:
                case FLT_MT_LRX_BELL:
                case FLT_MT_LRX_LADDERPASS:
                case FLT_MT_LRX_LADDERREJ:
                case FLT_MT_LRX_BANDPASS:
                case FLT_MT_LRX_ALLPASS:
                {
                    // Calculate filter parameters
                    fp.fFreq2           = fp.fFreq / fp.fFreq2;    // Normalize frequency
                    calc_lrx_filter(sParams.nType - 1, &fp);
                    nMode               = FM_MATCHED;
                    break;
                }

                case FLT_DR_APO_LOPASS:
                case FLT_DR_APO_HIPASS:
                case FLT_DR_APO_BANDPASS:
                case FLT_DR_APO_NOTCH:
                case FLT_DR_APO_ALLPASS:
                case FLT_DR_APO_PEAKING:
                case FLT_DR_APO_LOSHELF:
                case FLT_DR_APO_HISHELF:
                {
                    calc_apo_filter(sParams.nType, &fp);
                    nMode               = FM_APO;
                    break;
                }

                case FLT_DR_APO_ALLPASS2:
                {
                    calc_apo_filter(FLT_DR_APO_ALLPASS, &fp);
                    fp.fFreq            = sParams.fFreq2;
                    fp.fGain            = 1.0f;
                    calc_apo_filter(FLT_DR_APO_ALLPASS, &fp);
                    nMode               = FM_APO;
                    break;
                }

                case FLT_DR_APO_LADDERPASS:
                {
                    calc_apo_filter(FLT_DR_APO_HISHELF, &fp);
                    fp.fFreq            = sParams.fFreq2;
                    fp.fGain            = 1.0f / sParams.fGain;
                    calc_apo_filter(FLT_DR_APO_HISHELF, &fp);
                    nMode               = FM_APO;
                    break;
                }

                case FLT_DR_APO_LADDERREJ:
                {
                    calc_apo_filter(FLT_DR_APO_LOSHELF, &fp);
                    fp.fFreq            = sParams.fFreq2;
                    calc_apo_filter(FLT_DR_APO_HISHELF, &fp);
                    nMode               = FM_APO;
                    break;
                }

                case FLT_NONE:
                default:
                    nMode               = FM_BYPASS;
                    break;
            }

            if (nMode == FM_BILINEAR)
                bilinear_transform();
            else if (nMode == FM_MATCHED)
                matched_transform();

            // Complete bank if it is internal bank
            if (nFlags & FF_OWN_BANK)
                pBank->end(nFlags & FF_CLEAR);

            nFlags     &= FF_OWN_BANK; // Clear all flags except FF_OWN_BANK
        }

        void Filter::apo_complex_transfer_calc(float *re, float *im, float f)
        {
            // Calculating normalized frequency, wrapped for maximal accuracy:
            float kf    = f / float(nSampleRate);
            float w     = 2.0 * M_PI * (kf - floor(kf));

            // Auxiliary variables:
            float cw    = cosf(w);
            float sw    = sinf(w);

            // These equations are valid since sw has valid sign
            float c2w   = cw * cw - sw * sw;    // cos(2 * w)
            float s2w   = 2.0 * sw * cw;        // sin(2 * w)

            // Apo will be just one biquad, but let's write this to be able to calculate any digital biquads cascade.
            float r_re  = 1.0f, r_im = 0.0f;    // The result complex number
            float b_re, b_im;                   // Temporary values for computing complex multiplication

            for (size_t i=0; i<nItems; ++i)
            {
                dsp::f_cascade_t *c  = &vItems[i];

                float alpha     = c->t[0] + c->t[1] * cw + c->t[2] * c2w;
                float beta      = c->t[1] * sw + c->t[2] * s2w;
                float gamma     = c->b[0] + c->b[1] * cw + c->b[2] * c2w;
                float delta     = c->b[1] * sw + c->b[2] * s2w;

                float mag       = 1.0 / (gamma * gamma + delta * delta);

                // Compute current biquad's tranfer function
                float w_re      = mag * (alpha * gamma - beta * delta);
                float w_im      = mag * (alpha * delta + beta * gamma);

                // Compute common transfer function as a product between current biquad's
                // transfer function and previous value
                b_re            = r_re*w_re - r_im*w_im;
                b_im            = r_re*w_im + r_im*w_re;

                // Commit changes to the result complex number
                r_re            = b_re;
                r_im            = b_im;
            }

            *re     = r_re;
            *im     = r_im;
        }

        void Filter::freq_chart(float *re, float *im, const float *f, size_t count)
        {
            // Temporary buffer to store updated frequency
            float freqs[STACK_BUF_SIZE] __lsp_aligned32;
            filter_mode_t mode = (nItems > 0) ? nMode : FM_BYPASS;

            // Calculate frequency chart
            switch (mode)
            {
                case FM_BILINEAR:
                {
                    float nf    = M_PI / float(nSampleRate);
                    float kf    = 1.0/tanf(sParams.fFreq * nf);
                    float lf    = nSampleRate * 0.499;

                    while (count > 0)
                    {
                        size_t to_do    = lsp_min(count, STACK_BUF_SIZE);

                        // Compute transfer function
                        for (size_t i=0; i<to_do; ++i)
                        {
                            float w     = f[i];
                            freqs[i]      = tanf((w > lf ? lf : w) * nf) * kf;
                        }
                        dsp::filter_transfer_calc_ri(re, im, &vItems[0], freqs, to_do);
                        for (size_t i=1; i<nItems; ++i)
                            dsp::filter_transfer_apply_ri(re, im, &vItems[i], freqs, to_do);

                        // Update pointers
                        re         += to_do;
                        im         += to_do;
                        f          += to_do;
                        count      -= to_do;
                    }
                    break;
                }

                case FM_MATCHED:
                {
                    float kf    = 1.0 / sParams.fFreq;

                    while (count > 0)
                    {
                        size_t to_do    = lsp_min(count, STACK_BUF_SIZE);

                        // Compute transfer function
                        dsp::mul_k3(freqs, f, kf, to_do);
                        dsp::filter_transfer_calc_ri(re, im, &vItems[0], freqs, to_do);
                        for (size_t i=1; i<nItems; ++i)
                            dsp::filter_transfer_apply_ri(re, im, &vItems[i], freqs, to_do);

                        // Update pointers
                        re         += to_do;
                        im         += to_do;
                        f          += to_do;
                        count      -= to_do;
                    }
                    break;
                }

                case FM_APO:
                {
                    while (count--)
                    {
                        apo_complex_transfer_calc(re++, im++, *(f++));
                    }
                    break;
                }

                case FM_BYPASS:
                default:
                    dsp::fill_one(re, count);
                    dsp::fill_zero(im, count);
                    return;
            }
        }

        void Filter::freq_chart(float *c, const float *f, size_t count)
        {
            // Temporary buffer to store updated frequency
            float freqs[STACK_BUF_SIZE] __lsp_aligned32;
            filter_mode_t mode = (nItems > 0) ? nMode : FM_BYPASS;

            // Calculate frequency chart
            switch (mode)
            {
                case FM_BILINEAR:
                {
                    float nf    = M_PI / float(nSampleRate);
                    float kf    = 1.0/tanf(sParams.fFreq * nf);
                    float lf    = nSampleRate * 0.499;

                    while (count > 0)
                    {
                        size_t to_do    = lsp_min(count, STACK_BUF_SIZE);

                        // Compute transfer function
                        for (size_t i=0; i<to_do; ++i)
                        {
                            float w     = f[i];
                            freqs[i]      = tanf((w > lf ? lf : w) * nf) * kf;
                        }
                        dsp::filter_transfer_calc_pc(c, &vItems[0], freqs, to_do);
                        for (size_t i=1; i<nItems; ++i)
                            dsp::filter_transfer_apply_pc(c, &vItems[i], freqs, to_do);

                        // Update pointers
                        c          += to_do*2;
                        f          += to_do;
                        count      -= to_do;
                    }

                    break;
                }

                case FM_MATCHED:
                {
                    float kf    = 1.0 / sParams.fFreq;

                    while (count > 0)
                    {
                        size_t to_do    = lsp_min(count, STACK_BUF_SIZE);

                        // Compute transfer function
                        dsp::mul_k3(freqs, f, kf, to_do);
                        dsp::filter_transfer_calc_pc(c, &vItems[0], freqs, to_do);
                        for (size_t i=1; i<nItems; ++i)
                            dsp::filter_transfer_apply_pc(c, &vItems[i], freqs, to_do);

                        // Update pointers
                        c          += to_do*2;
                        f          += to_do;
                        count      -= to_do;
                    }

                    break;
                }

                case FM_APO:
                {
                    while (count--)
                    {
                        apo_complex_transfer_calc(c, &c[1], *(f++));
                        c += 2; // Don't forget to move the pointer
                    }
                    break;
                }

                case FM_BYPASS:
                default:
                    dsp::pcomplex_fill_ri(c, 1.0f, 0.0f, count);
                    return;
            }
        }

        void Filter::process(float *out, const float *in, size_t samples)
        {
            // Check whether we need to rebuild filter
            if (nFlags & (~FF_OWN_BANK))
                rebuild();

            switch (nMode)
            {
                case FM_BILINEAR:
                case FM_MATCHED:
                {
                    pBank->process(out, in, samples);
                    break;
                }

                default:
                {
                    dsp::copy(out, in, samples);
                    break;
                }
            }
        }

        void Filter::calc_rlc_filter(size_t type, const filter_params_t *fp)
        {
            dsp::f_cascade_t *c         = NULL;
            nMode                       = FM_BILINEAR;

            switch (type)
            {
                case FLT_BT_AMPLIFIER:
                {
                    c           = add_cascade();
                    c->t[0]     = fp->fGain;
                    c->t[1]     = 0.0f;
                    c->t[2]     = 0.0f;

                    c->b[0]     = 1.0f;
                    c->b[1]     = 0.0f;
                    c->b[2]     = 0.0f;

                    break;
                }

                case FLT_BT_RLC_LOPASS:
                case FLT_BT_RLC_HIPASS:
                {
                    // Add cascade with one pole
                    size_t i        = fp->nSlope & 1;
                    if (i)
                    {
                        c           = add_cascade();
                        c->b[0]     = 1.0;
                        c->b[1]     = 1.0;

                        if (type == FLT_BT_RLC_LOPASS)
                            c->t[0]     = fp->fGain;
                        else
                            c->t[1]     = fp->fGain;
                    }

                    // Add additional cascades
                    for (size_t j=i; j < fp->nSlope; j+=2)
                    {
                        c           = add_cascade();
                        c->b[0]     = 1.0;
                        c->b[1]     = 2.0 / (1.0 + fp->fQuality);
                        c->b[2]     = 1.0;

                        if (type == FLT_BT_RLC_LOPASS)
                            c->t[0]     = (j == 0) ? fp->fGain : 1.0;
                        else
                            c->t[2]     = (j == 0) ? fp->fGain : 1.0;
                    }

                    break;
                }

                case FLT_BT_RLC_LOSHELF:
                case FLT_BT_RLC_HISHELF:
                {
                    size_t slope            = fp->nSlope * 2;
                    float gain              = sqrtf(fp->fGain);
                    float fg                = expf(logf(gain)/slope);

                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        c                       = add_cascade();
                        float *t                = (type == FLT_BT_RLC_LOSHELF) ? c->t : c->b;
                        float *b                = (type == FLT_BT_RLC_LOSHELF) ? c->b : c->t;

                        // Create transfer function
                        t[0]                    = fg;
                        t[1]                    = 2.0 / (1.0 + fp->fQuality);
                        t[2]                    = 1.0 / fg;

                        b[0]                    = 1.0 / fg;
                        b[1]                    = 2.0 / (1.0 + fp->fQuality);
                        b[2]                    = fg;

                        if (j == 0)
                        {
                            c->t[0]                *= gain;
                            c->t[1]                *= gain;
                            c->t[2]                *= gain;
                        }
                    }

                    break;
                }

                case FLT_BT_RLC_LADDERPASS:
                case FLT_BT_RLC_LADDERREJ:
                {
                    size_t slope            = fp->nSlope * 2;
                    float gain1             = (type == FLT_BT_RLC_LADDERREJ) ? sqrtf(1.0/fp->fGain) : sqrtf(fp->fGain);
                    float gain2             = (type == FLT_BT_RLC_LADDERREJ) ? sqrtf(fp->fGain) : sqrtf(1.0/fp->fGain);

                    float fg1               = expf(logf(gain1)/slope);
                    float fg2               = expf(logf(gain2)/slope);
                    float kf                = fp->fFreq2;

                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        // First shelf cascade, lo-shelf for LADDERREJ, hi-shelf for LADDERPASS
                        c                       = add_cascade();
                        float *t                = (type == FLT_BT_RLC_LADDERREJ) ? c->t : c->b;
                        float *b                = (type == FLT_BT_RLC_LADDERREJ) ? c->b : c->t;
                        float fg                = (type == FLT_BT_RLC_LADDERREJ) ? fg2 : fg1;
                        float gain              = (type == FLT_BT_RLC_LADDERREJ) ? gain2 : gain1;

                        // Create transfer function
                        t[0]                    = fg;
                        t[1]                    = 2.0 / (1.0 + fp->fQuality);
                        t[2]                    = 1.0 / fg;

                        b[0]                    = 1.0 / fg;
                        b[1]                    = 2.0 / (1.0 + fp->fQuality);
                        b[2]                    = fg;

                        if (j == 0)
                        {
                            c->t[0]                *= gain;
                            c->t[1]                *= gain;
                            c->t[2]                *= gain;
                        }

                        // Second shelf cascade, hi-shelf always
                        c                       = add_cascade();
                        t                       = c->b;
                        b                       = c->t;

                        // Create transfer function
                        t[0]                    = fg2;
                        t[1]                    = 2.0 * kf / (1.0 + fp->fQuality);
                        t[2]                    = kf*kf / fg2;

                        b[0]                    = 1.0 / fg2;
                        b[1]                    = 2.0 * kf / (1.0 + fp->fQuality);
                        b[2]                    = fg2 * kf * kf;

                        if (j == 0)
                        {
                            c->t[0]                *= gain2;
                            c->t[1]                *= gain2;
                            c->t[2]                *= gain2;
                        }
                    }

                    break;
                }

                case FLT_BT_RLC_BANDPASS:
                {
                    float f2                = 1.0 / fp->fFreq2;
                    float k                 = (1.0 + f2)/(1.0 + fp->fQuality);

                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        c                       = add_cascade();
                        c->t[1]                 = (j == 0) ? expf(fp->nSlope * logf(k)) * fp->fGain : 1.0;
                        c->b[0]                 = f2;
                        c->b[1]                 = k;
                        c->b[2]                 = 1.0;
                    }

                    break;
                }

                case FLT_BT_RLC_BELL:
                {
                    float fg                = expf(logf(fp->fGain)/fp->nSlope);
                    float angle             = atanf(fg);
                    float k                 = 2.0 * (1.0/fg + fg) / (1.0 + (2.0 * fp->fQuality) / fp->nSlope);
                    float kt                = k * sinf(angle);
                    float kb                = k * cosf(angle);

                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        c                       = add_cascade();

                        // Create transfer function
                        c->t[0]                 = 1.0;
                        c->t[1]                 = kt;
                        c->t[2]                 = 1.0;

                        c->b[0]                 = 1.0;
                        c->b[1]                 = kb;
                        c->b[2]                 = 1.0;
                    }

                    break;
                }

                // Resonance filter
                case FLT_BT_RLC_RESONANCE:
                {
                    float angle             = atanf(expf(logf(fp->fGain) / fp->nSlope));
                    float k                 = 2.0 / (1.0 + fp->fQuality);
                    float kt                = k * sinf(angle);
                    float kb                = k * cosf(angle);

                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        c                       = add_cascade();

                        // Create transfer function
                        c->t[0]                 = 1.0;
                        c->t[1]                 = kt;
                        c->t[2]                 = 1.0;

                        c->b[0]                 = 1.0;
                        c->b[1]                 = kb;
                        c->b[2]                 = 1.0;
                    }

                    break;
                }

                case FLT_BT_RLC_NOTCH:
                {
                    c                       = add_cascade();

                    // Create transfer function
                    c->t[0]                 = fp->fGain;
                    c->t[1]                 = 0;
                    c->t[2]                 = fp->fGain;

                    // Bottom polynom
                    c->b[0]                 = 1.0;
                    c->b[1]                 = 2.0 / (1.0 + fp->fQuality);
                    c->b[2]                 = 1.0;

                    break;
                }

                case FLT_BT_RLC_ALLPASS:
                {
                    // Single all-pass filter
                    size_t i        = fp->nSlope & 1;
                    if (i)
                    {
                        c                       = add_cascade();
                        c->t[0]                 = -1.0;
                        c->t[1]                 = 1.0;
                        c->t[2]                 = 0.0;

                        c->b[0]                 = 1.0;
                        c->b[1]                 = 1.0;
                        c->b[2]                 = 0.0;
                    }

                    // 2x all-pass filters in one cascade
                    for (size_t j=i; j < fp->nSlope; j += 2)
                    {
                        c                       = add_cascade();
                        // Create transfer function
                        c->t[0]                 = 1.0;
                        c->t[1]                 = -2.0;
                        c->t[2]                 = 1.0;

                        // Bottom polynom
                        c->b[0]                 = 1.0;
                        c->b[1]                 = 2.0;
                        c->b[2]                 = 1.0;
                    }

                    // Adjust gain for the last cascade
                    if (c != NULL)
                    {
                        c->t[0]    *= fp->fGain;
                        c->t[1]    *= fp->fGain;
                        c->t[2]    *= fp->fGain;
                    }

                    break;
                }

                case FLT_BT_RLC_ALLPASS2:
                {
                    float kf                = fp->fFreq2;
                    float kfp1              = 1.0 + kf;

                    // 2x all-pass filters in one cascade
                    for (size_t j=0; j < fp->nSlope; j++)
                    {
                        c                       = add_cascade();
                        // Create transfer function
                        c->t[0]                 = 1.0;
                        c->t[1]                 = -kfp1;
                        c->t[2]                 = kf;

                        // Bottom polynom
                        c->b[0]                 = 1.0;
                        c->b[1]                 = kfp1;
                        c->b[2]                 = kf;
                    }

                    // Adjust gain for the last cascade
                    if (c != NULL)
                    {
                        c->t[0]    *= fp->fGain;
                        c->t[1]    *= fp->fGain;
                        c->t[2]    *= fp->fGain;
                    }

                    break;
                }

                case FLT_BT_RLC_ENVELOPE:
                {
                    size_t slope    = fp->nSlope;
                    size_t cj       = 0;
                    if (slope & 1)
                    {
                        float k                 = 1.0f;

                        for (size_t i=0; i<3; ++i)
                        {
                            c               = add_cascade();
                            c->t[0]         = 1.0f;
                            c->t[1]         = (1.0f + 0.25f)*k;
                            c->t[2]         = 0.25f * k*k;

                            c->b[0]         = 1.0f;
                            c->b[1]         = (0.5f + 0.125f)*k;
                            c->b[2]         = 0.5f * 0.125f * k*k;

                            k              *= 0.0625f;
                            if (!(cj++))
                            {
                                c->t[0]        *= fp->fGain;
                                c->t[1]        *= fp->fGain;
                                c->t[2]        *= fp->fGain;
                            }
                        }
                    }
                    slope >>= 1;

                    for (size_t j=0; j < slope; j++)
                    {
                        c                       = add_cascade();
                        c->t[0]                 = (cj == 0) ? fp->fGain : 1.0f;
                        c->t[1]                 = (cj == 0) ? fp->fGain : 1.0f;
                        c->b[0]                 = 1.0f;
                        c->b[1]                 = 0.0005f;
                        cj ++;
                    }
                    break;
                }

                default:
                    nMode           = FM_BYPASS;
                    break;
            }
        }

        void Filter::calc_bwc_filter(size_t type, const filter_params_t *fp)
        {
            dsp::f_cascade_t *c     = NULL;

            switch (type)
            {
                case FLT_BT_BWC_LOPASS:
                case FLT_BT_BWC_HIPASS:
                {
                    float k     = 1.0f / (1.0f + fp->fQuality);
                    size_t i    = fp->nSlope & 1;
                    if (i)
                    {
                        c           = add_cascade();
                        c->b[0]     = 1.0;
                        c->b[1]     = 1.0;

                        if (type == FLT_BT_BWC_LOPASS)
                            c->t[0]     = fp->fGain;
                        else
                            c->t[1]     = fp->fGain;
                    }

                    for (size_t j=i; j < fp->nSlope; j += 2)
                    {
                        float theta     = ((j - i + 1)*M_PI_2)/fp->nSlope;
                        float tsin      = sinf(theta);
                        float tcos      = sqrtf(1.0 - tsin*tsin);
                        float kf        = tsin*tsin + k*k * tcos*tcos;

                        c               = add_cascade();

                        // Tranfer function
                        if (type == FLT_BT_BWC_HIPASS)
                        {
                            c->t[2]         = (j == 0) ? fp->fGain : 1.0;

                            c->b[0]         = 1.0 / kf;
                            c->b[1]         = 2.0 * k * tcos / kf;
                            c->b[2]         = 1.0;
                        }
                        else
                        {
                            c->t[0]         = (j == 0) ? fp->fGain : 1.0;

                            c->b[0]         = 1.0;
                            c->b[1]         = 2.0 * k * tcos / kf;
                            c->b[2]         = 1.0 / kf;
                        }
                    }

                    break;
                }

                case FLT_BT_BWC_ALLPASS:
                {
                    float k     = 1.0f / (1.0f + fp->fQuality);
                    size_t i    = fp->nSlope & 1;
                    if (i)
                    {
                        c           = add_cascade();
                        c->t[0]     = -fp->fGain;
                        c->t[1]     = fp->fGain;
                        c->t[2]     = 0.0;

                        c->b[0]     = 1.0;
                        c->b[1]     = 1.0;
                        c->b[2]     = 0.0;

                        if (type == FLT_BT_BWC_LOPASS)
                            c->t[0]     = fp->fGain;
                        else
                            c->t[1]     = fp->fGain;
                    }

                    for (size_t j=i; j < fp->nSlope; j += 2)
                    {
                        float theta     = ((j - i + 1)*M_PI_2)/fp->nSlope;
                        float tsin      = sinf(theta);
                        float tcos      = sqrtf(1.0 - tsin*tsin);
                        float kf        = tsin*tsin + k*k * tcos*tcos;

                        c               = add_cascade();

                        // Tranfer function
                        c->t[0]         = 1.0;
                        c->t[1]         = -2.0 * tcos;
                        c->t[2]         = 1.0;

                        c->b[0]         = 1.0 / kf;
                        c->b[1]         = 2.0 * k * tcos / kf;
                        c->b[2]         = 1.0;

                        if (j == 0)
                        {
                            c->t[0]        *= fp->fGain;
                            c->t[1]        *= fp->fGain;
                            c->t[2]        *= fp->fGain;
                        }
                    }

                    break;
                }

                case FLT_BT_BWC_HISHELF:
                case FLT_BT_BWC_LOSHELF:
                {
                    float gain              = sqrtf(fp->fGain);
                    float fg                = expf(logf(gain)/(2.0*fp->nSlope));
                    float k                 = 1.0f / (1.0 + fp->fQuality * (1.0 - expf(2.0 - gain - 1.0/gain)));

                    for (size_t j=0; j < fp->nSlope; ++j)
                    {
                        float theta         = ((2*j + 1)*M_PI_2)/(2*fp->nSlope);
                        float tsin          = sinf(theta);
                        float tcos          = sqrtf(1.0 - tsin*tsin);
                        float kf            = tsin*tsin + k*k * tcos*tcos;

                        c                   = add_cascade();
                        float *t            = (type == FLT_BT_BWC_HISHELF) ? c->t : c->b;
                        float *b            = (type == FLT_BT_BWC_HISHELF) ? c->b : c->t;

                        // Transfer function
                        t[0]                = kf / fg;
                        t[1]                = 2.0 * k * tcos;
                        t[2]                = fg;

                        b[0]                = fg;
                        b[1]                = 2.0 * k * tcos;
                        b[2]                = kf / fg;

                        if (j == 0)
                        {
                            c->t[0]            *= gain;
                            c->t[1]            *= gain;
                            c->t[2]            *= gain;
                        }
                    }

                    break;
                }

                case FLT_BT_BWC_LADDERPASS:
                case FLT_BT_BWC_LADDERREJ:
                {
                    size_t slope            = fp->nSlope * 2;
                    float gain1             = (type == FLT_BT_BWC_LADDERPASS) ? sqrtf(fp->fGain) : sqrtf(1.0/fp->fGain);
                    float gain2             = (type == FLT_BT_BWC_LADDERPASS) ? sqrtf(1.0/fp->fGain) : sqrtf(fp->fGain);

                    float fg1               = expf(logf(gain1)/(2.0*fp->nSlope));
                    float fg2               = expf(logf(gain2)/(2.0*fp->nSlope));
                    float k1                = 1.0f / (1.0 + fp->fQuality * (1.0 - expf(2.0 - gain1 - 1.0/gain1)));
                    float k2                = 1.0f / (1.0 + fp->fQuality * (1.0 - expf(2.0 - gain2 - 1.0/gain2)));
                    float xf                = fp->fFreq2;

                    for (size_t j=0; j < fp->nSlope; ++j)
                    {
                        float theta         = ((2*j + 1)*M_PI_2)/float(slope);
                        float tsin          = sinf(theta);
                        float tcos          = sqrtf(1.0 - tsin*tsin);

                        // First shelf cascade, lo-shelf for LADDERREJ, hi-shelf for LADDERPASS
                        float k             = (type == FLT_BT_BWC_LADDERPASS) ? k1 : k2;
                        float fg            = (type == FLT_BT_BWC_LADDERPASS) ? fg1 : fg2;
                        float gain          = (type == FLT_BT_BWC_LADDERPASS) ? gain1 : gain2;
                        float kf            = tsin*tsin + k*k * tcos*tcos;
                        c                   = add_cascade();
                        float *t            = (type == FLT_BT_BWC_LADDERPASS) ? c->t : c->b;
                        float *b            = (type == FLT_BT_BWC_LADDERPASS) ? c->b : c->t;

                        // Transfer function
                        t[0]                = kf / fg;
                        t[1]                = 2.0 * k * tcos;
                        t[2]                = fg;

                        b[0]                = fg;
                        b[1]                = 2.0 * k * tcos;
                        b[2]                = kf / fg;

                        if (j == 0)
                        {
                            c->t[0]            *= gain;
                            c->t[1]            *= gain;
                            c->t[2]            *= gain;
                        }

                        // Second shelf cascade, always hi-shelf
                        kf                  = tsin*tsin + k1*k1 * tcos*tcos;
                        c                   = add_cascade();
                        t                   = c->b;
                        b                   = c->t;

                        // Transfer function
                        t[0]                = kf / fg1;
                        t[1]                = 2.0 * k1 * xf * tcos;
                        t[2]                = fg1 * xf * xf;

                        b[0]                = fg1;
                        b[1]                = 2.0 * k1 * xf * tcos;
                        b[2]                = kf * xf * xf / fg1;

                        if (j == 0)
                        {
                            c->t[0]            *= gain2;
                            c->t[1]            *= gain2;
                            c->t[2]            *= gain2;
                        }
                    }

                    break;
                }

                case FLT_BT_BWC_BELL:
                {
                    float fg                = expf(logf(fp->fGain)/float(2*fp->nSlope));
                    float k                 = 1.0f / (1.0 + fp->fQuality);

                    for (size_t j=0; j < fp->nSlope; ++j)
                    {
                        float theta         = ((2*j + 1)*M_PI_2)/(2*fp->nSlope);
                        float tsin          = sinf(theta);
                        float tcos          = sqrtf(1.0 - tsin*tsin);
                        float kf            = tsin*tsin + k*k * tcos*tcos;

                        if (fp->fGain >= 1.0)
                        {
                            // First cascade
                            c                   = add_cascade();

                            c->t[0]             = 1.0;
                            c->t[1]             = 2.0 * k * tcos * fg / kf;
                            c->t[2]             = 1.0 * fg * fg / kf;

                            c->b[0]             = 1.0;
                            c->b[1]             = 2.0 * k * tcos / kf;
                            c->b[2]             = 1.0 / kf;

                            // Second cascade
                            c                   = add_cascade();

                            c->t[0]             = 1.0;
                            c->t[1]             = 2.0 * k * tcos / fg;
                            c->t[2]             = kf / (fg * fg);

                            c->b[0]             = 1.0;
                            c->b[1]             = 2.0 * k * tcos;
                            c->b[2]             = kf;
                        }
                        else
                        {
                            // First cascade
                            c                   = add_cascade();

                            c->t[0]             = 1.0;
                            c->t[1]             = 2.0 * k * tcos / kf;
                            c->t[2]             = 1.0 / kf;

                            c->b[0]             = 1.0;
                            c->b[1]             = 2.0 * k * tcos / (fg * kf);
                            c->b[2]             = 1.0 / (fg * fg * kf);

                            // Second cascade
                            c                   = add_cascade();

                            c->t[0]             = 1.0;
                            c->t[1]             = 2.0 * k * tcos;
                            c->t[2]             = kf;

                            c->b[0]             = 1.0;
                            c->b[1]             = 2.0 * k * tcos * fg;
                            c->b[2]             = kf * fg * fg;
                        }
                    }

                    break;
                }

                case FLT_BT_BWC_BANDPASS:
                {
                    float f2            = fp->fFreq2;
                    float k             = 1.0f / (1.0f + fp->fQuality);

                    for (size_t j=0; j < fp->nSlope; ++j)
                    {
                        float theta         = ((2*j + 1)*M_PI_2)/(2*fp->nSlope);
                        float tsin          = sinf(theta);
                        float tcos          = sqrtf(1.0 - tsin*tsin);
                        float kf            = tsin*tsin + k*k * tcos*tcos;

                        // Hi-pass cascade
                        c                   = add_cascade();

                        c->t[2]             = (j == 0) ? fp->fGain : 1.0;

                        c->b[0]             = 1.0 / kf;
                        c->b[1]             = 2.0 * k * tcos / kf;
                        c->b[2]             = 1.0;

                        // Lo-pass cascade
                        c                   = add_cascade();

                        c->t[0]             = 1.0;

                        c->b[0]             = 1.0;
                        c->b[1]             = 2.0 * k * tcos * f2 / kf;
                        c->b[2]             = f2 * f2 / kf;
                    }

                    break;
                }

                default:
                    nMode           = FM_BYPASS;
                    break;
            }
        }

        void Filter::calc_lrx_filter(size_t type, const filter_params_t *fp)
        {
            dsp::f_cascade_t *c1, *c2;

            // LRX filter is just twice repeated BWC filter
            // Calculate the same chain twice
            switch (type)
            {
                case FLT_BT_LRX_LOPASS:
                    type = FLT_BT_BWC_LOPASS;
                    break;
                case FLT_BT_LRX_HIPASS:
                    type = FLT_BT_BWC_HIPASS;
                    break;
                case FLT_BT_LRX_LOSHELF:
                    type = FLT_BT_BWC_LOSHELF;
                    break;
                case FLT_BT_LRX_HISHELF:
                    type = FLT_BT_BWC_HISHELF;
                    break;
                case FLT_BT_LRX_BELL:
                    type = FLT_BT_BWC_BELL;
                    break;
                case FLT_BT_LRX_BANDPASS:
                    type = FLT_BT_BWC_BANDPASS;
                    break;
                case FLT_BT_LRX_LADDERPASS:
                    type = FLT_BT_BWC_LADDERPASS;
                    break;
                case FLT_BT_LRX_LADDERREJ:
                    type = FLT_BT_BWC_LADDERREJ;
                    break;
                case FLT_BT_LRX_ALLPASS:
                {
                    float k     = 1.0f / (1.0f + fp->fQuality);
                    size_t i    = sParams.nSlope * 2;

                    // Emit 2x butterworth filters
                    for (size_t j=0; j < i; j += 2)
                    {
                        float theta     = ((j+1) * M_PI_2)/i;
                        float tsin      = sinf(theta);
                        float tcos      = sqrtf(1.0 - tsin*tsin);
                        float kf        = tsin*tsin + k*k * tcos*tcos;

                        c1              = add_cascade();
                        c2              = add_cascade();

                        // Top part
                        float xeta      = ((j+0.5) * M_PI)/i;
                        c1->t[0]        = 1.0;
                        c1->t[1]        = -2.0 * cosf(xeta);
                        c1->t[2]        = 1.0;

                        xeta            = ((j+1.5) * M_PI)/i;
                        c2->t[0]        = 1.0;
                        c2->t[1]        = -2.0 * cosf(xeta);
                        c2->t[2]        = 1.0;

                        // Bottom part
                        c1->b[0]        = 1.0 / kf;
                        c1->b[1]        = 2.0 * k * tcos / kf;
                        c1->b[2]        = 1.0;

                        c2->b[0]        = c1->b[0];
                        c2->b[1]        = c1->b[1];
                        c2->b[2]        = c1->b[2];

                        if (j == 0)
                        {
                            c1->t[0]       *= fp->fGain;
                            c1->t[1]       *= fp->fGain;
                            c1->t[2]       *= fp->fGain;
                        }
                    }
                    return;
                }
                default:
                    nMode           = FM_BYPASS;
                    return;
            }

            // Have to do some hacks with chain parameters: reduce slope and gain
            filter_params_t bfp = *fp;
            bfp.nSlope          = sParams.nSlope*2;
            bfp.fGain           = sqrtf(bfp.fGain);

            // Calculate two similar chains
            calc_bwc_filter(type, &bfp);
            calc_bwc_filter(type, &bfp);
        }

        void Filter::calc_apo_filter(size_t type, const filter_params_t *fp)
        {
            float a0, a1, a2;
            float b0, b1, b2;

            float omega    = 2.0 * M_PI * fp->fFreq / float(nSampleRate);
            float cs       = sinf(omega);
            float cc       = cosf(omega); // Have to use trig functions for both to have correct sign
            float Q        = (fp->fQuality > MIN_APO_Q) ? fp->fQuality : MIN_APO_Q;
            float alpha    = 0.5 * cs / Q;

            // In LSP convention, the b coefficients are in the denominator. The a coefficients are in the
            // numerator. This is opposite to the most usual convention.
            // Coefficients are normalised so that b0 = 1

            switch (type)
            {
                case FLT_DR_APO_LOPASS:
                {
                    float A     = fp->fGain;

                    a0 = A * 0.5 * (1.0 - cc);
                    a1 = A * (1.0 - cc);
                    a2 = a0;
                    b0 = 1.0 + alpha;
                    b1 = -2.0 * cc;
                    b2 = 1.0 - alpha;

                    break;
                }

                case FLT_DR_APO_HIPASS:
                {
                    float A     = fp->fGain;

                    a0 = A * 0.5 * (1.0 + cc);
                    a1 = A * (-1.0 - cc);
                    a2 = a0;
                    b0 = 1.0 + alpha;
                    b1 = -2.0 * cc;
                    b2 = 1.0 - alpha;

                    break;
                }

                case FLT_DR_APO_BANDPASS:
                {
                    float A     = fp->fGain;

                    a0 = A * alpha;
                    a1 = 0.0;
                    a2 = A * -alpha;
                    b0 = 1.0 + alpha;
                    b1 = -2.0 * cc;
                    b2 = 1 - alpha;

                    break;
                }

                case FLT_DR_APO_NOTCH:
                {
                    float A     = fp->fGain;

                    a0 = A;
                    a1 = A * -2.0 * cc;
                    a2 = a0;
                    b0 = 1.0 + alpha;
                    b1 = -2.0 * cc;
                    b2 = 1.0 - alpha;

                    break;
                }

                case FLT_DR_APO_ALLPASS:
                {
                    float A     = fp->fGain;

                    a0 = A * (1.0 - alpha);
                    a1 = A * -2.0 * cc;
                    a2 = A * (1.0 + alpha);
                    b0 = a2;
                    b1 = a1;
                    b2 = a0;

                    break;
                }

                case FLT_DR_APO_PEAKING:
                {
                    float A     = sqrtf(fp->fGain);

                    a0 = 1.0 + alpha * A;
                    a1 = -2.0 * cc;
                    a2 = 1.0 - alpha * A;
                    b0 = 1.0 + alpha / A;
                    b1 = a1;
                    b2 = 1.0 - alpha / A;

                    break;
                }

                case FLT_DR_APO_LOSHELF:
                {
                    float A     = sqrtf(fp->fGain);
                    float beta  = 2.0 * alpha * sqrtf(A);

                    a0 = A * ((A + 1.0) - (A - 1.0) * cc + beta);
                    a1 = 2.0 * A * ((A - 1.0) - (A + 1.0) * cc);
                    a2 = A * ((A + 1.0) - (A - 1.0) * cc - beta);
                    b0 = (A + 1.0) + (A - 1.0) * cc + beta;
                    b1 = -2.0 * ((A - 1.0) + (A + 1.0) * cc);
                    b2 = (A + 1.0) + (A - 1.0) * cc - beta;

                    break;
                }

                case FLT_DR_APO_HISHELF:
                {
                    float A     = sqrtf(fp->fGain);
                    float beta  = 2.0 * alpha * sqrtf(A);

                    a0 = A * ((A + 1.0) + (A - 1.0) * cc + beta);
                    a1 = -2.0 * A * ((A - 1.0) + (A + 1.0) * cc);
                    a2 = A * ((A + 1.0) + (A - 1.0) * cc - beta);
                    b0 = (A + 1.0) - (A - 1.0) * cc + beta;
                    b1 = 2.0 * ((A - 1.0) - (A + 1.0) * cc);
                    b2 = (A + 1.0) - (A - 1.0) * cc - beta;

                    break;
                }
                default:
                    return;
            }

            dsp::biquad_x1_t *f = pBank->add_chain();
            if (f == NULL)
                return;

            // Storing with appropriate normalisation and sign as required by biquad_process_x1().
            f->b0   = a0 / b0;
            f->b1   = a1 / b0;
            f->b2   = a2 / b0;
            f->a1   = -b1 / b0;
            f->a2   = -b2 / b0;
            f->p0   = 0.0f;
            f->p1   = 0.0f;
            f->p2   = 0.0f;

            // Storing the coefficient for plotting
            dsp::f_cascade_t *c  = add_cascade();
            c->t[0] = f->b0;
            c->t[1] = f->b1;
            c->t[2] = f->b2;
            c->b[0] = 1.0;
            c->b[1] = -f->a1;
            c->b[2] = -f->a2;
        }

        /*
            Original filter chain:

                       t[0] + t[1] * p + t[2] * p^2
              H(p) =  ------------------------------
                       b[0] + b[1] * p + b[2] * p^2

            Bilinear transform:

              x    = z^-1

              kf   = 1 / tan(pi * frequency / sample_rate) - frequency shift factor

                           1 - x
              p    = kf * -------   - analog -> digital bilinear transform expression
                           1 + x

            Applied bilinear transform:

                       (t[0] + t[1]*kf + t[2]*kf^2) + 2*(t[0] - t[2]*kf^2)*x + (t[0] - t[1]*kf + t[2]*kf^2)*x^2
              H[x] =  -----------------------------------------------------------------------------------------
                       (b[0] + b[1]*kf + b[2]*kf^2) + 2*(b[0] - b[2]*kf^2)*x + (b[0] - b[1]*kf + b[2]*kf^2)*x^2

            Finally:

              T    =   { t[0], t[1]*kf, t[2]*kf*kf }
              B    =   { b[0], b[1]*kf, b[2]*kf*kf }

                       (T[0] + T[1] + T[2]) + 2*(T[0] - T[2])*z^-1 + (T[0] - T[1] + T[2])*z^-2
              H[z] =  -------------------------------------------------------------------------
                       (B[0] + B[1] + B[2]) + 2*(B[0] - B[2])*z^-1 + (B[0] - B[1] + B[2])*z^-2
         */

        void Filter::bilinear_transform()
        {
            float kf        = 1.0/tanf(sParams.fFreq * M_PI / float(nSampleRate));
            float kf2       = kf * kf;
            float T[4], B[4], N;
            size_t chains   = 0;

            for (size_t i=0; i<nItems; ++i)
            {
                dsp::f_cascade_t *c  = &vItems[i];
                float *t        = c->t;
                float *b        = c->b;

                // Calculate top coefficients
                T[0]            = t[0];
                T[1]            = t[1]*kf;
                T[2]            = t[2]*kf2;

                // Calculate bottom coefficients
                B[0]            = b[0];
                B[1]            = b[1]*kf;
                B[2]            = b[2]*kf2;

                // Calculate the convolution
                N               = 1.0 / (B[0] + B[1] + B[2]);

                // Initialize filter parameters
                if ((++chains) > FILTER_CHAINS_MAX)
                    break;
                dsp::biquad_x1_t *f  = pBank->add_chain();
                if (f == NULL)
                    break;

                f->b0           = (T[0] + T[1] + T[2]) * N;
                f->b1           = 2.0 * (T[0] - T[2]) * N;
                f->b2           = (T[0] - T[1] + T[2]) * N;
                f->a1           = 2.0 * (B[2] - B[0]) * N; // Sign negated
                f->a2           = (B[1] - B[2] - B[0]) * N; // Sign negated
                f->p0           = 0.0f;
                f->p1           = 0.0f;
                f->p2           = 0.0f;
            }
        }

        /*
            Original filter chain:

                       t[0] + t[1] * p + t[2] * p^2     k1 * (p + a[0]) * (p + a[1])
              H(p) =  ------------------------------ = -----------------------------
                       b[0] + b[1] * p + b[2] * p^2     k2 * (p + b[0]) * (p + b[1])

              a[0], a[1], b[0], b[1] may not exist, so there are series of solutions

            Matched Z-transform:

              T    = discretization period

              x    = z^-1

              p + a = 1 - x*exp(-a * T)

              kf   = 1 / f, f = filter frequency

            After the Matched Z-transform the Frequency Chart of the filter has to be normalized!

        */
        void Filter::matched_transform()
        {
            float T[4], B[4], A[2], I[2];
            float f         = sParams.fFreq;
            float TD        = 2.0*M_PI / nSampleRate;
            size_t chains   = 0;

            // Iterate each cascade
            for (size_t i=0; i<nItems; ++i)
            {
                dsp::f_cascade_t *c = &vItems[i];

                // Process each polynom (top, bottom) individually
                for (size_t i=0; i<2; ++i)
                {
                    float *p    = (i) ? c->b : c->t;
                    float *P    = (i) ? B : T;

                    if (p[2] == 0.0) // Test polynom for second-order
                    {
                        P[2]    = 0.0;
                        if (p[1] == 0.0) // Test polynom for first order
                        {
                            // Zero-order polynom
                            P[0]    = p[0];
                            P[1]    = 0.0;
                        }
                        else
                        {
                            // First-order polynom:
                            //   p(s) = p[0] + p[1]*(s/f)
                            //
                            // Transformed polynom:
                            //   P[z] = p[1]/f - p[1]/f * exp(-f*p[0]*T/p[1]) * z^-1
                            float k     = p[1]/f;
                            float R     = -p[0]/k;
                            P[0]        = k;
                            P[1]        = -k * expf(R*TD);
                        }
                    }
                    else
                    {
                        // Second-order polynom:
                        //   p(s) = p[0] + p[1]*(s/f) + p[2]*(s/f)^2 = p[2]/f^2 * (p[0]*f^2/p[2] + p[1]*f/p[2]*s + s^2)
                        //
                        // Calculate the roots of the second-order polynom equation a*x^2 + b*x + c = 0
                        float k     = p[2];
                        float a     = 1.0/(f*f);
                        float b     = p[1]/(f*p[2]);
                        float c     = p[0]/p[2];
                        float D     = b*b - 4.0*a*c;

                        if (D >= 0)
                        {
                            // Has real roots R0 and R1
                            // Transformed form is:
                            //   P[z] = k*(1 - (exp(R0*T) + exp(R1*T))*z^-1 + exp((R0+R1)*T)*z^-2)
                            D           = sqrtf(D);
                            float R0    = (-b - D)/(2.0*a);
                            float R1    = (-b + D)/(2.0*a);
                            P[0]        = k;
                            P[1]        = -k * (expf(R0*TD) + expf(R1*TD));
                            P[2]        = k * expf((R0+R1)*TD);
                        }
                        else
                        {
                            // Has complex roots R+j*K and R-j*K
                            // Transformed form is:
                            //   P[z] = k*(1 - 2*exp(R*T)*cos(K*T)*z^-1 + exp(2*R*T)*z^-2)
                            D           = sqrtf(-D);
                            float R     = -b / (2.0*a);
                            float K     = D / (2.0*a);
                            P[0]        = k;
                            P[1]        = -2.0 * k * expf(R*TD) * cosf(K*TD);
                            P[2]        = k * expf(2.0*R*TD);
                        }
                    }

                    // We have to calculate the norming factor of the digital filter
                    // To do this, we should get the amplitude of the discrete transfer function
                    // at the control frequency and the amplitude of the continuous transfer function
                    // at the same frequency.
                    // As control frequency we take the f/10 value
                    // For the discrete transfer function it will be PI*0.2*f / SR
                    // For the normalized continuous transfer function it will be always 0.1

                    // Calculate the discrete transfer function part at specified frequency
                    double w    = M_PI * 0.2 * sParams.fFreq / nSampleRate;
                    double re   = P[0]*cos(2*w) + P[1]*cos(w) + P[2];
                    double im   = P[0]*sin(2*w) + P[1]*sin(w);
                    A[i]        = sqrt(re*re + im*im);

                    // Calculate the continuous transfer function part at 1 Hz
                    w           = 0.1;
                    re          = p[0] - p[2]*w*w;
                    im          = p[1]*w;
                    I[i]        = sqrt(re*re + im*im);
                }

                // Now calculate the convolution for the new polynom:
                /*
                           T[0] + T[1]*z^-1 + T[2]*z^-2
                  H(z) =  ------------------------------
                           B[0] + B[1]*z^-1 + B[2]*z^-2

                 */
                double AN       = (A[1]*I[0]) / (A[0]*I[1]); // Normalizing factor for the amplitude to match the analog filter
                double N        = 1.0 / B[0];

                // Initialize filter parameters
                if ((++chains) > FILTER_CHAINS_MAX)
                    break;
                dsp::biquad_x1_t *f  = pBank->add_chain();
                if (f == NULL)
                    break;

                f->b0           = T[0] * N * AN;
                f->b1           = T[1] * N * AN;
                f->b2           = T[2] * N * AN;
                f->a1           = -B[1] * N; // Sign negated
                f->a2           = -B[2] * N; // Sign negated
                f->p0           = 0.0f;
                f->p1           = 0.0f;
                f->p2           = 0.0f;
            }
        }

        bool Filter::impulse_response(float *out, size_t length)
        {
            if (!(nFlags & FF_OWN_BANK))
                return false;

            if (nFlags & (~FF_OWN_BANK))
                rebuild();

            pBank->impulse_response(out, length);
            return true;
        }

        void Filter::dump(IStateDumper *v) const
        {
            if (nFlags & FF_OWN_BANK)
                v->write_object("pBank", pBank);
            else
                v->write("pBank", pBank);

            v->begin_object("sParams", &sParams, sizeof(filter_params_t));
            {
                v->write("nType", sParams.nType);
                v->write("fFreq", sParams.fFreq);
                v->write("fFreq2", sParams.fFreq2);
                v->write("fGain", sParams.fGain);
                v->write("nSlope", sParams.nSlope);
                v->write("fQuality", sParams.fQuality);
            }
            v->end_object();

            v->write("nSampleRate", nSampleRate);
            v->write("nMode", nMode);
            v->write("nItems", nItems);
            v->begin_array("vItems", vItems, nItems);
            for (size_t i=0; i<nItems; ++i)
            {
                dsp::f_cascade_t *c = &vItems[i];
                v->begin_object(c, sizeof(dsp::f_cascade_t));
                {
                    v->writev("t", c->t, 4);
                    v->writev("b", c->b, 4);
                }
                v->end_object();
            }
            v->end_array();
            v->write("vData", vData);
            v->write("nFlags", nFlags);
            v->write("nLatency", nLatency);
        }
    }
}


