// Chebyshev and Butterworth filters
// Version 2.5, 1.8.2003
// Copyright 1998-2003, Calerga.
//
//   Disclaimer
//
// Absolutely no guarantee is made about the content of this file, the
// underlying algorithms, their suitability with respect to any particular
// application, or the functionning of the code with any software. The user
// supports the whole responsibility of the reading and the use of this file
// and the use of the results which might be obtained with it.
//
// Permission is granted to licensees of SQ to use, modify, and distribute
// modified versions of this file or new files which borrow some code from
// it to other licensees of SQ, provided that its original author (Yves Piguet)
// is acknowledged and that it is made clear that the new file is a derivated
// work. The licensee assumes all the legal consequences which might result
// from the use and the distribution of the derivative work.
//
// The "data" block at the end of this file, if there is one, was added
// after the release of this file and is even more outside of the scope of
// the author's responsibility.

// Ref: Parks, T.W. and C.S. Burrus, "Digital Filter Design", John Wiley & Sons,
//  New York, 1987

title "Filtros"

version
{@
Version 2.5, 1st August 2003

Copyright 1998-2003, Calerga Sarl.
@}

help
{@
Different filters, defined in continuous time or discrete time. Low-pass,
high-pass, band-pass and band-stop can be adjusted interactively in the
frequency magnitude diagram.

A two-by-two array of figures is displayed. The top row represents a
continuous-time filter, while the bottom row represents the same filter
converted with the bilinear transform. The left column shows the magnitude
of the frequency response of the filter; the right column shows the zeros
(as circles) and the poles (as crosses) of the filters in the complex plane
of the Laplace transform for the continuous-time filter and of the z
transform for the discrete-time filter. Initially, the filter is a
Chebyshev filter, whose bandwidth (vertical blue line) and bandwidth
ripples (horizontal red line) can be manipulated.
@}

variable wl			// lower limit of the passband (0 for a lowpass)
variable wh			// upper limit of the passband (inf for a highpass)
variable rp			// lower limit of the passband ripples (1 means butterworth or inv cheb)
variable rs			// upper limit of the stopband ripples (0 means butterworth or cheb)
variable n			// filter order (or half order for bandpass)
variable num den	// continuous-time tf of the filter
variable Ts			// sampling time

init (wl, wh, rp, rs, n, Ts) = init

make (num, den) = calcFilter(n, rp, rs, wl, wh)

export "Filter" (_xdata, _xdatatype) = exportFilter(num, den)

menu "Butterworth Filter" _checkmark(rp==1&&rs==0) (rp, rs) = butterFilter
menu "Chebyshev Filter" _checkmark(rp<1&&rs==0) (rp, rs) = chebFilter
menu "Inv. Chebyshev Filter" _checkmark(rp==1&&rs>0) (rp, rs) = cheb2Filter
separator
menu "Lowpass Filter" _checkmark(wl==0) (wl, wh) = lowpass(wl, wh)
menu "Highpass Filter" _checkmark(isinf(wh)) (wl, wh) = highpass(wl, wh)
menu "Bandpass Filter" _checkmark(wl>0&&~isinf(wh)&&wl<wh) (wl, wh) = bandpass(wl, wh)
menu "Bandstop Filter" _checkmark(wl>0&&~isinf(wh)&&wl>wh) (wl, wh) = bandstop(wl, wh)
separator
menu "Filter Order..." n = setOrder(n)
menu "Transition Frequencies..." (wl, wh) = setTransFreq(wl, wh)
menu "Sampling Time..." Ts = setSamplingTime(Ts)

figure "Frequency Response Magnitude"
	draw drawMag(num, den, wl, wh, rp, rs)
	mousedrag (wl, wh, rp, rs, _msg) = dragMag(wl, wh, rp, rs, n, Ts, _id, _x1, _y1, _m)
	mouseover (_msg, _cursor) = overMag(wl, wh, _id, _x0, _y0)
figure "Frequency Response Phase"
	draw drawPhase(num, den)
figure "Poles"
	draw drawPoles(num, den)
