/* -*- MaTX -*-
 *  
 *  NAME
 *      bode_ss()      - Bode response of cont-time linear ss. systems
 *      bode_tf()      - Bode response of SISO cont-time linear systems
 *      bode_tfn()     - Bode response of SISO cont-time linear systems
 *      bode_tfm()     - Bode response of MIMO cont-time linear systems
 *
 *      bode_plot_ss()  -  Plot Bode response of cont-time linear system
 *      bode_plot_tf()  -  Plot the bode response of a transfer function
 *      bode_plot_tfn() -  Plot the bode response of a transfer function
 *      bode_plot_tfm() -  Plot Bode response of MIMO C.T. linear system
 *
 *  SYNOPSIS
 *      {Mag,Phase,w} = bode_ss(A,B,C,D)
 *         Array Mag,Phase;
 *         Arrwy w;
 *         Matrix A,B,C,D;
 *
 *      {Mag,Phase,w} = bode_ss(A,B,C,D,iu)
 *         Array Mag,Phase;
 *         Arrwy w;
 *         Matrix A,B,C,D;
 *         Integer iu;
 *
 *      {Mag,Phase,w} = bode_ss(A,B,C,D,iu,w)
 *         Array Mag,Phase;
 *         Arrwy w;
 *         Matrix A,B,C,D;
 *         Integer iu;
 *
 *      {mag,phase,w} = bode_tf(num,den)
 *         Array mag,phase;
 *         Array w;
 *         Matrix num, den;
 *         Rational G;
 *
 *      {mag,phase,w} = bode_tf(num,den,w)
 *         Array mag,phase;
 *         Array w;
 *         Matrix num,den;
 *         Rational G;
 *
 *      {mag,phase,w} = bode_tfn(g)
 *         Array mag,phase;
 *         Arrwy w;
 *         Rational g;
 *
 *      {mag,phase,w} = bode_tfn(g,w)
 *         Array mag,phase;
 *         Array w;
 *         Rational g;
 *
 *      {mag,phase,w} = bode_tfm(G)
 *         Array mag, phase;
 *         Array w;
 *         RaMatrix G;
 *
 *      {mag,phase,w} = bode_tfm(G,w)
 *         Array mag, phase;
 *         Array w;
 *         RaMatrix G;
 *
 *      bode_plot_ss(A,B,C,D)
 *         Matrix A,B,C,D;
 *
 *      bode_plot_ss(A,B,C,D,iu)
 *         Matrix A,B,C,D;
 *         Integer iu;
 *
 *      bode_plot_ss(A,B,C,D,iu,w)
 *         Matrix A,B,C,D;
 *         Integer iu;
 *         Array w;
 *
 *      bode_plot_tf(num,den)
 *         Matrix num, den;
 *
 *      bode_plot_tf(num,den,w)
 *         Matrix num,den;
 *         Array w;
 *
 *      bode_plot_tfn(g)
 *         Rational g;
 *
 *      bode_plot_tfn(g,w)
 *         Rational g;
 *         Array w;
 *
 *      bode_plot_tfm(G)
 *         RaMatrix G;
 *
 *      bode_plot_tfm(G,w)
 *         RaMatrix G;
 *         Array w;
 *
 *  DESCRIPTION
 *      bode_ss(A,B,C,D,iu,w) returns the frequency response of the system:
 *         .
 *         x = Ax + Bu
 *         y = Cx + Du
 *
 *      from the iu'th input.  w must contain the frequencies (in radians/s)
 *      at which the transfer function is to be evaluated.
 *
 *      bode_ss() returns Mag (absolute value) and Phase (in degrees)
 *      with (# of outputs)-by-(length(w)).
 *
 *      bode_tf(num,den) and bode_tfn(g) returns the frequency response 
 *      of the system whose transfer function is g(s) = num(s)/den(s).
 *
 *      bode_tfm(G) returns the frequency response of the system whose 
 *      transfer function matrix is G(s),
 *
 *      bode_plot_xxx() plots gain (absolute value) and phase (in degree).
 *
 *  SEE ALSO
 *      logspace, dbode, and nyquist
 */

