#include <stdlib.h>

typedef struct {
    unsigned int length; // number of taps (FIR) or 2nd order sections (IIR)
    double *history;     // pointer to history in filter
    coefType *coef;      // pointer to coefficients of filter
} FILTER;

coefType rxNotch4_28MHz[13] = {
    0.875858023357, 0.060371771939, 0.791096869568, 0.067413184585, 1.000000000000,   
    -0.045092550393, 0.980074362739, 0.006016312704, 1.000000000000,  0.178157737307,
    0.980149276082, 0.128683115389,   1.000000000000
};

FILTER rxNotch4 = {
    3,              // 3 sections 
    NULL,
    rxNotch4_28MHz
};

signalType iir_filter (signalType, FILTER *);

void RxNotchFix (signalType x, signalType *y)
{
    *y = iir_filter (x, &rxNotch4);
}

/*************************************************************************
iir_filter - Perform IIR filtering sample by sample on doubles
*************************************************************************/
signalType iir_filter (
		       signalType input, // new input sample
		       FILTER *iir       // pointer to FILTER structure
		       )
{
    unsigned    int i;
    double      *hist1_ptr, *hist2_ptr;

    coefType     *coef_ptr;
    signalTypeWN (output);
    signalTypeWN (new_hist);
    signalTypeWN (history1);
    signalTypeWN (history2);
    signalTypeWN (prod1);
    signalTypeWN (prod2);

    /* allocate history array if different size than last call */

    if (!iir->history) {
        iir->history = (double *) calloc (2*iir->length, sizeof (double));
        if (!iir->history) {
            // printf("\nUnable to allocate history array in iir_filter\n");
            exit(1);
        }
    }

    coef_ptr = iir->coef;     // coefficient pointer

    hist1_ptr = iir->history; // first history
    hist2_ptr = hist1_ptr +1; // next history

    output = input * (*coef_ptr++);  // overall input scale factor

    for (i=0; i<iir->length; i++) {
        history1 = *hist1_ptr; // history values
        history2 = *hist2_ptr;

	prod1 = history1 * (*coef_ptr++);
	prod2 = history2 * (*coef_ptr++);
        new_hist = output -prod1 -prod2;

        prod1 = history1 * (*coef_ptr++);
        prod2 = history2 * (*coef_ptr++);
	output = new_hist +prod1 +prod2;
        
        *hist2_ptr++ = *hist1_ptr;
        *hist1_ptr++ = new_hist;
        hist1_ptr++;
        hist2_ptr++;
    }

    return (output);
}