/* -*- MaTX -*-
 *
 *  NAME
 *      nyquist_ss()  - Nyquist response of MIMO cont-time linear ss. systems
 *      nyquist_tf()  - Nyquist response of SISO cont-time linear systems
 *      nyquist_tfn() - Nyquist response of SISO cont-time linear systems
 *      nyquist_tfm() - Nyquist response of MIMO cont-time linear systems
 *
 *      nyquist_plot_ss()  - Plot Nyquist response of SISO C.T. linear systems
 *      nyquist_plot_tf()  - Plot Nyquist response of SISO C.T. linear systems
 *      nyquist_plot_tfn() - Plot Nyquist response of SISO C.T. linear systems
 *      nyquist_plot_tfm() - Plot Nyquist response of MIMO C.T. linear systems
 *
 *  SYNOPSIS
 *      {re,im} = nyquist_ss(A,B,C,D)
 *         Array re,im;
 *         Matrix A,B,C,D;
 *
 *      {re,im} = nyquist_ss(A,B,C,D,iu)
 *         Array re,im;
 *         Matrix A,B,C,D;
 *         Integer iu;
 *
 *      {re,im} = nyquist_ss(A,B,C,D,iu,w)
 *         Array re,im;
 *         Matrix A,B,C,D;
 *         Integer iu;
 *         Array w;
 *
 *      {re,im} = nyquist_tf(num,den)
 *         Array re,im;
 *         Matrix num,den;
 *
 *      {re,im} = nyquist_tf(G,num,den)
 *         Array re,im;
 *         Matrix num,den;
 *         Array w;
 *
 *      {re,im} = nyquist_tfn(g)
 *         Array re, im;
 *         Rational g;
 *
 *      {re,im} = nyquist_tfn(g, w)
 *         Array re, im;
 *         Rational g;
 *         Array w;
 *
 *      {re,im} = nyquist_tfm(G)
 *         Array re, im;
 *         RaMatrix G;
 *
 *      {re,im} = nyquist_tfm(G, w)
 *         Array re, im;
 *         RaMatrix G;
 *         Array w;
 *
 *      nyquist_plot_ss(A,B,C,D)
 *         Matrix A,B,C,D;
 *
 *      nyquist_plot_ss(A,B,C,D,iu)
 *         Matrix A,B,C,D;
 *         Integer iu;
 *
 *      nyquist_plot_ss(A,B,C,D,iu,w)
 *         Matrix A,B,C,D;
 *         Integer iu;
 *         Array w;
 *
 *      nyquist_plot_tf(num, den)
 *         Matrix num, den;
 *
 *      nyquist_plot_tf(num, den, w)
 *         Matrix num, den;
 *         Array w;
 *
 *      nyquist_plot_tfn(g)
 *         Rational g;
 *
 *      nyquist_plot_tfn(g, w)
 *         Rational g;
 *         Array w;
 *
 *      nyquist_plot_tfm(G)
 *         RaMatrix G;
 *
 *      nyquist_plot_tfm(G, w)
 *         RaMatrix G;
 *         Array w;
 *
 *  DESCRIPTION
 *      nyquist_ss(A,B,C,D,iu,w) returns the nyquist response of the system:
 *          .
 *          x = Ax + Bu
 *          y = Cx + Du
 *
 *      from the iu'th input.  w must contain the frequencies (in radians)
 *      at which the Nyquist response is to be evaluated.
 *
 *      nyquist_ss() returns Rows(y)-by-length(w) matrices which are real
 *      and imaginary part of Nyquist responses.
 *
 *      nyquist_tf(num,den) and nyquist_tfn(g) returns the nyquist response 
 *      of the system whose transfer function is g(s) = num(s)/den(s).
 *
 *      nyquist_tfm(G) returns the nyquist response of the system whose 
 *      transfer function matrix is G(s),
 *
 *      nyquist_plot_xx() plots the nyquist responses.
 *
 *  SEE ALSO
 *      logspace and bode
 */


Func List nyquist_ss(A_, B_, C_, D_, iu, w, ...)
	Matrix A_, B_, C_, D_;
	Integer iu;
	Array w;
{
	Complex j;
	Matrix A,B,C,D,Q,T;
	CoArray G;
	String msg;

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

	if (length((msg = abcdchk(A_, B_, C_, D_))) > 0) {
		error("nyquist_ss(): " + msg);
	}

	if (nargs < 6) {
		w = logspace(-2.0, 3.0);
		w = [-fliplr(w) w];
	}
	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);

	j = (0,1);
	G = ltifr(A, B, j*w);

	G = C*Matrix(G) + vec2diag(D)*ONE(Rows(C),length(w));
	return {Re(G), Im(G)};
}

Func List nyquist_tfn(g, w, ...)
	Rational g;
	Array w;
{
	Matrix A,B,C,D;
	Array data_r, data_i;

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

	{A,B,C,D} = tfn2ss(g);
	
	if (nargs == 1) {
		{data_r,data_i} = nyquist_ss(A,B,C,D);
	} else {
		{data_r,data_i} = nyquist_ss(A,B,C,D,1,w);
	}

	return {data_r, data_i};
}

Func List nyquist_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Integer i, k, flag;
	Real eps;
	Complex j;
	Array data;

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

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

	j = (0,1);
	eps = 1.0e-10;
	
	flag = 0;
	for (i = 1; i <= Rows(G); i++) {
		for (k = 1; k <= Cols(G); k++) {
			if (any(abs(Re(poles(G(i,k)))) .< eps)) {
				flag = 1;
				break;
			}
		}
		if (flag == 1) {
			break;
		}
	}
	
	if (flag) {
		data = eval(G, j*w .+ eps);
	} else {
		data = eval(G, j*w);
	}
	return {Re(data), Im(data)};
}

