#include <math.h>
#include <string.h>

#include "biquad_filter.h"

/**
 *  Unit_BiquadFilter implements a second order IIR filter.
 *
 *  Here is the equation that we use for this filter:
 *      y(n) = a0*x(n) + a1*x(n-1)  + a2*x(n-2) - b1*y(n-1)  - b2*y(n-2)
 *
 * @author (C) 2002 Phil Burk, SoftSynth.com, All Rights Reserved
 */

#define FILTER_PI  (3.141592653589793238462643)
/***********************************************************
** Calculate coefficients common to many parametric biquad filters.
*/
static void BiquadFilter_CalculateCommon( BiquadFilter *filter, double ratio, double Q )
{
    double omega;

    memset( filter, 0, sizeof(BiquadFilter) );

/* Don't let frequency get too close to Nyquist or filter will blow up. */
    if( ratio >= 0.499 ) ratio = 0.499;
    omega = 2.0 * (double)FILTER_PI * ratio;

    filter->cos_omega = (double) cos( omega );
    filter->sin_omega = (double) sin( omega );
    filter->alpha = filter->sin_omega / (2.0 * Q);
}

/*********************************************************************************
 ** Calculate coefficients for Highpass filter.
 */
void BiquadFilter_SetupHighPass( BiquadFilter *filter, double ratio, double Q )
{
    double    scalar, opc;

    if( ratio  < BIQUAD_MIN_RATIO )  ratio  = BIQUAD_MIN_RATIO;
    if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;

    BiquadFilter_CalculateCommon( filter, ratio, Q );

    scalar = 1.0 / (1.0 + filter->alpha);
    opc = (1.0 + filter->cos_omega);

    filter->a0 = opc * 0.5 * scalar;
    filter->a1 =  - opc * scalar;
    filter->a2 = filter->a0;
    filter->b1 = -2.0 * filter->cos_omega * scalar;
    filter->b2 = (1.0 - filter->alpha) * scalar;
}


/*********************************************************************************
 ** Calculate coefficients for Notch filter.
 */
void BiquadFilter_SetupNotch( BiquadFilter *filter, double ratio, double Q )
{
    double    scalar, opc;

    if( ratio  < BIQUAD_MIN_RATIO )  ratio  = BIQUAD_MIN_RATIO;
    if( Q < BIQUAD_MIN_Q ) Q = BIQUAD_MIN_Q;

    BiquadFilter_CalculateCommon( filter, ratio, Q );

    scalar = 1.0 / (1.0 + filter->alpha);
    opc = (1.0 + filter->cos_omega);

    filter->a0 = scalar;
    filter->a1 =  -2.0 * filter->cos_omega * scalar;
    filter->a2 = filter->a0;
    filter->b1 = filter->a1;
    filter->b2 = (1.0 - filter->alpha) * scalar;
}

/*****************************************************************
** Perform core IIR filter calculation without permutation.
*/
void BiquadFilter_Filter( BiquadFilter *filter, float *inputs, float *outputs, int numSamples )
{
    int i;
    double xn, yn;
    // Pull values from structure to speed up the calculation.
    double a0 = filter->a0;
    double a1 = filter->a1;
    double a2 = filter->a2;
    double b1 = filter->b1;
    double b2 = filter->b2;
    double xn1 = filter->xn1;
    double xn2 = filter->xn2;
    double yn1 = filter->yn1;
    double yn2 = filter->yn2;

    for( i=0; i<numSamples; i++)
    {
        // Generate outputs by filtering inputs.
        xn = inputs[i];
        yn = (a0 * xn) + (a1 * xn1) + (a2 * xn2) - (b1 * yn1) - (b2 * yn2);
        outputs[i] = yn;

        // Delay input and output values.
        xn2 = xn1;
        xn1 = xn;
        yn2 = yn1;
        yn1 = yn;

        if( (i & 7) == 0 )
        {
            // Apply a small bipolar impulse to filter to prevent arithmetic underflow.
            // Underflows can cause the FPU to interrupt the CPU.
            yn1 += (double) 1.0E-26;
            yn2 -= (double) 1.0E-26;
        }
    }

    filter->xn1 = xn1;
    filter->xn2 = xn2;
    filter->yn1 = yn1;
    filter->yn2 = yn2;
}