separator
figure "Frequency Response Magnitude (discrete-time)"
	draw drawDMag(num, den, wl, wh, rp, rs, Ts)
	mousedrag (wl, wh, rp, rs, _msg) = dragDMag(wl, wh, rp, rs, n, Ts, _id, _x1, _y1)
	mouseover (_msg, _cursor) = overDMag(wl, wh, Ts, _id, _x0, _y0)
figure "Frequency Response Phase (discrete-time)"
	draw drawDPhase(num, den, Ts)
figure "Poles (discrete-time)"
	draw drawDPoles(num, den, Ts)

functions
{@
function (wl, wh, rp, rs, n, Ts) = init
	
	wl = 0;
	wh = 2;
	n = 6;
	rp = 1;
	rs = 0;
	Ts = 1;
	subplots('Frequency Response Magnitude\tPoles');
	subplotprops([0,-0.4097, 20.0761, -2.0795e-2, 1.0190
		0, -5, 5, -5, 5]);		% lock continuous-time figures

function (xdata, xdatatype) = exportFilter(num, den)
	
	xdata.num = num;
	xdata.den = den;
	xdatatype = 'tf s';

function (rp, rs) = butterFilter
	
	rp = 1;
	rs = 0;

function (rp, rs) = chebFilter
	
	rp = 0.8;
	rs = 0;

function (rp, rs) = cheb2Filter
	
	rp = 1;
	rs = 0.2;

function n = setOrder(n)
	
	n = dialog('Filter Order:', n);

function (wl, wh) = setTransFreq(wl, wh)
	
	while true
		w = dialog('Transition frequencies:', [wl, wh]);
		if any(w < 0)
			dialog('Frequencies must be nonnegative.');
		else
			if length(w) ~= 2
				w = [0, w];
			end
			wl = w(1);
			wh = w(2);
			break;
		end
	end

function Ts = setSamplingTime(Ts)
	
	Ts = dialog('Sampling Time:', Ts);

function (wl, wh) = lowpass(wl, wh)
	if isinf(wh)
		wh = wl;
	end
	wl = 0;

function (wl, wh) = highpass(wl, wh)
	if wl == 0
		wl = wh;
	end
	wh = inf;

function (wl, wh) = bandpass(wl, wh)
	if wl == 0
		wl = wh / 2;
	elseif isinf(wh)
		wh = wl * 2;
	elseif wl > wh
		tmp = wl;
		wl = wh;
		wh = tmp;
	end

function (wl, wh) = bandstop(wl, wh)
	if wl == 0
		wl = wh * 2;
	elseif isinf(wh)
		wh = wl / 2;
	elseif wl < wh
		tmp = wl;
		wl = wh;
		wh = tmp;
	end

function drawMag(num, den, wl, wh, rp, rs)
	
	scale('linlin/logdb');
	bodemag(num, den);
	line([0, 1], sqrt(2)/2, 'g-');
	text(20, sqrt(2)/2+0.01, '0,7071', 'rb');
	if wl > 0
		line([1,0], wl, 'b', 1);	% lower bandpass limit
	end
	if ~isinf(wh)
		line([1,0], wh, 'b', 2);	% higher bandpass limit
	end
	line([0,1], 1, 'k');
	if rp ~= 1
		line([0,1], rp, 'r', 3);	% lower bandpass ripple limit
	end
	if rs ~= 0
		line([0,1], rs, 'r', 4);	% higher bandstop ripple limit
	end

function (wl, wh, rp, rs, msg) = dragMag(wl0, wh0, rp0, rs0, n, Ts, id, x1, y1, m)
	
	if isempty(id)
		cancel;
	end
	wl = wl0;
	wh = wh0;
	rp = rp0;
	rs = rs0;
	msg = overMag(wl, wh, id, x1, y1);
	switch id
		case 1
			if x1 > 0 && (x1 < 0.95 * wh || x1 > wh * 1.05)
				if m
					wh = wh * x1 / wl;
				end
				wl = x1;
			end
		case 2
			if x1 > 0 && (x1 > 1.05 * wl || x1 < wl * 0.95)
				if m
					wl = wl * x1 / wh;
				end
				wh = x1;
			end
		case 3
			if y1 > 0.99
				rp = 0.99;
			elseif y1 < 0.05
				rp = 0.05;
			else
				rp = y1;
			end
		case 4
			if y1 < 0.01
				rs = 0.01;
			elseif y1 > 0.95
				rs = 0.95;
			else
				rs = y1;
			end
	end

function (msg, cursor) = overMag(wl, wh, id, x, y)
	
	if isempty(id)
		cancel;
	end
	msg = '';
	switch id
		case 1
			if x > 0 && (x < 0.95 * wh || x > wh * 1.05)
				msg = sprintf('w = %.2g', x);
			end
		case 2
			if x > 0 && (x > 1.05 * wl || x < wl * 0.95)
				msg = sprintf('w = %.2g', x);
			end
		case 3
			if y > 0.99
				y = 0.99;
			elseif y < 0.05
				y = 0.05;
			end
			msg = sprintf('Ripple lower bound = %.2g', y);
		case 4
			if y < 0.01
				y = 0.01;
			elseif y > 0.95
				y = 0.95;
			end
			msg = sprintf('Ripple upper bound = %.2g', y);
	end
	cursor = true;

function drawDMag(num, den, wl, wh, rp, rs, Ts)
	
	scale('linlin/logdb',[0,pi/Ts,0,1]);
	(num, den) = c2dm(num, den, Ts, 'tustin');
	dbodemag(num, den, Ts);
	if wl > 0
		line([1,0], 2*atan(Ts*wl/2)/Ts, 'b', 1);	% lower bandpass limit
	end
	if ~isinf(wh)
		line([1,0], 2*atan(Ts*wh/2)/Ts, 'b', 2);	% higher bandpass limit
	end
	line([0,1], 1, 'k');
	if rp ~= 1
		line([0,1], rp, 'r', 3);	% lower bandpass ripple limit
	end
	if rs ~= 0
		line([0,1], rs, 'r', 4);	% higher bandstop ripple limit
	end

function (wl, wh, rp, rs, msg) = dragDMag(wl0, wh0, rp0, rs0, n, Ts, id, x1, y1)
	
	wl = wl0;
	wh = wh0;
	rp = rp0;
	rs = rs0;
	msg = overDMag(wl, wh, Ts, id, x1, y1);
	switch id
		case 1
			if x1 < 0.95 * 2*atan(Ts*wh/2) | x1 > 2*atan(Ts*wh/2) * 1.05
				wl = 2*tan(Ts*x1/2)/Ts;
			end
		case 2
			if x1 > 1.05 * 2*atan(Ts*wl/2) | x1 < 2*atan(Ts*wl/2) * 0.95
				wh = 2*tan(Ts*x1/2)/Ts;
			end
		case 3
			if y1 > 0.99
				rp = 0.99;
			elseif y1 < 0.05
				rp = 0.05;
			else
				rp = y1;
			end
		case 4
			if y1 < 0.01
				rs = 0.01;
			elseif y1 > 0.95
				rs = 0.95;
			else
				rs = y1;
			end
	end

function (msg, cursor) = overDMag(wl, wh, Ts, id, x, y)
	
	if isempty(id)
		cancel;
	end
	msg = '';
	switch id
		case 1
			if x < 0.95 * 2*atan(Ts*wh/2) | x > 2*atan(Ts*wh/2) * 1.05
				msg = sprintf('w = %.2g', x);
			end
		case 2
			if x > 1.05 * 2*atan(Ts*wl/2) | x < 2*atan(Ts*wl/2) * 0.95
				msg = sprintf('w = %.2g', x);
			end
		case 3
			if y > 0.99
				y = 0.99;
			elseif y < 0.05
				y = 0.05;
			end
			msg = sprintf('Ripple lower bound = %.2g', y);
		case 4
			if y < 0.01
				y = 0.01;
			elseif y > 0.95
				y = 0.95;
			end
			msg = sprintf('Ripple upper bound = %.2g', y);
	end
	cursor = true;

function drawPhase(num, den)
	
	scale('linlin/loglin');
	bodephase(num, den);

function drawDPhase(num, den, Ts)
	
	scale('linlin/loglin', [0,pi/Ts]);
	dbodephase(num, den, Ts);

function drawPoles(num, den)
	
	scale('equal');
	sgrid;
	plotroots(num, 'o');
	plotroots(den, 'x');

function drawDPoles(num, den, Ts)
	
	(numd, dend) = c2dm(num, den, Ts, 'tustin');
	scale('equal', [-1,1,-1,1]);
	zgrid;
	plotroots(dend, 'x');
	if length(num) == 1
		plotroots([1, 1], 'o');	% multiple zeros at -1 (avoid potential numerical problems)
	elseif all(num(2:end)==0)
		plotroots([1, -1], 'o');	% multiple zeros at 1
	else
		plotroots(numd, 'o');
	end

function (num, den) = calcFilter(n, rp, rs, wl, wh)
	
	if rp == 1
		if rs == 0
			(b, a) = butterworth(n);
		else
			(b, a) = chebyshev2(n, rs);
		end
	else
		(b, a) = chebyshev(n, rp);
	end
	(num, den) = changeFreq(b, a, wl, wh);

function (num, den) = butterworth(n)
	
	den = real(poly(exp(1j*pi*(n+1:2:3*n-1)/(2*n))));
	num = den(end);

function (num, den) = chebyshev(n, rp)
	
	e = sqrt(1/rp^2-1);
	a = asinh(1/e)/n;
	psi = pi * (n+1:2:3*n-1) / (2 * n);
	den = real(poly(sinh(a)*cos(psi)+1j*cosh(a)*sin(psi)));
	if mod(n,2)
		num = den(end);
	else
		num = den(end) * rp;
	end

function (num, den) = chebyshev2(n, rs)
	
	e = rs/sqrt(1-rs^2);
	a = asinh(1/e)/n;
	if mod(n, 2) == 0
		num = real(poly(1j./sin(pi*(-n+1:2:n-1)/(2 * n))));
	else
		num = real(poly(1j./sin(pi*([-n+1:2:-2,2:2:n-1])/(2 * n))));
	end	
	psi = pi * (n+1:2:3*n-1) / (2 * n);
	den = real(poly(1./(sinh(a)*cos(psi)+1j*cosh(a)*sin(psi))));
	num = num * den(end) / num(end);

function (b, a) = changeFreq(b0, a0, wl, wh)
	% change lowpass filter b0/a0 with unit cutoff frequency to bandpass filter
	% between wl and wh, or lowpass if wl == 0, or highpass if wh == inf,
	% or bandstop if wl > wh
	
	if wl == 0			% lowpass
		b = b0 .* (1/wh).^(length(b0)-1:-1:0);
		a = a0 .* (1/wh).^(length(a0)-1:-1:0);
	elseif isinf(wh)	% highpass
		b = [fliplr(b0), zeros(1,length(a0)-length(b0))] .* (1/wl).^(length(a0)-1:-1:0);
		a = fliplr(a0) .* (1/wl).^(length(a0)-1:-1:0);
	elseif wh > wl		% bandpass
		(b, a) = substRat(b0, a0, [1, 0, wl*wh]/(wh-wl), [1, 0]);
	else				% bandstop
		(b, a) = substRat(b0, a0, [wl-wh, 0], [1, 0, wl*wh]);
	end

function (b, a) = substRat(b0, a0, ns, ds)
	% calc. b(w)/a(w) = b0(v)/a0(v) with v = ns(w)/ds(w),
	% where b, a, b0, a0, ns, and ds are polynomials
	
	n = length(a0);
	b0 = [zeros(1, n-length(b0)), b0];	% pad b0 with zeros
	
	b = b0(1);
	a = a0(1);
	k = 1;
	for i = 2:n
		k = conv(k, ds);
		b = addpol(conv(b, ns), b0(i)*k);
		a = addpol(conv(a, ns), a0(i)*k);
	end
	
	while b(1) == 0		% remove leading zeros
		b = b(2:end);
	end

@}