/* -*- MaTX -*-
 *
 *  NAME
 *      rlocus_ss()       - Root locus of MIMO cont-time linear ss. systems
 *      rlocus_tf()       - Root locus of SISO cont-time linear systems
 *      rlocus_tfn()      - Root locus of SISO cont-time linear systems
 *      rlocus_tfm()      - Root locus of MIMO cont-time linear systems
 *      rlocus_plot_ss()  - Root locus plot
 *      rlocus_plot_tf()  - Root locus plot
 *      rlocus_plot_tfn() - Root locus plot
 *      rlocus_plot_tfm() - Root locus plot
 *
 *  SYNOPSIS
 *      RL = rlocus_ss(A,b,c,d)
 *         CoArray RL;
 *         Matrix A,b,c,d;
 *
 *      RL = rlocus_ss(A,b,c,d,K)
 *         CoArray RL;
 *         Matrix A,b,c,d;
 *         Array K;
 *
 *      RL = rlocus_tf(num,den)
 *         CoArray RL;
 *         Matrix num, den;
 *
 *      RL = rlocus_tf(num,den,K)
 *         CoArray RL;
 *         Matrix num, den;
 *         Array K;
 *
 *      RL = rlocus_tfn(g)
 *         CoArray RL;
 *         Rational g;
 *
 *      RL = rlocus_tfn(g,K)
 *         CoArray RL;
 *         Rational g;
 *         Array K;
 *
 *      RL = rlocus_tfm(g)
 *         CoArray RL;
 *         RaMatrix g;
 *
 *      RL = rlocus_tfm(g,K)
 *         CoArray RL;
 *         RaMatrix g;
 *         Array K;
 *
 *      rlocus_plot_ss(A,b,c,d)
 *         Matrix A,b,c,d;
 *
 *      rlocus_plot_ss(A,b,c,d,K)
 *         Matrix A,b,c,d;
 *         Array K;
 *
 *      rlocus_plot_tf(num,den)
 *         Matrix num,den;
 *
 *      rlocus_plot_tf(num,den,K)
 *         Matrix num,den;
 *         Array K;
 *
 *      rlocus_plot_tfn(g)
 *         Rational g;
 *
 *      rlocus_plot_tfn(g,K)
 *         Rational g;
 *         Array K;
 *
 *      rlocus_plot_tfm(g)
 *         RaMatrix g;
 *
 *      rlocus_plot_tfm(g,K)
 *         RaMatrix g;
 *         Array K;
 *
 *  DESCRIPTION
 *       rlocus_tf(num, den) returns the locus of the zeros of
 *
 *                        num(s)
 *         H(s) = 1 + k ----------  =  0
 *                        den(s)
 *
 *      for the gains specified in K.  
 *
 *      rlocus_tf(num,den,K) returns a (length(den)-1)-by-length(K) matrix.
 *      Each column RL(:,i) corresponds to a gain K(i).
 *
 *      rlocus(A,b,c,d,K) returns the root-locus of the system
 *              .
 *              x = Ax + Bu
 *              y = Cx + Du
 *              u = -k*y
 *
 *      rlocus_plot_xx() plots root-locus.
 *
 *  SEE ALSO
 *      nyquist and bode
 */

Func CoArray rlocus_ss(A, b, c, d, K, ...)
	Matrix A, b, c, d;
	Array K;
{
	Integer i;
	Matrix BC,k;
	CoArray RL;
	String msg;

	error(nargchk(4, 5, nargs, "rlocus_ss"));

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

	if (nargs == 4) {
		K = logspace(1e-3, 1e3, 1000);
	}

	RL = (Z(Rows(b),length(K)),:);

	k = K/(1 .+ K*d(1));
	BC = b*c;
	for (i = 1; i <= length(K); i++) {
		RL(:,i) = eigval(A - BC*k(i));
	}

	return RL;
}

Func CoArray rlocus_tfn(g, K, ...)
	Rational g;
	Array K;
{
	Matrix A,b,c,d;
	CoArray R;

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

	{A,b,c,d} = tfn2ss(g);

	if (nargs == 2) {
		R = rlocus_ss(A,b,c,d);
	} else {
		R = rlocus_ss(A,b,c,d,K);
	}
	return R;
}

Func CoArray rlocus_tfm(G, K, ...)
	RaMatrix G;
	Array K;
{
	Matrix A,b,c,d;
	CoArray R;
	
	error(nargchk(1, 2, nargs, "rlocus_tfm"));

	{A,b,c,d} = tfm2ss(G);
	
	if (nargs == 1) {
		R = rlocus_ss(A,b,c,d);
	} else {
		R = rlocus_ss(A,b,c,d,K);
	}
	return R;
}

Func CoArray rlocus_tf(num, den, K, ...)
	Matrix num, den;
	Array K;
{
	Matrix A,b,c,d;
	CoArray R;

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

	{A,b,c,d} = tf2ss(num,den);

	if (nargs == 2) {
		R = rlocus_ss(A,b,c,d);
	} else {
		R = rlocus_ss(A,b,c,d,K);
	}
	return R;
}

Func void rlocus_plot_ss(A, b, c, d, K,...)
	Matrix A, b, c, d;
	Array K;
{
	Integer win;
	CoArray R;

	error(nargchk(4, 5, nargs, "rlocus_plot_ss"));
	
	if (nargs == 4) {
		R = rlocus_ss(A,b,c,d);
	} else {
		R = rlocus_ss(A,b,c,d,K);
	}

	win = mgplot_cur_win();
	mgplot_grid(win, 1);
	mgplot_key(win, 0);
	mgplot_cmd(win, "set data style points");
	mgplot(win, Re(R), Im(R));
}

Func void rlocus_plot_tfn(g, K,...)
	Rational g;
	Array K;
{
	Matrix A,b,c,d;
	
	error(nargchk(1, 2, nargs, "rlocus_plot_tfn"));

	{A,b,c,d} = tfn2ss(g);
	
	if (nargs == 2) {
		rlocus_plot_ss(A,b,c,d);
	} else {
		rlocus_plot_ss(A,b,c,d,K);
	}	
}

Func void rlocus_plot_tfm(G, K,...)
	RaMatrix G;
	Array K;
{
	Matrix A,b,c,d;

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

	{A,b,c,d} = tfm2ss(G);
	
	if (nargs == 1) {
		rlocus_plot_ss(A,b,c,d);
	} else {
		rlocus_plot_ss(A,b,c,d,K);
	}
}

Func void rlocus_plot_tf(num, den, K,...)
	Matrix num, den;
	Array K;
{
	Matrix A,b,c,d;
	
	error(nargchk(2, 3, nargs, "rlocus_plot_tf"));

	{A,b,c,d} = tf2ss(num,den);
	
	if (nargs == 2) {
		rlocus_plot_ss(A,b,c,d);
	} else {
		rlocus_plot_ss(A,b,c,d,K);
	}	
}

Func CoArray rlocus(num, den, K, ...)
	Matrix num, den;
	Array K;
{
	CoArray R;

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

	if (nargs == 2) {
		R = rlocus_tf(num, den);
	} else {
		R = rlocus_tf(num, den, K);
	}

	return R;
}

Func void rlocusp(num, den, K, ...)
	Matrix num, den;
	Array K;
{
	error(nargchk(2, 3, nargs, "rlocusp"));

	pause "rlocusp() is obsolete. Use rlocus_plot_tf()", 1;
	print "\n";

	if (nargs == 2) {
		rlocus_plot_tf(num, den);
	} else {
		rlocus_plot_tf(num, den, K);
	}
}