/*  iir.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2014, 2022, 2023 Warren Pratt, NR0V

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

The author can be reached by email at

warren@wpratt.com

*/

#include "comm.h"

/********************************************************************************************************
*                                                                                                       *
*                                           Bi-Quad Notch                                               *
*                                                                                                       *
********************************************************************************************************/

void calc_snotch (SNOTCH a)
{
    double fn, qk, qr, csn;
    fn = a->f / (double)a->rate;
    csn = cos (TWOPI * fn);
    qr = 1.0 - 3.0 * a->bw;
    qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn));
    a->a0 = + qk;
    a->a1 = - 2.0 * qk * csn;
    a->a2 = + qk;
    a->b1 = + 2.0 * qr * csn;
    a->b2 = - qr * qr;
    flush_snotch (a);
}

SNOTCH create_snotch (int run, int size, double* in, double* out, int rate, double f, double bw)
{
    SNOTCH a = (SNOTCH) malloc0 (sizeof (snotch));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->f = f;
    a->bw = bw;
    InitializeCriticalSectionAndSpinCount ( &a->cs_update, 2500 );
    calc_snotch (a);
    return a;
}

void destroy_snotch (SNOTCH a)
{
    DeleteCriticalSection (&a->cs_update);
    _aligned_free (a);
}

void flush_snotch (SNOTCH a)
{
    a->x1 = a->x2 = a->y1 = a->y2 = 0.0;
}

void xsnotch (SNOTCH a)
{
    EnterCriticalSection (&a->cs_update);
    if (a->run)
    {
        int i;
        for (i = 0; i < a->size; i++)
        {
            a->x0 = a->in[2 * i + 0];
            a->out[2 * i + 0] = a->a0 * a->x0 + a->a1 * a->x1 + a->a2 * a->x2 + a->b1 * a->y1 + a->b2 * a->y2;
            a->y2 = a->y1;
            a->y1 = a->out[2 * i + 0];
            a->x2 = a->x1;
            a->x1 = a->x0;
        }
    }
    else if (a->out != a->in)
        memcpy (a->out, a->in, a->size * sizeof (complex));
    LeaveCriticalSection (&a->cs_update);
}

