#include <stdlib.h>
#include <math.h>
#include <stdint.h>

#include "crap_util.h"

/* used to resemble https://github.com/swh/ladspa/blob/master/util/biquad.h */

void
biquad_init(biquad *bq) {
	bq->x1 = 0;
	bq->x2 = 0;
	bq->y1 = 0;
	bq->y2 = 0;
}

biquad_interim
design(double cw, double sw,
    double num0, double num1, double num2,
    double den0, double den1, double den2) {
	return (biquad_interim) {
		.b0 = num0*   (1 + cw) + num1*sw + num2*  (1 - cw),
		.b1 = num0*-2*(1 + cw)           + num2*2*(1 - cw),
		.b2 = num0*   (1 + cw) - num1*sw + num2*  (1 - cw),
		.a0 = den0*   (1 + cw) + den1*sw + den2*  (1 - cw),
		.a1 = den0*-2*(1 + cw)           + den2*2*(1 - cw),
		.a2 = den0*   (1 + cw) - den1*sw + den2*  (1 - cw),
	};
}

biquad
biquad_gen(int type, double fc, double gain, double bw, double fs) {
	/* TODO: use enum for type instead of just int */
	double w0, cw, sw, A, As, Q;
	w0 = ANGULAR_LIM(fc, fs);
	cw = cos(w0);
	sw = sin(w0);
	A = DB2LIN(gain/2);
	As = sqrt(A);
	Q = 1/(4 * sinh(LN_2_2 * bw * w0/sw));
	//Q = bw? - (w0/M_PI)^2;
	/* skip = (fabs(A - 1) <= TINY); */

	biquad_interim bqi;
	if (type == 0) bqi = design(cw,sw, 1,  A/Q, 1,   1,  1/A/Q,   1);
	if (type == 1) bqi = design(cw,sw, 1, As/Q, A,   1, 1/As/Q, 1/A);
	if (type == 2) bqi = design(cw,sw, A, As/Q, 1, 1/A, 1/As/Q,   1);
	if (type == 3) bqi = design(cw,sw, 0,    0, 1,   1,    1/Q,   1);
	if (type == 4) bqi = design(cw,sw, 1,    0, 0,   1,    1/Q,   1);
	if (type == 5) bqi = design(cw,sw, 1, -1/Q, 1,   1,    1/Q,   1);
	if (type == 6) bqi = design(cw,sw, 0,    1, 0,   1,    1/Q,   1);
	if (type == 7) bqi = design(cw,sw, 0,  1/Q, 0,   1,    1/Q,   1);
	if (type == 8) bqi = design(cw,sw, 1,    0, 1,   1,    1/Q,   1);
	if (type == 9) bqi = design(cw,sw, A,    A, A, 1/A,    1/A, 1/A);

	double a0r = 1/bqi.a0;

	return (biquad) {
		.b0 = a0r * bqi.b0,
		.b1 = a0r * bqi.b1,
		.b2 = a0r * bqi.b2,
		.a1 = -a0r * bqi.a1,
		.a2 = -a0r * bqi.a2
	};
}

bq_t
biquad_run(biquad *bq, bq_t x) {
	bq_t y;

	y = bq->b0 * x + bq->b1 * bq->x1 + bq->b2 * bq->x2
	               + bq->a1 * bq->y1 + bq->a2 * bq->y2;
	if (IS_DENORMAL(y)) y = 0;
	bq->x2 = bq->x1;
	bq->x1 = x;
	bq->y2 = bq->y1;
	bq->y1 = y;

	return y;
}