Func List bode_ss(a, b, c, d, iu, w, ...)
	Matrix a, b, c, d;
	Integer iu;
	Array w;
{
	Integer i;
	Complex j;
	String msg;
	Matrix A, B, C, D, T, Q;
	CoArray G;
	Array Mag, Phase;
	Index idx;
	RaMatrix gg;

	error(nargchk(4, 6, nargs, "bode_ss"));

	if (length((msg = abcdchk(a, b, c, d))) > 0) {
		error("bode_ss(): " + msg);
	}

	if (nargs < 6) {
		w = logspace(-2.0, 3.0);
	}
	if (nargs < 5) {
		iu = 1;
	}

	// Balance A
	{T, A} = balance(a);
	B = T \ b;
	C = c * T;

	{Q, A} = hess(A);
	B = Q# * B(:,iu);
	C = C * Q;
	D = d(:,iu);

	// Frequency response
	j = (0,1);
	G = ltifr(A, B, j*w);
	G = C*Matrix(G) + vec2diag(D)*ONE(Rows(C),length(w));

	// Mag = 20*log10(abs(G));
	Mag = abs(G);
	Phase = 180/PI*unwrap(Im(log(G)));

    // Correct phase of the plant with integrators by subtracting 360 deg
	gg = ss2tfm(A, B, C, D);
	for (i = 1; i <= Rows(gg); i++) {
		idx = find(Phase(i,:) .> 0 && eval(De(gg(i)), 0*w) .== 0);
		Phase(i,idx) = Phase(i,idx) .- 360;
	}

	return {Mag, Phase, w};
}

Func List bode_tf(num, den, w, ...)
	Matrix num, den;
	Array w;
{
	Rational g;
	Array gain, phase_;

	error(nargchk(1, 2, nargs, "bode_tf"));

	g = tf2tfn(num, den);
	
	if (nargs == 1) {
		{gain, phase_, w} = bode_tfn(g);
	} else {
		{gain, phase_, w} = bode_tfn(g, w);
	}

	return {gain, phase_, w};
}

Func List bode_tfn(g, w, ...)
	Rational g;
	Array w;
{
	Complex j;
	Array data, gain, phase_;
	Index idx;

	error(nargchk(1, 2, nargs, "bode_tfn"));

	if (nargs == 1) {
		w = logspace(-2.0, 3.0);
	}

	j = (0,1);
	data = eval(g, j*w);

	gain = abs(data);
//	gain = 20*log10(abs(data));
	phase_ = 180/PI*unwrap(Im(log(data)));

    // Correct phase for plants with integrators by subtracting 360 deg
	idx = find(phase_ .> 0 && eval(De(g), 0*w) .== 0);
	phase_(idx) = phase_(idx) .- 360;

	return {gain, phase_, w};
}

Func List bode_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Complex j;
	Array data, gain, phase_;
	Index idx;

	error(nargchk(1, 2, nargs, "bode_tfm"));

	if (nargs == 1) {
		w = logspace(-2.0, 3.0);
	}

	j = (0,1);
	data = eval(G, j*w);

	gain = abs(data);
//	gain = 20*log10(abs(data));
	phase_ = 180/PI*unwrap(Im(log(data)));

	return {gain, phase_, w};
}

Func void bode_plot_ss(A, B, C, D, iu, w, ...)
	Matrix A, B, C, D;
	Integer iu;
	Array w;
{
	Array gain, phase_, dB0, deg180;
	Integer i, p, win;
	List name1, name2;
	String msg;

	error(nargchk(4, 6, nargs, "bode_plot_ss"));

	msg = abcdchk(A, B, C, D);
	if (length(msg) > 0) {
		error("bode_plot_ss(): " + msg);
	}

	if (nargs < 5) {
		iu = 1;
	}
	if (nargs < 6) {
		w = logspace(-2.0, 3.0);
	}

	dB0 = 0.00001*Z(w);
	deg180 = -180*ONE(w);
	{gain, phase_} = bode(A, B, C, D, iu, w);
	gain = 20*log10(gain);

	p = Rows(C);
	name1 = makelist(p+1);
	name2 = makelist(p+1);
	for (i = 1; i <= p; i++) {
		name1(i) = sprintf("gain-%d", i);
		name2(i) = sprintf("phase-%d", i);
	}
	name1(p+1) = "";
	name2(p+1) = "";

	win = mgplot_cur_win();
	mgplot_reset(win);

	mgplot_hold(win, 1);
	mgplot_subplot(win,2,1,1);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "gain [dB]");
	mgplot_title(win, "Bode Plot (gain)");
	mgplot_semilogx(win, w, [[gain][dB0]], name1);

	mgplot_subplot(win,2,1,2);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "phase [deg]");
	mgplot_title(win, "Bode Plot (phase)");
	mgplot_semilogx(win, w, [[phase_][deg180]], name2);

	mgplot_hold(win, 0);
}

Func void bode_plot_tf(num, den, w, ...)
	Matrix num, den;
	Array w;
{
	Rational g;

	error(nargchk(2, 3, nargs, "bode_plot_tf"));

	g = tf2tfn(num, den);
	
	if (nargs == 2) {
		bode_plot_tfn(g);
	} else {
		bode_plot_tfn(g, w);
	}
}

