/* For information on usage and redistribution, and for a DISCLAIMER OF ALL
* WARRANTIES, see the file, "LICENSE.txt," in this distribution.

iemlib1 written by Thomas Musil, Copyright (c) IEM KUG Graz Austria 2000 - 2003 */

#ifdef NT
#pragma warning( disable : 4244 )
#pragma warning( disable : 4305 )
#endif


#include "m_pd.h"
#include "iemlib.h"
#include <math.h>
#include <stdio.h>
#include <string.h>


/* ---------- para_bp2~ - parametric bandpass 2. order ----------- */

typedef struct sigpara_bp2
{
	t_object x_obj;
	float	wn1;
	float	wn2;
	float	a0;
	float	a1;
	float	a2;
	float	b1;
	float	b2;
	float	sr;
	float	cur_f;
	float	cur_l;
	float	cur_a;
	float	cur_g;
	float	delta_f;
	float	delta_a;
	float	delta_g;
	float	end_f;
	float	end_a;
	float	end_g;
	float	ticks_per_interpol_time;
	float	rcp_ticks;
	float	interpol_time;
	int	  ticks;
	int	  counter_f;
	int	  counter_a;
	int	  counter_g;
	int	  event_mask;
	void	*x_debug_outlet;
	t_atom x_at[5];
	float	x_msi;
} t_sigpara_bp2;

t_class *sigpara_bp2_class;

static void sigpara_bp2_calc(t_sigpara_bp2 *x)
{
	float l, al, gal, l2, rcp;

	l = x->cur_l;
	l2 = l*l + 1.0;
	al = l*x->cur_a;
	gal = al*x->cur_g;
	rcp = 1.0f/(al + l2);
	x->a0 = rcp*(l2 + gal);
	x->a1 = rcp*2.0f*(2.0f - l2);
	x->a2 = rcp*(l2 - gal);
	x->b1 = -x->a1;
	x->b2 = rcp*(al - l2);
}

static void sigpara_bp2_dsp_tick(t_sigpara_bp2 *x)
{
	if(x->event_mask)
	{
		float discriminant;

		if(x->counter_f)
		{
			float l, si, co;

			if(x->counter_f <= 1)
			{
				x->cur_f = x->end_f;
				x->counter_f = 0;
				x->event_mask &= 6;/*set event_mask_bit 0 = 0*/
			}
			else
			{
				x->counter_f--;
				x->cur_f *= x->delta_f;
			}
			l = x->cur_f * x->sr;
			if(l < 1.0e-20f)
				x->cur_l = 1.0e20f;
			else if(l > 1.57079632f)
				x->cur_l = 0.0f;
			else
			{
				si = sin(l);
				co = cos(l);
				x->cur_l = co/si;
			}
		}
		if(x->counter_a)
		{
			if(x->counter_a <= 1)
			{
				x->cur_a = x->end_a;
				x->counter_a = 0;
				x->event_mask &= 5;/*set event_mask_bit 1 = 0*/
			}
			else
			{
				x->counter_a--;
				x->cur_a *= x->delta_a;
			}
		}
		if(x->counter_g)
		{
			if(x->counter_g <= 1)
			{
				x->cur_g = x->end_g;
				x->counter_g = 0;
				x->event_mask &= 3;/*set event_mask_bit 2 = 0*/
			}
			else
			{
				x->counter_g--;
				x->cur_g *= x->delta_g;
			}
		}

		sigpara_bp2_calc(x);

		/* stability check */
		
		discriminant = x->b1 * x->b1 + 4.0f * x->b2;
		if(x->b1 <= -1.9999996f)
			x->b1 = -1.9999996f;
		else if(x->b1 >= 1.9999996f)
			x->b1 = 1.9999996f;

		if(x->b2 <= -0.9999998f)
			x->b2 = -0.9999998f;
		else if(x->b2 >= 0.9999998f)
			x->b2 = 0.9999998f;

		if(discriminant >= 0.0f)
		{
			if(0.9999998f - x->b1 - x->b2 < 0.0f)
				x->b2 = 0.9999998f - x->b1;
			if(0.9999998f + x->b1 - x->b2 < 0.0f)
				x->b2 = 0.9999998f + x->b1;
		}
	}
}

