/* -*- MaTX -*-
 *
 *  NAME
 *      pplace() - Pole placement
 *
 *  SYNOPSIS
 *      F = pplace(A,B,P)
 *         Matrix F;
 *         Matrix A, B;
 *         CoMatrix P;
 *
 *      F = pplace(A,B,P,batch)
 *         Matrix F;
 *         Matrix A, B;
 *         CoMatrix P;
 *         Integer batch;
 *
 *  DESCRIPTION
 *       pplace() returns the feedback gain matrix F such that the system
 *      	.
 *      	x = Ax + bu 
 *
 *       with a feedback law
 *
 *          u = -Fx
 *
 *       has closed loop poles specified in P, i.e.,  p = eigval(A-b*f).
 *
 *       A warning message is printed if the nonzero closed loop poles
 *       are greater than 10% from the desired locations specified in P.
 *
 *       PoleAssign() is obsolete.  Use  F = - pplace(A,B,P).
 *
 *  SEE ALSO
 */

Func Matrix pplace(A, B, P, batch, ...)
	Matrix A, B;
	CoMatrix P;
	Integer batch;
{
	Matrix F;
	CoMatrix Po,Pc;
	Index idx;

	Matrix pplaceSISO(), pplaceMIMO(...);
	
	error(nargchk(3, 4, nargs, "pplace"));

	if (nargs == 3) {
		batch = 1;
	}

	if (Cols(B) == 1) {
		F = pplaceSISO(A, B, P);
	} else {
		F = pplaceMIMO(A, B, P, batch);
	}

	// Check results
	Pc = eigval(A - B*F);
	{Po} = sort(P);
	{Pc} = sort(Pc);
	idx = find(Po .!= 0.0);
	Po = Po(idx);
	Pc = Pc(idx);

	if (max(abs(Po-Pc)/abs(Po)) > 0.1) {
		warning("pplace(): Pole locations are more than 10% in error.\n");
	}

	return F;
}

Func Matrix pplaceSISO(A, b, P)
	Matrix A, b;
	CoMatrix P;
{
	String msg;
	Integer n;
	Matrix Ti, Vi, f, p1, p2;

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

	n = Rows(A);

	if (n != length(P)) {
		error("pplaceSISO(): Number of poles is invalid.\n");
	}

	p1 = Matrix(makepoly(A));
	p2 = Matrix(Re(makepoly(P)));

	Vi = inv(ctrm(A,b));
	Ti = obsm(A,Vi(n,:));

	if (version() == 4) {
		f = [(p2 - p1)](1:n) * Ti;
	} else {
		f = [fliplr(p2 - p1)](1:n) * Ti;
	}

	if (isreal(A) && isreal(b)) {
		return Re(f);
	} else {
		return f;
	}
}

Func Matrix pplaceMIMO(A, B, P, batch, ...)
	Matrix A, B;
	CoMatrix P;
	Integer batch;
{
	Integer i, m, n;
	Real a, b;
	Matrix gzi, V, V1, V2, F;

	error(nargchk(3, 4, nargs, "pplaceMIMO"));

	if (nargs == 3) {
		batch = 1;
	}

	n = Rows(A);
	m = Cols(B);

	V = Z(n,n);
	if (batch == 0) {
		gzi = Z(m,n);
		read gzi;
	} else {
		gzi = rand(m,n);
	}

	while (1) {
		for (i = 1; i <= n; i++) {
			a = Re(P(i,1));
			b = Im(P(i,1));

			if (abs(b) < EPS) {	// P(i) is real a real pole
				V(1:n,i) = (A - a*I(n))~ * B * gzi(1:m,i:i);
			} else { // P(i) and P(i+1) are complex conjugate poles
				V1 = ((A - a*I(n))^2 + b^2*I(n))~ * (A - a*I(n))*B;
				V2 = ((A - a*I(n))^2 + b^2*I(n))~ * (b*B);

				V(1:n,i)   = V1*gzi(*,i) - V2*gzi(*,i+1);
				V(1:n,i+1) = V1*gzi(*,i+1) + V2*gzi(*,i);
				i++;
			}
		}

		// V is a singular matrix
		if (minsing(V) < EPS) {
			print "\n'gzi' is not a proper matrix.\n";
			print "Enter a matrix 'gzi' again.\n";
			pause;
		} else {
			break;
		}

		read gzi;
	}

	F = gzi * V~;

	if (isreal(A) && isreal(B)) {
		return Re(F);
	} else {
		return F;
	}
}

Func Matrix PoleAssign(A, B, P, batch, ...)
	Matrix A, B;
	CoMatrix P;
	Integer batch;
{
	Matrix F;

	error(nargchk(3, 4, nargs, "PoleAssign"));

	if (nargs == 3) {
		F = - pplace(A, B, P);
	} else {
		F = - pplace(A, B, P, batch);
	}

	return F;
}