/* 

notch~.c

emulation of a Butterworth type band-reject filter
based on the example in Dodge and Jerse "Computer Music"

*/

#include "ext.h"
#include "math.h"
#include "z_dsp.h"

#define	ASSIST_RSRC	9591 // resource number for assistance strings

#ifndef PI
#define PI 3.14159265358979323846
#endif

#ifndef TWOPI
#define TWOPI 6.28318530717958647692
#endif

void *notch_class;

typedef struct _notch
{
    t_pxobject n_obj;
	float n_CF, n_BW; // center frequency and bandwidth of notch filter
    float n_xnm1, n_xnm2, n_ynm1, n_ynm2; // store past samples from one perform pass to another
    float n_oneoversr; // reciprocal of sample rate of input signal
    void *n_outlet;
} t_notch;

void *notch_new(float CF, float BW);
t_int *notch_perform(t_int *w); // if signals for both parameters
t_int *notch_perform1(t_int *w); // if no signal for CF
t_int *notch_perform2(t_int *w); // if no signal for BW
t_int *notch_perform3(t_int *w); // if no signals for parameters
void notch_int(t_notch *x, long n);
void notch_float(t_notch *x, float f);
void notch_dsp(t_notch *x, t_signal **sp, short *count);
void notch_assist(t_notch *x, void *b, long m, long a, char *s);
void notch_free(t_notch *x);
void dsp_initclass(void);

void main()
{
	setup((t_messlist **)&notch_class, (method)notch_new,(method)notch_free, (short)sizeof(t_notch), 0L, A_DEFFLOAT, A_DEFFLOAT, 0);
    addint((method)notch_int);
    addfloat((method)notch_float);
    addmess((method)notch_dsp, "dsp", A_CANT, 0);
	addmess((method)notch_assist,"assist",A_CANT,0);
	finder_addclass("MSP Filters","notch~");
    dsp_initclass();
	rescopy('STR#',9591);
}

void notch_assist(t_notch *x, void *b, long m, long a, char *s)
{
	assist_string(ASSIST_RSRC,m,a,1,4,s);
}

void notch_int(t_notch *x, long n) {
	if (x->n_obj.z_in == 1)
		x->n_CF = (float)n;
	else if (x->n_obj.z_in == 2)
		x->n_BW = (float)n;
}

void notch_float(t_notch *x, float f) {
	if (x->n_obj.z_in == 1)
		x->n_CF = f;
	else if (x->n_obj.z_in == 2)
		x->n_BW = f;
}

void *notch_new(float CF, float BW)
{
	t_notch *x;
	
	x = (t_notch *)newobject(notch_class); // point to this instance of notch~
	dsp_setup((t_pxobject *)x,3); /* create 3 signal inlets */
    outlet_new((t_object *)x,"signal"); // create 1 signal outlet
    
	x->n_CF = CF;
	x->n_BW = BW;
	x->n_xnm1 = x->n_xnm2 = x->n_ynm1 = x->n_ynm2 = 0.0;
	
	return x;
}

void notch_free(t_notch *x) {
	dsp_free((t_pxobject *)x);
}

void notch_dsp(t_notch *x, t_signal **sp, short *count) {
	x->n_oneoversr = 1. / (double)sp[0]->s_sr;
	if (count[1] && count[2]) { // signals connected for both parameters
		dsp_add(notch_perform, 6, x, sp[0]->s_vec, sp[1]->s_vec, sp[2]->s_vec, sp[3]->s_vec, sp[0]->s_n);
	} else {
		if (count[2]) { // then !count[1], so no signal for CF
			dsp_add(notch_perform1, 5, x, sp[0]->s_vec, sp[2]->s_vec, sp[3]->s_vec, sp[0]->s_n);
		} else if (count[1]) { // then !count[2], so no signal for BW
			dsp_add(notch_perform2, 5, x, sp[0]->s_vec, sp[1]->s_vec, sp[3]->s_vec, sp[0]->s_n);
		} else { // no signal for either CF or BW, so use stored float values
			dsp_add(notch_perform3, 4, x, sp[0]->s_vec, sp[3]->s_vec, sp[0]->s_n);
		}
	}
}

