/*************** AutoWah.c ********************************/

#include "bp_iir.h"
#include "AutoWah.h"

static short center_freq;
static short samp_freq;
static short counter;
static short counter_limit;
static short control;
static short max_freq;
static short min_freq;
static short f_step;
static struct bp_filter H;

/*
This is the auto wah effect initialization function.
This initializes the band pass filter and the effect control variables
*/
void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step) {
        double C;

        /*Process variables*/
        center_freq = 0;
        samp_freq = sampling;
        counter = effect_rate;
        control = 0;

        /*User Parametters*/
        counter_limit = effect_rate;
       
        /*Convert frequencies to index ranges*/
        min_freq = 0;
        max_freq = (maxf - minf)/freq_step;

        bp_iir_init(sampling,gainfactor,Q,freq_step,minf);
        f_step = freq_step;
}

/*
This function generates the current output value
Note that if the input and output signal are integer
unsigned types, we need to add a half scale offset
*/
double AutoWah_process(int xin) {
        double yout;

        yout = bp_iir_filter(xin,&H);
        #ifdef INPUT_UNSIGNED
                yout += 32767;
        #endif
       
        return yout;
}

/*
This function will emulate a LFO that will vary according
to the effect_rate parameter set in the AutoWah_init function.
*/
void AutoWah_sweep(void) {
        double yout;
     
        if (!--counter) {
                if (!control) {
                        bp_iir_setup(&H,(center_freq+=f_step));
                        if (center_freq > max_freq) {
                                control = 1;
                        }
                }
                else if (control) {
                        bp_iir_setup(&H,(center_freq-=f_step));
                        if (center_freq == min_freq) {
                                control = 0;
                        }
                }

                counter = counter_limit;
        }
}

/*************** AutoWah.h ****************************/

#ifndef __AUTOWAH_H__
#define __AUTOWAH_H__

extern void AutoWah_init(short effect_rate,short sampling,short maxf,short minf,short Q,double gainfactor,short freq_step);
extern double AutoWah_process(int xin);
extern void AutoWah_sweep(void);

#endif

/************** Usage Example **************************/

#include "AutoWah.h"

void main(void) {
    short xin;
    short yout;
    AutoWah_init(2000,  /*Effect rate 2000*/
                 16000, /*Sampling Frequency*/
                 1000,  /*Maximum frequency*/
                 500,   /*Minimum frequency*/
                 4,     /*Q*/
                 0.707, /*Gain factor*/
                 10     /*Frequency increment*/
                 );  

    while(1) {
        if (new_sample_flag()) {
            /*When there's new sample at your ADC or CODEC input*/
            /*Read the sample*/
            xin = read_sample();
            /*Apply AutoWah_process function to the sample*/
            yout =AutoWah_process(xin);

            /*Send the output value to your DAC or codec output*/
            write_output(yout);
            /*Makes the LFO vary*/
            AutoWah_sweep();
        }                              
    }
}