#include <u.h>
#include <libc.h>
#include <thread.h>
#include <ctype.h>
#include "abuf.h"
#include "guic.h"

typedef struct Biquad Biquad;

enum{
	Gexit,
	Gbw,
	Ga,
	Gfreq,
};

char *keywords[] = {
	"exit",
	"bw",
	"a",
	"freq",
	0,
};

struct Biquad {
	double b0, b1, b2;
	double a1, a2;
};


void
peak(Biquad *q, double w0, double Q, double A)
{
	double α;
	double cosw0, a0;
	
	α = sin(w0)/(2*Q);
	cosw0 = cos(w0);
	a0 = 1 + α/A;
	
	q->b0 = (1 + α*A) / a0;
	q->b1 = -2*cosw0 / a0;
	q->b2 = (1 - α*A) / a0;
	q->a1 = -2*cosw0 / a0;
	q->a2 = (1 - α/A) / a0;
}

double
response(Biquad *q, double w)
{
	double s1w, s2w, c1w, c2w;
	double nre, nim, dre, dim;
	s1w = sin(w);
	s2w = sin(2*w);
	c1w = cos(w);
	c2w = cos(2*w);
	nre = q->b2 + q->b1 * c1w + q->b0 * c2w;
	nim = q->b1 * s1w + q->b0 * s2w;
	dre = q->a2 + q->a1 * c1w + c2w;
	dim = q->a1 * s1w + s2w;
	return (nre*nre + nim*nim)/(dre*dre + dim*dim);
}

double
db(double x)
{
	return 10*log(x)/log(10);
}

void
drawbq(Biquad *q)
{
	double x, dx;
	
	dx = PI/1024;
	for(x=0; x<PI; x+=dx)
		print("%f %f\n", x, db(response(q, x)));
	print("response\n");
}

enum {
	GraphW = 500,
	GraphH = 200,
};

void
graphbq(Biquad *q, char *s)
{
	double g, x;
	int i, y;
	
	for(i=0; i<GraphW; i++){
		x = exp(log(22050/32)*(double)i/GraphW) * 32;
		x = x * 2 * PI / SRATE;
		//fprint(2, "i=%d, x=%f\n", i, x);
		g = db(response(q, x)) / 72 + 0.5;
		y = GraphH - g*GraphH;
		s = s + sprint(s, "%d,", y);
	}
}

void
printbq(Biquad *q)
{
	print("%f %f %f / 1 %f %f\n",
		q->b0, q->b1, q->b2, q->a1, q->a2);
}

char buf[8192];

double
expctlr(double x, double range)
{
	return exp(x * log(range));
}

void
threadmain(int argc, char *argv[])
{
	Biquad bq;
	Event ev;
	double q, a, w, bw;
	
	if(argc < 2)
		sysfatal("args");

 	guistart(argv[1]);
	guiwrite("graph freqresp %d %d\nnl\n", GraphW, GraphH);
	guiwrite("slider bw\ntext _ bw\nnl\n");
	guiwrite("slider a\ntext _ a\nnl\n");
	guiwrite("slider freq\ntext _ freq\nnl\n");
	guiwrite("button exit\nnl\n");

	a = q = 1;
	w = PI / 4;
	while(recv(guichan, &ev) == 1){
		switch(ev.k){
		case Gexit:
			threadexitsall(0);
		case Gbw:
			/* maximal bandwidth is 2 octaves */
			bw = (ev.n + 1) / 64.0;
			/* compute q from bandwidth */
			q = 1 / (2*sinh(bw * log(2) / 2));
			break;
		case Ga:
			/* peak is A^4, for 24dB = 2^8 we need 2^2 range */
			a = expctlr(ev.n / 63.0 - 1.0, 8);
			break;
		case Gfreq:
			w = exp(log(22050/32)*ev.n/128.0) * 32;
			fprint(2, "freq=%f\n", w);
			w = w * 2 * PI / SRATE;
			break;
		}
		peak(&bq, w, q, a);
		graphbq(&bq, buf);
		guiwrite("set freqresp %s\n", buf);
	}
}