t_int *notch_perform(t_int *w) { // if signals for both parameters; 6 arguments
	t_notch *x; // pointer to this object
	short n; // vector size (counter variable)
	float *in, *out; // pointers to input and output signals
	float *CF, *BW; // pointers to parameter signals
	float oneoversr; // reciprocal of the input signal sample rate
	float C, D; // intermediate values for calculating filter coefficients
	float a0, a1, a2, b1, b2; // filter coefficients
	float xn, xnm1, xnm2, yn, ynm1, ynm2; // current and past sample values
	x = (t_notch *)(w[1]); // point to the object
	in = (float *)(w[2]); // point to the input signal
	CF = (float *)(w[3]); // point to the center frequency parameter (signal)
	BW = (float *)(w[4]); // point to the bandwidth parameter (signal)
	out = (float *)(w[5]); // point to the output signal
	n = (w[6]); // this is the vector size
	oneoversr = x->n_oneoversr;
	// get previously stored sample values
	xnm1 = x->n_xnm1;
	xnm2 = x->n_xnm2;
	ynm1 = x->n_ynm1;
	ynm2 = x->n_ynm2;
	// actual filter calculation
	while (n--) { // for each sample in the vector...
		xn = *in++; // get current input sample
		// calculate intermediate values, based on signal parameter inputs
		C = tan(PI*(*BW++)*oneoversr);
		D = 2.0*cos(TWOPI*(*CF++)*oneoversr);
		// calculate filter coefficients for this sample
		a0 = a2 = 1.0/(1.0+C);
		a1 = b1 = -1.0*a0*D;
		b2 = a0*(1.0-C);
		yn = (a0*xn) + (a1*xnm1) + (a2*xnm2) - (b1*ynm1) - (b2*ynm2); // calculate current output sample
		*out++ = yn; // output it
		// store past sample values
		xnm2 = xnm1;
		xnm1 = xn;
		ynm2 = ynm1;
		ynm1 = yn;
	}
	// store all past sample values in object, for use in next perform pass
	x->n_xnm1 = xnm1;
	x->n_xnm2 = xnm2;
	x->n_ynm1 = ynm1;
	x->n_ynm2 = ynm2;
	
	return w+7; // return pointer just beyond arguments
}

t_int *notch_perform1(t_int *w) { // if no signals for CF parameter; 5 arguments
	t_notch *x; // pointer to this object
	short n; // vector size (counter variable)
	float *in, *out; // pointers to input and output signals
	float *BW; // pointer to BW parameter signals
	float oneoversr; // reciprocal of the input signal sample rate
	float C, D; // intermediate values for calculating filter coefficients
	float a0, a1, a2, b1, b2; // filter coefficients
	float xn, xnm1, xnm2, yn, ynm1, ynm2; // current and past sample values
	x = (t_notch *)(w[1]); // point to the object
	in = (float *)(w[2]); // point to the input signal
	BW = (float *)(w[3]); // point to the bandwidth parameter (signal)
	out = (float *)(w[4]); // point to the output signal
	n = (w[5]); // this is the vector size
	oneoversr = x->n_oneoversr;
	// get previously stored sample values
	xnm1 = x->n_xnm1;
	xnm2 = x->n_xnm2;
	ynm1 = x->n_ynm1;
	ynm2 = x->n_ynm2;
	D = 2.0*cos(TWOPI*(x->n_CF)*oneoversr); // calculate intermediate value, based on float input for CF
	// actual filter calculation
	while (n--) { // for each sample in the vector...
		xn = *in++; // get current input sample
		C = tan(PI*(*BW++)*oneoversr); // calculate intermediate value, based on signal parameter input for BW
		// calculate filter coefficients for this sample
		a0 = a2 = 1.0/(1.0+C);
		a1 = b1 = -1.0*a0*D;
		b2 = a0*(1.0-C);
		yn = (a0*xn) + (a1*xnm1) + (a2*xnm2) - (b1*ynm1) - (b2*ynm2); // calculate current output sample
		*out++ = yn; // output it
		// store past sample values
		xnm2 = xnm1;
		xnm1 = xn;
		ynm2 = ynm1;
		ynm1 = yn;
	}
	// store all past sample values in object, for use in next perform pass
	x->n_xnm1 = xnm1;
	x->n_xnm2 = xnm2;
	x->n_ynm1 = ynm1;
	x->n_ynm2 = ynm2;
	
	return w+6; // return pointer just beyond arguments
}