Func void bode_plot_tfn(g, w, ...)
	Rational g;
	Array w;
{
	Integer win;
	Array gain, phase_, dB0, deg180;

	error(nargchk(1, 2, nargs, "bode_plot_tfn"));

	if (nargs == 1) {
		w = logspace(-2.0, 3.0);
	}

	dB0 = 0.00001*Z(w);
	deg180 = -180*ONE(w);
	{gain, phase_} = bode_tfn(g, w);
	gain = 20*log10(gain);

	win = mgplot_cur_win();
	mgplot_reset(win);

	mgplot_hold(win, 1);

	mgplot_subplot(win, 2, 1, 1);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "gain [dB]");
	mgplot_semilogx(win, w, [[gain][dB0]], {"gain", ""});

	mgplot_subplot(win, 2, 1, 2);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "phase [deg]");
	mgplot_semilogx(win, w, [[phase_][deg180]], {"phase", ""});

	mgplot_hold(win, 0);
}

Func void bode_plot_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Integer win;
	Array gain, phase_, dB0, deg180;

	error(nargchk(1, 2, nargs, "bode_plot_tfm"));

	if (nargs == 1) {
		w = logspace(-2.0, 3.0);
	}

	dB0 = 0.00001*Z(w);
	deg180 = -180*ONE(w);
	{gain, phase_} = bode_tfm(G, w);
	gain = 20*log10(gain);

	win = mgplot_cur_win();
	mgplot_reset(win);

	mgplot_hold(win, 1);

	mgplot_subplot(win, 2, 1, 1);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "gain [dB]");
	mgplot_semilogx(win, w, [[gain][dB0]], {"gain", ""});

	mgplot_subplot(win, 2, 1, 2);
	mgplot_xlabel(win, "w [rad/sec]");
	mgplot_ylabel(win, "phase [deg]");
	mgplot_semilogx(win, w, [[phase_][deg180]], {"phase", ""});

	mgplot_hold(win, 0);
}


Func List bode(a, b, c, d, iu, w, ...)
	Matrix a, b, c, d;
	Integer iu;
	Array w;
{
	Array Mag, Phase, w2;

	error(nargchk(4, 6, nargs, "bode"));

	if (nargs == 4) {
		{Mag,Phase,w2} = bode_ss(a,b,c,d);
	} else if (nargs == 5) {
		{Mag,Phase,w2} = bode_ss(a,b,c,d,iu);
	} else {
		{Mag,Phase,w2} = bode_ss(a,b,c,d,iu,w);
	}
	
	return {Mag,Phase,w2};
}

Func List Bode_tf(num, den, w, ...)
	Matrix num, den;
	Array w;
{
	Array Mag, Phase, w2;

	error(nargchk(1, 2, nargs, "Bode_tf"));

	pause "Bode_tf() is obsolete. Use bode_tf()", 1;
	print "\n";
	
	if (nargs == 2) {
		{Mag,Phase,w2} = bode_tf(num,den);
	} else {
		{Mag,Phase,w2} = bode_tf(num,den,w);
	} 
	return {Mag,Phase,w2};
}

Func List Bode_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Array Mag, Phase, w2;

	error(nargchk(1, 2, nargs, "Bode_tfm"));

	pause "Bode_tfm() is obsolete. Use bode_tfm()", 1;
	print "\n";	

	if (nargs == 1) {
		{Mag,Phase,w2} = bode_tfm(G);
	} else {
		{Mag,Phase,w2} = bode_tfm(G,w);
	}

	return {Mag,Phase,w2};
}

Func void bodeplot(A, B, C, D, iu, w, ...)
	Matrix A, B, C, D;
	Integer iu;
	Array w;
{
	error(nargchk(4, 6, nargs, "bodeplot"));

	pause "bodeplot() is obsolete. Use bode_plot_ss()", 1;
	print "\n";
	
	if (nargs == 4) {
		bode_plot_ss(A,B,C,D);
	} else if (nargs == 5) {
		bode_plot_ss(A,B,C,D,iu);
	} else {
		bode_plot_ss(A,B,C,D,iu,w);
	}
}

Func void BodePlot_tf(g, w, ...)
	Rational g;
	Array w;
{
	error(nargchk(1, 2, nargs, "BodePlot_tf"));

	pause "BodePlot_tf() is obsolete. Use bode_plot_tfm()", 1;
	print "\n";
	
	if (nargs == 1) {
		bode_plot_tfn(g);
	} else {
		bode_plot_tfn(g,w);
	}
}