/* -*- MaTX -*-
 *
 *  NAME
 *      dbode_ss()      - Discrete-time Bode response 
 *      dbode_plot_ss() - Plot discrete-time Bode response
 *
 *  SYNOPSIS
 *      {Mag,Phase,w} = dbode_ss(A,B,C,D,dt)
 *          Array Mag, Phase;
 *          Matrix A,B,C,D;
 *          Real dt;
 *
 *      {Mag,Phase,w} = dbode_ss(A,B,C,D,dt,iu)
 *          Array Mag, Phase;
 *          Matrix A,B,C,D;
 *          Real dt;
 *          Integer iu;
 *
 *      {Mag,Phase,w} = dbode_ss(A,B,C,D,dt,iu,w)
 *          Array Mag, Phase;
 *          Matrix A,B,C,D;
 *          Real dt;
 *          Integer iu;
 *          Array w;
 *
 *      dbode_plot_ss(A,B,C,D,dt)
 *          Matrix A,B,C,D;
 *          Real dt;
 *
 *      dbode_plot_ss(A,B,C,D,dt,iu)
 *          Matrix A,B,C,D;
 *          Real dt;
 *          Integer iu;
 *
 *      dbode_plot_ss(A,B,C,D,dt,iu,w)
 *          Matrix A,B,C,D;
 *          Real dt;
 *          Integer iu;
 *          Array w;
 *
 *  DESCRIPTION
 *       dbode_ss() calculates the frequency response of the system:
 *
 *         x[n+1] = Ax[n] + Bu[n]                     -1
 *         y[n]   = Cx[n] + Du[n]        G(z) = C(zI-A) B + D
 *
 *         mag(w) = abs(G(exp(jw))),   phase(w) = angle(G(exp(jw)))
 *
 *       from the iu'th input.  dt is the sampling interval. w must 
 *       contain the frequencies (in radians/s), at which the transfer function
 *       is to be evaluated.
 *
 *       dbode_ss() returns Mag (absolute value) and Phase (in degrees) 
 *       with (# of outputs)-by-(length(w)).
 *
 *       dbode_plot_xx() plots gain (absolute value) and phase (in degree).
 *
 *  SEE ALSO
 *       logspace and bode
 */

Func List dbode_ss(a, b, c, d, dt, iu, w, ...)
	Matrix a, b, c, d;
	Real dt;
	Integer iu;
	Array w;
{
	Integer p, nw;
	Complex j;
	String msg;
	Matrix A, B, C, D, H, T;
	Array Mag, Phase, G;

	error(nargchk(4, 7, nargs, "dbode_ss"));

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

	if (nargs < 5) {
		dt = 1.0;
	}
	if (nargs < 6) {
		iu = 1;
	}
	if (nargs < 7) {
		w = logspace(-2.0, 3.0);
	}
	
	j = (0,1);
	p = Rows(c);
	nw = Cols(w);

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

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

	// Frequency response
	G = ltifr(A, B, exp(j*w*dt));
	G = C*Matrix(G) + vec2diag(D)*ONE(p,nw);

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

	return {Mag, Phase, w};
}

Func void dbode_plot_ss(A, B, C, D, dt, iu, w, ...)
	Matrix A, B, C, D;
	Real dt;
	Integer iu;
	Array w;
{
	Integer i, p, win;
	Array gain, phase, dB0, deg180;
	List name1, name2;
	
	if (nargs < 4 || 7 < nargs) {
		error("dbode_plot_ss(): Incorrect number of arguments.\n");
	}
	if (nargs < 5) {
		iu = 1;
	}
	if (nargs < 6) {
		w = logspace(-2.0, 3.0);
	}
	if (nargs < 7) {
		dt = 1.0;
	}

    dB0 = 0.00001*Z(w);
    deg180 = -180*ONE(w);
	{gain, phase} = dbode_ss(A, B, C, D, dt, 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_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_title(win, "Bode Plot (phase)");
    mgplot_xlabel(win, "w [rad/sec]");
    mgplot_ylabel(win, "phase [deg]");
	mgplot_semilogx(win, w, [[phase][deg180]], name2);

	mgplot_reset(win);
}