/* -*- MaTX -*-
 *
 *  NAME
 *      feedbk() - Common negative feedback
 *
 *  SYNOPSIS
 *      {A,B,C,D} = feedbk(A1,B1,C1,D1,type)
 *          Matrix A,B,C,D;
 *          Matrix A1,B1,C1,D1;
 *          Integer type;
 *
 *      {A,B,C,D} = feedbk(A1,B1,C1,D1,type,A2,B2,C2,D2)
 *          Matrix A,B,C,D;
 *          Matrix A1,B1,C1,D1;
 *          Integer type;
 *          Matrix A2,B2,C2,D2;
 *
 *      {A,B,C,D} = feedbk(A1,B1,C1,D1,type,FH)
 *          Matrix A,B,C,D;
 *          Matrix A1,B1,C1,D1;
 *          Integer type;
 *          Matrix FH;
 *
 *  DESCRIPTION
 *       feedbk() generates a negative feedback system. type takes a values
 *       among the following.
 *    
 *         1: forward = I,             back = (A1,B1,C1,D1)
 *         2: forward = (A1,B1,C1,D1), back = I
 *         3: forward = (A1,B1,C1,D1), back = (A2,B2,C2,D2)
 *         4: forward = (A1,B1,C1,D1), back = state feedback
 *         5: forward = (A1,B1,C1,D1), back = output injection
 * 
 *  SEE ALSO
 *      feedback
 */

Func List feedbk(A1,B1,C1,D1,type,A2,B2,C2,D2, ...)
	Matrix A1,B1,C1,D1,A2,B2,C2,D2;
	Integer type;
{
	Matrix A,B,C,D;
	Matrix D1i,D21i,D12i,F,H;
	String msg;

	error(nargchk(5, 9, nargs, "feedbk"));

	if (length((msg = abcdchk(A1, B1, C1, D1))) > 0) {
		error("feedbk(): " + msg);
	}
	
	switch (type) {
	  case 1:
		D1i = (1 .+ D1)~;
		A = A1 - B1*D1i*C1;
		B = B1*D1i;
		C = -D1i*C1;
		D = D1i;
		break;
	  case 2:
		D1i = (1 .+ D1)~;
		A = A1 - B1*D1i*C1;
		B = B1*D1i;
		C = D1i*C1;
		D = D1i*D1;
		break;
	  case 3:
		if (length((msg = abcdchk(A2, B2, C2, D2))) > 0) {
			error("feedback(): " + msg);
		}

		D21i = (1 .+ D2*D1)~;
		D12i = (1 .+ D1*D2)~;
		A = [[A1 - B1*D21i*D2*C1,     -B1*D21i*C2  ]
			 [    B2*D12i*C1     A2 - B2*D12i*D1*C2]];
		B = [[ B1*D21i  ]
			 [B2*D12i*D1]];
		C = [D12i*C1, -D12i*D1*C2];
		D = D12i*D1;
		break;
	  case 4:
		F = A2;
		A = A1 - B1*F;
		B = B1;
		C = C1 - D1*F;
		D = D1;
		break;
	  case 5:
		H = A2;
		A = A1 - H*C1;
		B = B1 - H*D1;
		C = C1;
		D = D1;
		break;
	}

	return {A,B,C,D};
}