t_int *notch_perform2(t_int *w) { // if no signal for BW parameter; 5 arguments
	t_notch *x; // pointer to this object
	short n; // vector size (counter variable)
	float *in, *out; // pointers to input and output signals
	float *CF; // pointer to CF parameter signal
	float oneoversr; // reciprocal of the input signal sample rate
	float C, D; // intermediate values for calculating filter coefficients
	float a0, a1, a2, b1, b2; // filter coefficients
	float xn, xnm1, xnm2, yn, ynm1, ynm2; // current and past sample values
	x = (t_notch *)(w[1]); // point to the object
	in = (float *)(w[2]); // point to the input signal
	CF = (float *)(w[3]); // point to the center frequency parameter (signal)
	out = (float *)(w[4]); // point to the output signal
	n = (w[5]); // this is the vector size
	oneoversr = x->n_oneoversr;
	// get previously stored sample values
	xnm1 = x->n_xnm1;
	xnm2 = x->n_xnm2;
	ynm1 = x->n_ynm1;
	ynm2 = x->n_ynm2;
	C = tan(PI*(x->n_BW)*oneoversr); // calculate intermediate value, based on float parameter input for BW
	a0 = a2 = 1.0/(1.0+C); // calculate these filter coefficients for the entire vector
	b2 = a0*(1.0-C);  // then calculate this filter coefficient for the entire vector
	// actual filter calculation
	while (n--) { // for each sample in the vector...
		xn = *in++; // get current input sample
		// calculate this intermediate value, based on signal parameter input for center frequency
		D = 2.0*cos(TWOPI*(*CF++)*oneoversr);
		a1 = b1 = -1.0*a0*D; // calculate this filter coefficient for this sample
		yn = (a0*xn) + (a1*xnm1) + (a2*xnm2) - (b1*ynm1) - (b2*ynm2); // calculate current output sample
		*out++ = yn; // output it
		// store past sample values
		xnm2 = xnm1;
		xnm1 = xn;
		ynm2 = ynm1;
		ynm1 = yn;
	}
	// store all past sample values in object, for use in next perform pass
	x->n_xnm1 = xnm1;
	x->n_xnm2 = xnm2;
	x->n_ynm1 = ynm1;
	x->n_ynm2 = ynm2;
	
	return w+6; // return pointer just beyond arguments
}

t_int *notch_perform3(t_int *w) { // if no signals for parameters; 4 arguments
	t_notch *x; // pointer to this object
	short n; // vector size (counter variable)
	float *in, *out; // pointers to input and output signals
	float C, D; // intermediate values for calculating filter coefficients
	float a0, a1, a2, b1, b2; // filter coefficients
	float xn, xnm1, xnm2, yn, ynm1, ynm2; // current and past sample values
	x = (t_notch *)(w[1]); // point to the object
	in = (float *)(w[2]); // point to the input signal
	out = (float *)(w[3]); // point to the output signal
	n = (w[4]); // this is the vector size
	// calculate intermediate values, based on float parameter inputs
	C = tan(PI*(x->n_BW)*(x->n_oneoversr));
	D = 2.0*cos(TWOPI*(x->n_CF)*(x->n_oneoversr));
	// calculate filter coefficients for the entire vector
	a0 = a2 = 1.0/(1.0+C);
	a1 = b1 = -1.0*a0*D;
	b2 = a0*(1.0-C);
	// get previously stored sample values
	xnm1 = x->n_xnm1;
	xnm2 = x->n_xnm2;
	ynm1 = x->n_ynm1;
	ynm2 = x->n_ynm2;
	// actual filter calculation
	while (n--) { // for each sample in the vector...
		xn = *in++; // get current input sample
		yn = (a0*xn) + (a1*xnm1) + (a2*xnm2) - (b1*ynm1) - (b2*ynm2); // calculate current output sample
		*out++ = yn; // output it
		// store past sample values
		xnm2 = xnm1;
		xnm1 = xn;
		ynm2 = ynm1;
		ynm1 = yn;
	}
	// store all past sample values in object, for use in next perform pass
	x->n_xnm1 = xnm1;
	x->n_xnm2 = xnm2;
	x->n_ynm1 = ynm1;
	x->n_ynm2 = ynm2;
	
	return w+5; // return pointer just beyond arguments
}