static t_int *sigpara_bp2_perform(t_int *w)
{
	float *in = (float *)(w[1]);
	float *out = (float *)(w[2]);
	t_sigpara_bp2 *x = (t_sigpara_bp2 *)(w[3]);
	int i, n = (t_int)(w[4]);
	float wn0, wn1=x->wn1, wn2=x->wn2;
	float a0=x->a0, a1=x->a1, a2=x->a2;
	float b1=x->b1, b2=x->b2;

	sigpara_bp2_dsp_tick(x);
	for(i=0; i<n; i++)
	{
		wn0 = *in++ + b1*wn1 + b2*wn2;
		*out++ = a0*wn0 + a1*wn1 + a2*wn2;
		wn2 = wn1;
		wn1 = wn0;
	}
	/* NAN protect */
	if(PD_BADFLOAT(wn2))
		wn2 = 0.0f;
	if(PD_BADFLOAT(wn1))
		wn1 = 0.0f;

	x->wn1 = wn1;
	x->wn2 = wn2;
	return(w+5);
}
/*   yn0 = *out;
	 xn0 = *in;
	 *************
	 yn0 = a0*xn0 + a1*xn1 + a2*xn2 + b1*yn1 + b2*yn2;
	 yn2 = yn1;
	 yn1 = yn0;
	 xn2 = xn1;
	 xn1 = xn0;
	 *************************
	 y/x = (a0 + a1*z-1 + a2*z-2)/(1 - b1*z-1 - b2*z-2);*/

static t_int *sigpara_bp2_perf8(t_int *w)
{
	float *in = (float *)(w[1]);
	float *out = (float *)(w[2]);
	t_sigpara_bp2 *x = (t_sigpara_bp2 *)(w[3]);
	int i, n = (t_int)(w[4]);
	float wn[10];
	float a0=x->a0, a1=x->a1, a2=x->a2;
	float b1=x->b1, b2=x->b2;

	sigpara_bp2_dsp_tick(x);
	wn[0] = x->wn2;
	wn[1] = x->wn1;
	for(i=0; i<n; i+=8, in+=8, out+=8)
	{
		wn[2] = in[0] + b1*wn[1] + b2*wn[0];
		out[0] = a0*wn[2] + a1*wn[1] + a2*wn[0];
		wn[3] = in[1] + b1*wn[2] + b2*wn[1];
		out[1] = a0*wn[3] + a1*wn[2] + a2*wn[1];
		wn[4] = in[2] + b1*wn[3] + b2*wn[2];
		out[2] = a0*wn[4] + a1*wn[3] + a2*wn[2];
		wn[5] = in[3] + b1*wn[4] + b2*wn[3];
		out[3] = a0*wn[5] + a1*wn[4] + a2*wn[3];
		wn[6] = in[4] + b1*wn[5] + b2*wn[4];
		out[4] = a0*wn[6] + a1*wn[5] + a2*wn[4];
		wn[7] = in[5] + b1*wn[6] + b2*wn[5];
		out[5] = a0*wn[7] + a1*wn[6] + a2*wn[5];
		wn[8] = in[6] + b1*wn[7] + b2*wn[6];
		out[6] = a0*wn[8] + a1*wn[7] + a2*wn[6];
		wn[9] = in[7] + b1*wn[8] + b2*wn[7];
		out[7] = a0*wn[9] + a1*wn[8] + a2*wn[7];
		wn[0] = wn[8];
		wn[1] = wn[9];
	}
	/* NAN protect */
	if(PD_BADFLOAT(wn[0]))
		wn[0] = 0.0f;
	if(PD_BADFLOAT(wn[1]))
		wn[1] = 0.0f;

	x->wn1 = wn[1];
	x->wn2 = wn[0];
	return(w+5);
}

