/* via http://nbviewer.ipython.org/urls/music-synthesizer-for-android.googlecode.com/git/lab/Second%20order%20sections%20in%20matrix%20form.ipynb */

typedef struct {
	float A0[2], A1[2], B[2], C[3], memory[2];
} svf;

typedef struct {
	double A0[2], A1[2], B[2], C[3];
} svf_interim;

typedef struct {
	v4sf a, b, c, d, memory;
} svf_matrix;

static svf_interim
svf_design(double w0, double Q, double c0, double c1, double c2, double gc)
{
	svf_interim svfi;
	double g = tan(0.5*w0)*gc;
	double a1 = 1/(1 + g*(g + 1/Q));
	double a2 = g*a1;
	double a3 = g*a2;
	svfi.A0[0] = 2*a1 - 1;
	svfi.A0[1] = -2*a2;
	svfi.A1[0] = 2*a2;
	svfi.A1[1] = 1 - 2*a3;
	svfi.B[0] = 2*a2;
	svfi.B[1] = 2*a3;
	double v0[3] = {1, 0, 0};
	double v1[3] = {a2, a1, -a2};
	double v2[3] = {a3, a2, 1 - a3};
	svfi.C[0] = v0[0]*c0 + v1[0]*c1 + v2[0]*c2;
	svfi.C[1] = v0[1]*c0 + v1[1]*c1 + v2[1]*c2;
	svfi.C[2] = v0[2]*c0 + v1[2]*c1 + v2[2]*c2;
	return svfi;
}

static svf
svf_gen_raw(filter_t type, double w0, double A, double Q)
{
	double As = sqrt(A);

	svf_interim svfi;

	#define d(Q,c0,c1,c2,gc) svfi = svf_design(w0,Q,c0,c1,c2,gc)
	switch (type) {
	case FILT_PEAKING:    d(Q*A, 1, (A*A - 1)/A/Q, 0, 1);        break;
	case FILT_LOWSHELF:   d(Q, 1,     (A - 1)/Q, A*A - 1, 1/As); break;
	case FILT_HIGHSHELF:  d(Q, A*A, (1 - A)*A/Q, 1 - A*A,   As); break;
	case FILT_LOWPASS:    d(Q, 0, 0, 1, 1);                      break;
	case FILT_HIGHPASS:   d(Q, 1, -1/Q, -1, 1);                  break;
	case FILT_ALLPASS:    d(Q, 0, 0, 0, 1);                      break; // TODO: implement
	case FILT_BANDPASS:   d(Q, 0, 1, 0, 1);                      break; // FIXME: no gain
	case FILT_BANDPASS_2: d(Q, 0, 0, 0, 1);                      break; // TODO: implement
	case FILT_NOTCH:      d(Q, 1, -1/Q, 0, 1);                   break;
	case FILT_GAIN:       d(Q, 0, 0, 0, 1);                      break; // TODO: implement
	}
	#undef d

	svf s;
	s.A0[0] = svfi.A0[0];
	s.A0[1] = svfi.A0[1];
	s.A1[0] = svfi.A1[0];
	s.A1[1] = svfi.A1[1];
	s.B[0] = svfi.B[0];
	s.B[1] = svfi.B[1];
	s.C[0] = svfi.C[0];
	s.C[1] = svfi.C[1];
	s.C[2] = svfi.C[2];
	s.memory[0] = 0;
	s.memory[1] = 0;
	return s;
}

static svf
svf_gen(filter_t type, double fc, double gain, double bw, double fs)
{
	double w0 = ANGULAR_LIM(fc, fs);
	double A = DB2LIN(gain/2);
	//double Q = M_SQRT1_2/bw;
	double Q = M_SQRT1_2*(1 - (w0/M_PI)*(w0/M_PI))/bw;
	return svf_gen_raw(type, w0, A, Q);
}

INNER float
svf_run(svf *s, float x)
{
	float y = s->C[0]*x + s->C[1]*s->memory[0] + s->C[2]*s->memory[1];
	float temp = s->memory[0];
	s->memory[0] = s->B[0]*x + s->A0[0]*temp + s->A0[1]*s->memory[1];
	s->memory[1] = s->B[1]*x + s->A1[0]*temp + s->A1[1]*s->memory[1];
	return y;
}

static svf_matrix
svf_gen_matrix(svf s)
{
	// note: does not copy memory
	/*
	AA = dot(A, A);
	AB = dot(A, B);
	CA = dot(C[1:], A);
	cb = dot(C[1:], B);
	return [[ C[0],    0,     C[1],     C[2]],
	        [   cb, C[0],    CA[0],    CA[1]],
	        [AB[0], B[0], AA[0][0], AA[0][1]],
	        [AB[1], B[1], AA[1][0], AA[1][1]]];
	*/
	float AA0[2], AA1[2], AB[2], CA[2], cb;
	AA0[0] = s.A0[0]*s.A0[0] + s.A0[1]*s.A1[0];
	AA1[0] = s.A1[0]*s.A0[0] + s.A1[1]*s.A1[0];
	AA0[1] = s.A0[0]*s.A0[1] + s.A0[1]*s.A1[1];
	AA1[1] = s.A1[0]*s.A0[1] + s.A1[1]*s.A1[1];
	AB[0] = s.A0[0]*s.B[0] + s.A0[1]*s.B[1];
	AB[1] = s.A1[0]*s.B[0] + s.A1[1]*s.B[1];
	CA[0] = s.A0[0]*s.C[1] + s.A0[1]*s.C[2];
	CA[1] = s.A1[0]*s.C[1] + s.A1[1]*s.C[2];
	cb = s.C[1]*s.B[0] + s.C[2]*s.B[1];

	svf_matrix mat;
	mat.memory = (v4sf){0, 0, 0, 0};
	mat.a = (v4sf){s.C[0],       0, s.C[1], s.C[2]};
	mat.b = (v4sf){     cb, s.C[0],   CA[0],   CA[1]};
	mat.c = (v4sf){  AB[0], s.B[0],  AA0[0],  AA0[1]};
	mat.d = (v4sf){  AB[1], s.B[1],  AA1[0],  AA1[1]};
	return mat;
}