void setBuffers_snotch (SNOTCH a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_snotch (SNOTCH a, int rate)
{
    a->rate = rate;
    calc_snotch (a);
}

void setSize_snotch (SNOTCH a, int size)
{
    a->size = size;
    flush_snotch (a);
}

/********************************************************************************************************
*                                                                                                       *
*                                           RXA Properties                                              *
*                                                                                                       *
********************************************************************************************************/

void SetSNCTCSSFreq (SNOTCH a, double freq)
{
    EnterCriticalSection (&a->cs_update);
    a->f = freq;
    calc_snotch (a);
    LeaveCriticalSection (&a->cs_update);
}

void SetSNCTCSSRun (SNOTCH a, int run)
{
    EnterCriticalSection (&a->cs_update);
    a->run = run;
    LeaveCriticalSection (&a->cs_update);
}


/********************************************************************************************************
*                                                                                                       *
*                                           Complex Bi-Quad Peaking                                     *
*                                                                                                       *
********************************************************************************************************/

void calc_speak (SPEAK a)
{
    double ratio;
    double f_corr, g_corr, bw_corr, bw_parm, A, f_min;

    switch (a->design)
    {
    case 0:
        ratio = a->bw / a->f;
        switch (a->nstages)
        {
        case 4:
            bw_parm = 2.4;
            f_corr  = 1.0 - 0.160 * ratio + 1.440 * ratio * ratio;
            g_corr = 1.0 - 1.003 * ratio + 3.990 * ratio * ratio;
            break;
        default:
            bw_parm = 1.0;
            f_corr  = 1.0;
            g_corr = 1.0;
            break;
        }
        {
            double fn, qk, qr, csn;
            a->fgain = a->gain / g_corr;
            fn = a->f / (double)a->rate / f_corr;
            csn = cos (TWOPI * fn);
            qr = 1.0 - 3.0 * a->bw / (double)a->rate * bw_parm;
            qk = (1.0 - 2.0 * qr * csn + qr * qr) / (2.0 * (1.0 - csn));
            a->a0 = 1.0 - qk;
            a->a1 = 2.0 * (qk - qr) * csn;
            a->a2 = qr * qr - qk;
            a->b1 = 2.0 * qr * csn;
            a->b2 = - qr * qr;
        }
        break;

    case 1:
        if (a->f < 200.0) a->f = 200.0;
        ratio = a->bw / a->f;
        switch (a->nstages)
        {
        case 4:
            bw_parm = 5.0;
            bw_corr = 1.13 * ratio - 0.956 * ratio * ratio;
            A = 2.5;
            f_min = 50.0;
            break;
        default:
            bw_parm = 1.0;
            bw_corr  = 1.0;
            g_corr = 1.0;
            A = 2.5;
            f_min = 50.0;
            break;
        }
        {
            double w0, sn, c, den;
            if (a->f < f_min) a->f = f_min;
            w0 = TWOPI * a->f / (double)a->rate;
            sn = sin (w0);
            a->cbw = bw_corr * a->f;
            c = sn * sinh(0.5 * log((a->f + 0.5 * a->cbw * bw_parm) / (a->f - 0.5 * a->cbw * bw_parm)) * w0 / sn);
            den = 1.0 + c / A;
            a->a0 = (1.0 + c * A) / den;
            a->a1 = - 2.0 * cos (w0) / den;
            a->a2 = (1 - c * A) / den;
            a->b1 = - a->a1;
            a->b2 = - (1 - c / A ) / den;
            a->fgain = a->gain / pow (A * A, (double)a->nstages);
        }
        break;
    }
    flush_speak (a);
}

SPEAK create_speak (int run, int size, double* in, double* out, int rate, double f, double bw, double gain, int nstages, int design)
{
    SPEAK a = (SPEAK) malloc0 (sizeof (speak));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->f = f;
    a->bw = bw;
    a->gain = gain;
    a->nstages = nstages;
    a->design = design;
    a->x0 = (double *) malloc0 (a->nstages * sizeof (complex));
    a->x1 = (double *) malloc0 (a->nstages * sizeof (complex));
    a->x2 = (double *) malloc0 (a->nstages * sizeof (complex));
    a->y0 = (double *) malloc0 (a->nstages * sizeof (complex));
    a->y1 = (double *) malloc0 (a->nstages * sizeof (complex));
    a->y2 = (double *) malloc0 (a->nstages * sizeof (complex));
    InitializeCriticalSectionAndSpinCount ( &a->cs_update, 2500 );
    calc_speak (a);
    return a;
}

void destroy_speak (SPEAK a)
{
    DeleteCriticalSection (&a->cs_update);
    _aligned_free (a->y2);
    _aligned_free (a->y1);
    _aligned_free (a->y0);
    _aligned_free (a->x2);
    _aligned_free (a->x1);
    _aligned_free (a->x0);
    _aligned_free (a);
}

void flush_speak (SPEAK a)
{
    int i;
    for (i = 0; i < a->nstages; i++)
    {
        a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0;
        a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0;
    }
}

void xspeak (SPEAK a)
{
    EnterCriticalSection (&a->cs_update);
    if (a->run)
    {
        int i, j, n;
        for (i = 0; i < a->size; i++)
        {
            for (j = 0; j < 2; j++)
            {
                a->x0[j] = a->fgain * a->in[2 * i + j];
                for (n = 0; n < a->nstages; n++)
                {
                    if (n > 0) a->x0[2 * n + j] = a->y0[2 * (n - 1) + j];
                    a->y0[2 * n + j]    = a->a0 * a->x0[2 * n + j]
                                        + a->a1 * a->x1[2 * n + j]
                                        + a->a2 * a->x2[2 * n + j]
                                        + a->b1 * a->y1[2 * n + j]
                                        + a->b2 * a->y2[2 * n + j];
                    a->y2[2 * n + j] = a->y1[2 * n + j];
                    a->y1[2 * n + j] = a->y0[2 * n + j];
                    a->x2[2 * n + j] = a->x1[2 * n + j];
                    a->x1[2 * n + j] = a->x0[2 * n + j];
                }
                a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j];
            }
        }
    }
    else if (a->out != a->in)
        memcpy (a->out, a->in, a->size * sizeof (complex));
    LeaveCriticalSection (&a->cs_update);
}