static void sigpara_bp2_ft4(t_sigpara_bp2 *x, t_floatarg t)
{
	int i = (int)((x->ticks_per_interpol_time)*t);

	x->interpol_time = t;
	if(i <= 0)
		i = 1;
	x->ticks = i;
	x->rcp_ticks = 1.0f / (float)i;
}

static void sigpara_bp2_ft3(t_sigpara_bp2 *x, t_floatarg l)
{
	float g = exp(0.11512925465 * l);

	if(g != x->cur_g)
	{
		x->end_g = g;
		x->counter_g = x->ticks;
		x->delta_g = exp(log(g/x->cur_g)*x->rcp_ticks);
		x->event_mask |= 4;/*set event_mask_bit 2 = 1*/
	}
}

static void sigpara_bp2_ft2(t_sigpara_bp2 *x, t_floatarg q)
{
	float a;

	if(q <= 0.0f)
		q = 0.000001f;
	a = 1.0f/q;
	if(a != x->cur_a)
	{
		x->end_a = a;
		x->counter_a = x->ticks;
		x->delta_a = exp(log(a/x->cur_a)*x->rcp_ticks);
		x->event_mask |= 2;/*set event_mask_bit 1 = 1*/
	}
}

static void sigpara_bp2_ft1(t_sigpara_bp2 *x, t_floatarg f)
{
	if(f <= 0.0f)
		f = 0.000001f;
	if(f != x->cur_f)
	{
		x->end_f = f;
		x->counter_f = x->ticks;
		x->delta_f = exp(log(f/x->cur_f)*x->rcp_ticks);
		x->event_mask |= 1;/*set event_mask_bit 0 = 1*/
	}
}

static void sigpara_bp2_print(t_sigpara_bp2 *x)
{
//	post("fb1 = %g, fb2 = %g, ff1 = %g, ff2 = %g, ff3 = %g", x->b1, x->b2, x->a0, x->a1, x->a2);
	x->x_at[0].a_w.w_float = x->b1;
	x->x_at[1].a_w.w_float = x->b2;
	x->x_at[2].a_w.w_float = x->a0;
	x->x_at[3].a_w.w_float = x->a1;
	x->x_at[4].a_w.w_float = x->a2;
	outlet_list(x->x_debug_outlet, &s_list, 5, x->x_at);	post("fb1 = %g, fb2 = %g, ff1 = %g, ff2 = %g, ff3 = %g", x->b1, x->b2, x->a0, x->a1, x->a2);
}

static void sigpara_bp2_dsp(t_sigpara_bp2 *x, t_signal **sp)
{
	float si, co, f;
	int i, n=(int)sp[0]->s_n;

	x->sr = 3.14159265358979323846f / (float)(sp[0]->s_sr);
	x->ticks_per_interpol_time = 0.001f * (float)(sp[0]->s_sr) / (float)n;
	i = (int)((x->ticks_per_interpol_time)*(x->interpol_time));
	if(i <= 0)
		i = 1;
	x->ticks = i;
	x->rcp_ticks = 1.0f / (float)i;
	f = x->cur_f * x->sr;
	if(f < 1.0e-20f)
		x->cur_l = 1.0e20f;
	else if(f > 1.57079632f)
		x->cur_l = 0.0f;
	else
	{
		si = sin(f);
		co = cos(f);
		x->cur_l = co/si;
	}
	if(n&7)
		dsp_add(sigpara_bp2_perform, 4, sp[0]->s_vec, sp[1]->s_vec, x, n);
	else
		dsp_add(sigpara_bp2_perf8, 4, sp[0]->s_vec, sp[1]->s_vec, x, n);
}