Func List nyquist_tf(num, den, w, ...)
	Matrix num, den;
	Array w;
{
	Matrix A,B,C,D;
	Array data_r, data_i;

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

	{A,B,C,D} = tf2ss(num, den);
	
	if (nargs == 2) {
		{data_r,data_i} = nyquist_ss(A,B,C,D);
	} else {
		{data_r,data_i} = nyquist_ss(A,B,C,D,1,w);
	}

	return {data_r, data_i};
}

Func void nyquist_plot_ss(A, B, C, D, iu, w, ...)
	Matrix A, B, C, D;
	Integer iu;
	Array w;
{
	Matrix im,re;
	Integer i, p, win;
	List name;

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

	if (nargs == 6) {
		{re,im} = nyquist_ss(A,B,C,D,iu,w);
	} else if (nargs == 5) {
		{re,im} = nyquist_ss(A,B,C,D,iu);
	} else {
		{re,im} = nyquist_ss(A,B,C,D);
	}

	p = Rows(C);
	name = makelist(p);
	for (i = 1; i <= p; i++) {
		name(i) = sprintf("nyquist-%d", i);
	}

	win = mgplot_cur_win();
	mgplot_reset(win);
	mgplot_xlabel(win, "Re");
	mgplot_ylabel(win, "Im");
	mgplot_title(win, "Nyquist Plot");
	mgplot(win, re, im, name);
}

Func void nyquist_plot_tfn(g, w, ...)
	Rational g;
	Array w;
{
	Integer win;
	Array re, im;
	Real min_re, max_re, max_im;

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

	if (nargs == 1) {
		{re, im} = nyquist_tfn(g);
	} else {
		{re, im} = nyquist_tfn(g, w);
	}

	max_re = max(re);
	min_re = min(re);
	max_im = max(im);

	win = mgplot_cur_win();
	mgplot_xlabel(win, "Re");
	mgplot_ylabel(win, "Im");
	mgplot_title(win, "Nyquist Plot");

	if (0.1 < abs(max_re) || 0.1 < abs(min_re)) {
		if (max_re < 1) {
			max_re = 1;
		} else if (10 < max_re) {
			max_re = 10;
		} else {
			max_re = ceil(max_re);
		}
		if (-1 < min_re) {
			min_re = -1;
		} else if (min_re < -10) {
			min_re = -10;
		} else {
			min_re = floor(min_re);
		}
 	}
	mgplot_cmd(win, sprintf("set xrange [%g:%g]", min_re, max_re));
	mgplot(win, re, im, {""});

	if (0.1 < abs(max_re) || 0.1 < abs(min_re)) {
		mgreplot(win, [-1], [0], {""}, {"w points"});
	}

	mgreplot(win, [min_re, max_re], [0 0], {""}, {"w line 3"});

	if (0.1 < abs(max_im)) {
		if (max_im < 1) {
			max_im = 1;
		} else if (10 < max_im) {
			max_im = 10;
		} else {
			max_im = ceil(max_im);
		}
	}
	mgreplot(win, [0 0], [-max_im max_im], {""}, {"w line 3"});
	mgplot_reset(win);
}

Func void nyquist_plot_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Matrix num, den;

	error(nargchk(1, 2, nargs, "nyquist_plot_tfm"));
	
	{num, den} = tfm2tf(G(1,1));
	
	if (nargs == 1) {
		nyquist_plot_tf(num, den);
	} else {
		nyquist_plot_tf(num, den, w);
	}
}

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

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

	g = tf2tfn(num,den);

	if (nargs == 2) {
		nyquist_plot_tfn(g);
	} else {
		nyquist_plot_tfn(g,w);
	}
}

Func List nyquist(A, B, C, D, iu, w, ...)
	Matrix A, B, C, D;
	Integer iu;
	Array w;
{
	Matrix re,im;

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

	if (nargs == 4) {
		{re,im} = nyquist_ss(A,B,C,D);
	} else if (nargs == 5) {
		{re,im} = nyquist_ss(A,B,C,D,iu);
	} else {
		{re,im} = nyquist_ss(A,B,C,D,iu,w);
	}
	return {re,im};
}

Func List Nyquit_tf(num, den, w, ...)
	Matrix num, den;
	Array w;
{
	Matrix re,im;

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

	pause "Nyquit_tf() is obsolete. Use nyquit_tf()", 1;
	print "\n";
	
	if (nargs == 2) {
		{re,im} = nyquist_tf(num,den);
	} else {
		{re,im} = nyquist_tf(num,den,w);
	} 
	return {re,im};
}

Func List Nyquist_tfm(G, w, ...)
	RaMatrix G;
	Array w;
{
	Matrix re,im;

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

	pause "Nyquit_tfm() is obsolete. Use nyquit_tfm()", 1;
	print "\n";
	
	if (nargs == 1) {
		{re,im} = nyquist_tfm(G);
	} else {
		{re,im} = nyquist_tfm(G,w);
	} 
	return {re,im};
}

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

	pause "NyquitPlot() is obsolete. Use nyquit_plot_tfn()", 1;
	print "\n";
	
	if (nargs == 1) {
		nyquist_plot_tfn(g);
	} else {
		nyquist_plot_tfn(g,w);
	}
}

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

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