void setBuffers_speak (SPEAK a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_speak (SPEAK a, int rate)
{
    a->rate = rate;
    calc_speak (a);
}

void setSize_speak (SPEAK a, int size)
{
    a->size = size;
    flush_speak (a);
}

/********************************************************************************************************
*                                                                                                       *
*                                           RXA Properties                                              *
*                                                                                                       *
********************************************************************************************************/

PORT
void SetRXASPCWRun (int channel, int run)
{
    SPEAK a = rxa[channel].speak.p;
    EnterCriticalSection (&a->cs_update);
    a->run = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXASPCWFreq (int channel, double freq)
{
    SPEAK a = rxa[channel].speak.p;
    EnterCriticalSection (&a->cs_update);
    a->f = freq;
    calc_speak (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXASPCWBandwidth (int channel, double bw)
{
    SPEAK a = rxa[channel].speak.p;
    EnterCriticalSection (&a->cs_update);
    a->bw = bw;
    calc_speak (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXASPCWGain (int channel, double gain)
{
    SPEAK a = rxa[channel].speak.p;
    EnterCriticalSection (&a->cs_update);
    a->gain = gain;
    calc_speak (a);
    LeaveCriticalSection (&a->cs_update);
}

/********************************************************************************************************
*                                                                                                       *
*                                       Complex Multiple Peaking                                        *
*                                                                                                       *
********************************************************************************************************/

void calc_mpeak (MPEAK a)
{
    int i;
    a->tmp = (double *) malloc0 (a->size * sizeof (complex));
    a->mix = (double *) malloc0 (a->size * sizeof (complex));
    for (i = 0; i < a->npeaks; i++)
    {
        a->pfil[i] = create_speak ( 1,
                                    a->size,
                                    a->in,
                                    a->tmp,
                                    a->rate,
                                    a->f[i],
                                    a->bw[i],
                                    a->gain[i],
                                    a->nstages,
                                    1 );
    }
}

void decalc_mpeak (MPEAK a)
{
    int i;
    for (i = 0; i < a->npeaks; i++)
        destroy_speak (a->pfil[i]);
    _aligned_free (a->mix);
    _aligned_free (a->tmp);
}

MPEAK create_mpeak (int run, int size, double* in, double* out, int rate, int npeaks, int* enable, double* f, double* bw, double* gain, int nstages)
{
    MPEAK a = (MPEAK) malloc0 (sizeof (mpeak));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->npeaks = npeaks;
    a->nstages = nstages;
    a->enable  = (int *) malloc0 (a->npeaks * sizeof (int));
    a->f    = (double *) malloc0 (a->npeaks * sizeof (double));
    a->bw   = (double *) malloc0 (a->npeaks * sizeof (double));
    a->gain = (double *) malloc0 (a->npeaks * sizeof (double));
    memcpy (a->enable, enable, a->npeaks * sizeof (int));
    memcpy (a->f, f, a->npeaks * sizeof (double));
    memcpy (a->bw, bw, a->npeaks * sizeof (double));
    memcpy (a->gain, gain, a->npeaks * sizeof (double));
    a->pfil = (SPEAK *) malloc0 (a->npeaks * sizeof (SPEAK));
    InitializeCriticalSectionAndSpinCount ( &a->cs_update, 2500 );
    calc_mpeak (a);
    return a;
}

void destroy_mpeak (MPEAK a)
{
    decalc_mpeak (a);
    DeleteCriticalSection (&a->cs_update);
    _aligned_free (a->pfil);
    _aligned_free (a->gain);
    _aligned_free (a->bw);
    _aligned_free (a->f);
    _aligned_free (a->enable);
    _aligned_free (a);
}

void flush_mpeak (MPEAK a)
{
    int i;
    for (i = 0; i < a->npeaks; i++)
        flush_speak (a->pfil[i]);
}

void xmpeak (MPEAK a)
{
    EnterCriticalSection (&a->cs_update);
    if (a->run)
    {
        int i, j;
        memset (a->mix, 0, a->size * sizeof (complex));
        for (i = 0; i < a->npeaks; i++)
        {
            if (a->enable[i])
            {
                xspeak (a->pfil[i]);
                for (j = 0; j < 2 * a->size; j++)
                    a->mix[j] += a->tmp[j];
            }
        }
        memcpy (a->out, a->mix, a->size * sizeof (complex));
    }
    else if (a->in != a->out)
        memcpy (a->out, a->in, a->size * sizeof (complex));
    LeaveCriticalSection (&a->cs_update);
}

void setBuffers_mpeak (MPEAK a, double* in, double* out)
{
    decalc_mpeak (a);
    a->in = in;
    a->out = out;
    calc_mpeak (a);
}

void setSamplerate_mpeak (MPEAK a, int rate)
{
    decalc_mpeak (a);
    a->rate = rate;
    calc_mpeak (a);
}

void setSize_mpeak (MPEAK a, int size)
{
    decalc_mpeak (a);
    a->size = size;
    calc_mpeak (a);
}

/********************************************************************************************************
*                                                                                                       *
*                                           RXA Properties                                              *
*                                                                                                       *
********************************************************************************************************/

PORT
void SetRXAmpeakRun (int channel, int run)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->run = run;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXAmpeakNpeaks (int channel, int npeaks)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->npeaks = npeaks;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXAmpeakFilEnable (int channel, int fil, int enable)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->enable[fil] = enable;
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXAmpeakFilFreq (int channel, int fil, double freq)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->f[fil] = freq;
    a->pfil[fil]->f = freq;
    calc_speak(a->pfil[fil]);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXAmpeakFilBw (int channel, int fil, double bw)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->bw[fil] = bw;
    a->pfil[fil]->bw = bw;
    calc_speak(a->pfil[fil]);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetRXAmpeakFilGain (int channel, int fil, double gain)
{
    MPEAK a = rxa[channel].mpeak.p;
    EnterCriticalSection (&a->cs_update);
    a->gain[fil] = gain;
    a->pfil[fil]->gain = gain;
    calc_speak(a->pfil[fil]);
    LeaveCriticalSection (&a->cs_update);
}


/********************************************************************************************************
*                                                                                                       *
*                                           Phase Rotator                                               *
*                                                                                                       *
********************************************************************************************************/

void calc_phrot (PHROT a)
{
    double g;
    a->x0 = (double *) malloc0 (a->nstages * sizeof (double));
    a->x1 = (double *) malloc0 (a->nstages * sizeof (double));
    a->y0 = (double *) malloc0 (a->nstages * sizeof (double));
    a->y1 = (double *) malloc0 (a->nstages * sizeof (double));
    g = tan (PI * a->fc / (double)a->rate);
    a->b0 = (g - 1.0) / (g + 1.0);
    a->b1 = 1.0;
    a->a1 = a->b0;
}

PHROT create_phrot (int run, int size, double* in, double* out, int rate, double fc, int nstages)
{
    PHROT a = (PHROT) malloc0 (sizeof (phrot));
    a->reverse = 0;
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->fc = fc;
    a->nstages = nstages;
    InitializeCriticalSectionAndSpinCount ( &a->cs_update, 2500 );
    calc_phrot (a);
    return a;
}

void decalc_phrot (PHROT a)
{
    _aligned_free (a->y1);
    _aligned_free (a->y0);
    _aligned_free (a->x1);
    _aligned_free (a->x0);
}

void destroy_phrot (PHROT a)
{
    decalc_phrot (a);
    DeleteCriticalSection (&a->cs_update);
    _aligned_free (a);
}

void flush_phrot (PHROT a)
{
    memset (a->x0, 0, a->nstages * sizeof (double));
    memset (a->x1, 0, a->nstages * sizeof (double));
    memset (a->y0, 0, a->nstages * sizeof (double));
    memset (a->y1, 0, a->nstages * sizeof (double));
}

void xphrot (PHROT a)
{
    EnterCriticalSection (&a->cs_update);
    if (a->reverse)
    {
        for (int i = 0; i < a->size; i++)
            a->in[2 * i + 0] = -a->in[2 * i + 0];
    }
    if (a->run)
    {
        int i, n;
        for (i = 0; i < a->size; i++)
        {
            a->x0[0] = a->in[2 * i + 0];
            for (n = 0; n < a->nstages; n++)
            {
                if (n > 0) a->x0[n] = a->y0[n - 1];
                a->y0[n]    = a->b0 * a->x0[n]
                            + a->b1 * a->x1[n]
                            - a->a1 * a->y1[n];
                a->y1[n] = a->y0[n];
                a->x1[n] = a->x0[n];
            }
            a->out[2 * i + 0] = a->y0[a->nstages - 1];
        }
    }
    else if (a->out != a->in)
        memcpy (a->out, a->in, a->size * sizeof (complex));
    LeaveCriticalSection (&a->cs_update);
}

void setBuffers_phrot (PHROT a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_phrot (PHROT a, int rate)
{
    decalc_phrot (a);
    a->rate = rate;
    calc_phrot (a);
}

void setSize_phrot (PHROT a, int size)
{
    a->size = size;
    flush_phrot (a);
}

/********************************************************************************************************
*                                                                                                       *
*                                           TXA Properties                                              *
*                                                                                                       *
********************************************************************************************************/

PORT
void SetTXAPHROTRun (int channel, int run)
{
    PHROT a = txa[channel].phrot.p;
    EnterCriticalSection (&a->cs_update);
    a->run = run;
    if (a->run) flush_phrot (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetTXAPHROTCorner (int channel, double corner)
{
    PHROT a = txa[channel].phrot.p;
    EnterCriticalSection (&a->cs_update);
    decalc_phrot (a);
    a->fc = corner;
    calc_phrot (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetTXAPHROTNstages (int channel, int nstages)
{
    PHROT a = txa[channel].phrot.p;
    EnterCriticalSection (&a->cs_update);
    decalc_phrot (a);
    a->nstages = nstages;
    calc_phrot (a);
    LeaveCriticalSection (&a->cs_update);
}

PORT
void SetTXAPHROTReverse (int channel, int reverse)
{
    PHROT a = txa[channel].phrot.p;
    EnterCriticalSection(&a->cs_update);
    a->reverse = reverse;
    LeaveCriticalSection(&a->cs_update);
}

/********************************************************************************************************
*                                                                                                       *
*                                   Complex Bi-Quad Low-Pass                                            *
*                                                                                                       *
********************************************************************************************************/

void calc_bqlp(BQLP a)
{
    double w0, cs, c, den;
    w0 = TWOPI * a->fc / (double)a->rate;
    cs = cos(w0);
    c = sin(w0) / (2.0 * a->Q);
    den = 1.0 + c;
    a->a0 = 0.5 * (1.0 - cs) / den;
    a->a1 = (1.0 - cs) / den;
    a->a2 = 0.5 * (1.0 - cs) / den;
    a->b1 = 2.0 * cs / den;
    a->b2 = (c - 1.0) / den;
    flush_bqlp(a);
}

BQLP create_bqlp(int run, int size, double* in, double* out, double rate, double fc, double Q, double gain, int nstages)
{
    BQLP a = (BQLP)malloc0(sizeof(bqlp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->fc = fc;
    a->Q = Q;
    a->gain = gain;
    a->nstages = nstages;
    a->x0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->x1 = (double*)malloc0(a->nstages * sizeof(complex));
    a->x2 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y1 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y2 = (double*)malloc0(a->nstages * sizeof(complex));
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_bqlp(a);
    return a;
}

void destroy_bqlp(BQLP a)
{
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a->y2);
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x2);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
    _aligned_free(a);
}

void flush_bqlp(BQLP a)
{
    int i;
    for (i = 0; i < a->nstages; i++)
    {
        a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0;
        a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0;
    }
}

void xbqlp(BQLP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, j, n;
        for (i = 0; i < a->size; i++)
        {
            for (j = 0; j < 2; j++)
            {
                a->x0[j] = a->gain * a->in[2 * i + j];
                for (n = 0; n < a->nstages; n++)
                {
                    if (n > 0) a->x0[2 * n + j] = a->y0[2 * (n - 1) + j];
                    a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j]
                        + a->a1 * a->x1[2 * n + j]
                        + a->a2 * a->x2[2 * n + j]
                        + a->b1 * a->y1[2 * n + j]
                        + a->b2 * a->y2[2 * n + j];
                    a->y2[2 * n + j] = a->y1[2 * n + j];
                    a->y1[2 * n + j] = a->y0[2 * n + j];
                    a->x2[2 * n + j] = a->x1[2 * n + j];
                    a->x1[2 * n + j] = a->x0[2 * n + j];
                }
                a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j];
            }
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(complex));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_bqlp(BQLP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_bqlp(BQLP a, int rate)
{
    a->rate = rate;
    calc_bqlp(a);
}

void setSize_bqlp(BQLP a, int size)
{
    a->size = size;
    flush_bqlp(a);
}

/********************************************************************************************************
*                                                                                                       *
*                                      Double Bi-Quad Low-Pass                                          *
*                                                                                                       *
********************************************************************************************************/

void calc_dbqlp(BQLP a)
{
    double w0, cs, c, den;
    w0 = TWOPI * a->fc / (double)a->rate;
    cs = cos(w0);
    c = sin(w0) / (2.0 * a->Q);
    den = 1.0 + c;
    a->a0 = 0.5 * (1.0 - cs) / den;
    a->a1 = (1.0 - cs) / den;
    a->a2 = 0.5 * (1.0 - cs) / den;
    a->b1 = 2.0 * cs / den;
    a->b2 = (c - 1.0) / den;
    flush_dbqlp(a);
}

BQLP create_dbqlp(int run, int size, double* in, double* out, double rate, double fc, double Q, double gain, int nstages)
{
    BQLP a = (BQLP)malloc0(sizeof(bqlp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->fc = fc;
    a->Q = Q;
    a->gain = gain;
    a->nstages = nstages;
    a->x0 = (double*)malloc0(a->nstages * sizeof(double));
    a->x1 = (double*)malloc0(a->nstages * sizeof(double));
    a->x2 = (double*)malloc0(a->nstages * sizeof(double));
    a->y0 = (double*)malloc0(a->nstages * sizeof(double));
    a->y1 = (double*)malloc0(a->nstages * sizeof(double));
    a->y2 = (double*)malloc0(a->nstages * sizeof(double));
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_dbqlp(a);
    return a;
}

void destroy_dbqlp(BQLP a)
{
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a->y2);
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x2);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
    _aligned_free(a);
}

void flush_dbqlp(BQLP a)
{
    int i;
    for (i = 0; i < a->nstages; i++)
    {
        a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0;
    }
}

void xdbqlp(BQLP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, n;
        for (i = 0; i < a->size; i++)
        {
            a->x0[0] = a->gain * a->in[i];
            for (n = 0; n < a->nstages; n++)
            {
                if (n > 0) a->x0[n] = a->y0[n - 1];
                a->y0[n] = a->a0 * a->x0[n]
                    + a->a1 * a->x1[n]
                    + a->a2 * a->x2[n]
                    + a->b1 * a->y1[n]
                    + a->b2 * a->y2[n];
                a->y2[n] = a->y1[n];
                a->y1[n] = a->y0[n];
                a->x2[n] = a->x1[n];
                a->x1[n] = a->x0[n];
            }
            a->out[i] = a->y0[a->nstages - 1];
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(double));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_dbqlp(BQLP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_dbqlp(BQLP a, int rate)
{
    a->rate = rate;
    calc_dbqlp(a);
}

void setSize_dbqlp(BQLP a, int size)
{
    a->size = size;
    flush_dbqlp(a);
}


/********************************************************************************************************
*                                                                                                       *
*                                   Complex Bi-Quad Band-Pass                                           *
*                                                                                                       *
********************************************************************************************************/

void calc_bqbp(BQBP a)
{
    double f0, w0, bw, q, sn, cs, c, den;
    bw = a->f_high - a->f_low;
    f0 = (a->f_high + a->f_low) / 2.0;
    q = f0 / bw;
    w0 = TWOPI * f0 / a->rate;
    sn = sin(w0);
    cs = cos(w0);
    c = sn / (2.0 * q);
    den = 1.0 + c;
    a->a0 = +c / den;
    a->a1 = 0.0;
    a->a2 = -c / den;
    a->b1 = 2.0 * cs / den;
    a->b2 = (c - 1.0) / den;
    flush_bqbp(a);
}

BQBP create_bqbp(int run, int size, double* in, double* out, double rate, double f_low, double f_high, double gain, int nstages)
{
    BQBP a = (BQBP)malloc0(sizeof(bqbp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->f_low = f_low;
    a->f_high = f_high;
    a->gain = gain;
    a->nstages = nstages;
    a->x0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->x1 = (double*)malloc0(a->nstages * sizeof(complex));
    a->x2 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y1 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y2 = (double*)malloc0(a->nstages * sizeof(complex));
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_bqbp(a);
    return a;
}

void destroy_bqbp(BQBP a)
{
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a->y2);
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x2);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
    _aligned_free(a);
}

void flush_bqbp(BQBP a)
{
    int i;
    for (i = 0; i < a->nstages; i++)
    {
        a->x1[2 * i + 0] = a->x2[2 * i + 0] = a->y1[2 * i + 0] = a->y2[2 * i + 0] = 0.0;
        a->x1[2 * i + 1] = a->x2[2 * i + 1] = a->y1[2 * i + 1] = a->y2[2 * i + 1] = 0.0;
    }
}

void xbqbp(BQBP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, j, n;
        for (i = 0; i < a->size; i++)
        {
            for (j = 0; j < 2; j++)
            {
                a->x0[j] = a->gain * a->in[2 * i + j];
                for (n = 0; n < a->nstages; n++)
                {
                    if (n > 0) a->x0[2 * n + j] = a->y0[2 * (n - 1) + j];
                    a->y0[2 * n + j] = a->a0 * a->x0[2 * n + j]
                        + a->a1 * a->x1[2 * n + j]
                        + a->a2 * a->x2[2 * n + j]
                        + a->b1 * a->y1[2 * n + j]
                        + a->b2 * a->y2[2 * n + j];
                    a->y2[2 * n + j] = a->y1[2 * n + j];
                    a->y1[2 * n + j] = a->y0[2 * n + j];
                    a->x2[2 * n + j] = a->x1[2 * n + j];
                    a->x1[2 * n + j] = a->x0[2 * n + j];
                }
                a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j];
            }
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(complex));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_bqbp(BQBP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_bqbp(BQBP a, int rate)
{
    a->rate = rate;
    calc_bqbp(a);
}

void setSize_bqbp(BQBP a, int size)
{
    a->size = size;
    flush_bqbp(a);
}

/********************************************************************************************************
*                                                                                                       *
*                                     Double Bi-Quad Band-Pass                                          *
*                                                                                                       *
********************************************************************************************************/

void calc_dbqbp(BQBP a)
{
    double f0, w0, bw, q, sn, cs, c, den;
    bw = a->f_high - a->f_low;
    f0 = (a->f_high + a->f_low) / 2.0;
    q = f0 / bw;
    w0 = TWOPI * f0 / a->rate;
    sn = sin(w0);
    cs = cos(w0);
    c = sn / (2.0 * q);
    den = 1.0 + c;
    a->a0 = +c / den;
    a->a1 = 0.0;
    a->a2 = -c / den;
    a->b1 = 2.0 * cs / den;
    a->b2 = (c - 1.0) / den;
    flush_dbqbp(a);
}

BQBP create_dbqbp(int run, int size, double* in, double* out, double rate, double f_low, double f_high, double gain, int nstages)
{
    BQBP a = (BQBP)malloc0(sizeof(bqbp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->f_low = f_low;
    a->f_high = f_high;
    a->gain = gain;
    a->nstages = nstages;
    a->x0 = (double*)malloc0(a->nstages * sizeof(double));
    a->x1 = (double*)malloc0(a->nstages * sizeof(double));
    a->x2 = (double*)malloc0(a->nstages * sizeof(double));
    a->y0 = (double*)malloc0(a->nstages * sizeof(double));
    a->y1 = (double*)malloc0(a->nstages * sizeof(double));
    a->y2 = (double*)malloc0(a->nstages * sizeof(double));
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_dbqbp(a);
    return a;
}

void destroy_dbqbp(BQBP a)
{
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a->y2);
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x2);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
    _aligned_free(a);
}

void flush_dbqbp(BQBP a)
{
    int i;
    for (i = 0; i < a->nstages; i++)
    {
        a->x1[i] = a->x2[i] = a->y1[i] = a->y2[i] = 0.0;
    }
}

void xdbqbp(BQBP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, n;
        for (i = 0; i < a->size; i++)
        {
            a->x0[0] = a->gain * a->in[i];
            for (n = 0; n < a->nstages; n++)
            {
                if (n > 0) a->x0[n] = a->y0[n - 1];
                a->y0[n] = a->a0 * a->x0[n]
                    + a->a1 * a->x1[n]
                    + a->a2 * a->x2[n]
                    + a->b1 * a->y1[n]
                    + a->b2 * a->y2[n];
                a->y2[n] = a->y1[n];
                a->y1[n] = a->y0[n];
                a->x2[n] = a->x1[n];
                a->x1[n] = a->x0[n];
            }
            a->out[i] = a->y0[a->nstages - 1];
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(double));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_dbqbp(BQBP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_dbqbp(BQBP a, int rate)
{
    a->rate = rate;
    calc_dbqbp(a);
}

void setSize_dbqbp(BQBP a, int size)
{
    a->size = size;
    flush_dbqbp(a);
}

/********************************************************************************************************
*                                                                                                       *
*                                    Complex Single-Pole High-Pass                                      *
*                                                                                                       *
********************************************************************************************************/

void calc_sphp(SPHP a)
{
    double g;
    a->x0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->x1 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y0 = (double*)malloc0(a->nstages * sizeof(complex));
    a->y1 = (double*)malloc0(a->nstages * sizeof(complex));
    g = exp(-TWOPI * a->fc / a->rate);
    a->b0 = +0.5 * (1.0 + g);
    a->b1 = -0.5 * (1.0 + g);
    a->a1 = -g;
}

SPHP create_sphp(int run, int size, double* in, double* out, double rate, double fc, int nstages)
{
    SPHP a = (SPHP)malloc0(sizeof(sphp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->fc = fc;
    a->nstages = nstages;
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_sphp(a);
    return a;
}

void decalc_sphp(SPHP a)
{
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
}

void destroy_sphp(SPHP a)
{
    decalc_sphp(a);
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a);
}

void flush_sphp(SPHP a)
{
    memset(a->x0, 0, a->nstages * sizeof(complex));
    memset(a->x1, 0, a->nstages * sizeof(complex));
    memset(a->y0, 0, a->nstages * sizeof(complex));
    memset(a->y1, 0, a->nstages * sizeof(complex));
}

void xsphp(SPHP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, j, n;
        for (i = 0; i < a->size; i++)
        {
            for (j = 0; j < 2; j++)
            {
                a->x0[j] = a->in[2 * i + j];
                for (n = 0; n < a->nstages; n++)
                {
                    if (n > 0) a->x0[2 * n + j] = a->y0[2 * (n - 1) + j];
                    a->y0[2 * n + j] = a->b0 * a->x0[2 * n + j]
                        + a->b1 * a->x1[2 * n + j]
                        - a->a1 * a->y1[2 * n + j];
                    a->y1[2 * n + j] = a->y0[2 * n + j];
                    a->x1[2 * n + j] = a->x0[2 * n + j];
                }
                a->out[2 * i + j] = a->y0[2 * (a->nstages - 1) + j];
            }
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(complex));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_sphp(SPHP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_sphp(SPHP a, int rate)
{
    decalc_sphp(a);
    a->rate = rate;
    calc_sphp(a);
}

void setSize_sphp(SPHP a, int size)
{
    a->size = size;
    flush_sphp(a);
}

/********************************************************************************************************
*                                                                                                       *
*                                    Double Single-Pole High-Pass                                       *
*                                                                                                       *
********************************************************************************************************/

void calc_dsphp(SPHP a)
{
    double g;
    a->x0 = (double*)malloc0(a->nstages * sizeof(double));
    a->x1 = (double*)malloc0(a->nstages * sizeof(double));
    a->y0 = (double*)malloc0(a->nstages * sizeof(double));
    a->y1 = (double*)malloc0(a->nstages * sizeof(double));
    g = exp(-TWOPI * a->fc / a->rate);
    a->b0 = +0.5 * (1.0 + g);
    a->b1 = -0.5 * (1.0 + g);
    a->a1 = -g;
}

SPHP create_dsphp(int run, int size, double* in, double* out, double rate, double fc, int nstages)
{
    SPHP a = (SPHP)malloc0(sizeof(sphp));
    a->run = run;
    a->size = size;
    a->in = in;
    a->out = out;
    a->rate = rate;
    a->fc = fc;
    a->nstages = nstages;
    InitializeCriticalSectionAndSpinCount(&a->cs_update, 2500);
    calc_dsphp(a);
    return a;
}

void decalc_dsphp(SPHP a)
{
    _aligned_free(a->y1);
    _aligned_free(a->y0);
    _aligned_free(a->x1);
    _aligned_free(a->x0);
}

void destroy_dsphp(SPHP a)
{
    decalc_dsphp(a);
    DeleteCriticalSection(&a->cs_update);
    _aligned_free(a);
}

void flush_dsphp(SPHP a)
{
    memset(a->x0, 0, a->nstages * sizeof(double));
    memset(a->x1, 0, a->nstages * sizeof(double));
    memset(a->y0, 0, a->nstages * sizeof(double));
    memset(a->y1, 0, a->nstages * sizeof(double));
}

void xdsphp(SPHP a)
{
    EnterCriticalSection(&a->cs_update);
    if (a->run)
    {
        int i, n;
        for (i = 0; i < a->size; i++)
        {
            a->x0[0] = a->in[i];
            for (n = 0; n < a->nstages; n++)
            {
                if (n > 0) a->x0[n] = a->y0[n - 1];
                a->y0[n] = a->b0 * a->x0[n]
                    + a->b1 * a->x1[n]
                    - a->a1 * a->y1[n];
                a->y1[n] = a->y0[n];
                a->x1[n] = a->x0[n];
            }
            a->out[i] = a->y0[a->nstages - 1];
        }
    }
    else if (a->out != a->in)
        memcpy(a->out, a->in, a->size * sizeof(double));
    LeaveCriticalSection(&a->cs_update);
}

void setBuffers_dsphp(SPHP a, double* in, double* out)
{
    a->in = in;
    a->out = out;
}

void setSamplerate_dsphp(SPHP a, int rate)
{
    decalc_dsphp(a);
    a->rate = rate;
    calc_dsphp(a);
}

void setSize_dsphp(SPHP a, int size)
{
    a->size = size;
    flush_dsphp(a);
}