static void *sigpara_bp2_new(t_symbol *s, int argc, t_atom *argv)
{
	t_sigpara_bp2 *x = (t_sigpara_bp2 *)pd_new(sigpara_bp2_class);
	int i;
	float si, co, f=0.0, q=1.0, l=0.0, interpol=0.0;

	inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_float, gensym("ft1"));
	inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_float, gensym("ft2"));
	inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_float, gensym("ft3"));
	inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_float, gensym("ft4"));
	outlet_new(&x->x_obj, &s_signal);
	x->x_debug_outlet = outlet_new(&x->x_obj, &s_list);
	x->x_msi = 0;

	x->x_at[0].a_type = A_FLOAT;
	x->x_at[1].a_type = A_FLOAT;
	x->x_at[2].a_type = A_FLOAT;
	x->x_at[3].a_type = A_FLOAT;
	x->x_at[4].a_type = A_FLOAT;

	x->event_mask = 1;
	x->counter_f = 1;
	x->counter_a = 0;
	x->counter_g = 0;
	x->delta_f = 0.0f;
	x->delta_a = 0.0f;
	x->delta_g = 0.0f;
	x->interpol_time = 500.0f;
	x->wn1 = 0.0;
	x->wn2 = 0.0;
	x->a0 = 0.0;
	x->a1 = 0.0;
	x->a2 = 0.0;
	x->b1 = 0.0;
	x->b2 = 0.0;
	x->sr = 3.14159265358979323846f / 44100.0f;
	x->cur_a = 1.0;
	if((argc == 4)&&IS_A_FLOAT(argv,3)&&IS_A_FLOAT(argv,2)&&IS_A_FLOAT(argv,1)&&IS_A_FLOAT(argv,0))
	{
		f = (float)atom_getfloatarg(0, argc, argv);
		q = (float)atom_getfloatarg(1, argc, argv);
		l = (float)atom_getfloatarg(2, argc, argv);
		interpol = (float)atom_getfloatarg(3, argc, argv);
	}
	if(f <= 0.0f)
		f = 0.000001f;
	x->cur_f = f;
	f *= x->sr;
	if(f < 1.0e-20f)
		x->cur_l = 1.0e20f;
	else if(f > 1.57079632f)
		x->cur_l = 0.0f;
	else
	{
		si = sin(f);
		co = cos(f);
		x->cur_l = co/si;
	}
	if(q <= 0.0f)
		q = 0.000001f;
	x->cur_a = 1.0f/q;
	x->cur_g = exp(0.11512925465 * l);
	if(interpol <= 0.0f)
		interpol = 0.0f;
	x->interpol_time = interpol;
	x->ticks_per_interpol_time = 0.5f;
	i = (int)((x->ticks_per_interpol_time)*(x->interpol_time));
	if(i <= 0)
		i = 1;
	x->ticks = i;
	x->rcp_ticks = 1.0f / (float)i;
	x->end_f = x->cur_f;
	x->end_a = x->cur_a;
	x->end_g = x->cur_g;
	return(x);
}

void sigpara_bp2_setup(void)
{
	sigpara_bp2_class = class_new(gensym("para_bp2~"), (t_newmethod)sigpara_bp2_new,
				0, sizeof(t_sigpara_bp2), 0, A_GIMME, 0);
	CLASS_MAINSIGNALIN(sigpara_bp2_class, t_sigpara_bp2, x_msi);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_dsp, gensym("dsp"), 0);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_ft1, gensym("ft1"), A_FLOAT, 0);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_ft2, gensym("ft2"), A_FLOAT, 0);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_ft3, gensym("ft3"), A_FLOAT, 0);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_ft4, gensym("ft4"), A_FLOAT, 0);
	class_addmethod(sigpara_bp2_class, (t_method)sigpara_bp2_print, gensym("print"), 0);
	class_sethelpsymbol(sigpara_bp2_class, gensym("iemhelp/help-para_bp2~"));
}