/* -*- MaTX -*-
 *
 *  NAME
 *      zp2ss() - Zero-pole to state-space conversion
 *
 *  SYNOPSIS 
 *      {A,B,C,D} = zp2ss(z,p,k)
 *          Matrix A,B,C,D;
 *          CoMatrix z, p;
 *          Matrix k;
 *
 *  DESCRIPTION
 *      zp2ss(z,p,k) returns a state-space representation 
 *           .
 *           x = Ax + Bu
 *           y = Cx + Du
 *
 *      for the system whose transfer function is given by
 *
 *                 (s-z1)(s-z2)...(s-zn)
 *        G(s) = k ---------------------
 *                 (s-p1)(s-p2)...(s-pn)
 *
 *      poles and zeros are given in p and z, and the gains for each 
 *      numerator of transfer function are in k. 
 *
 *      The A,B,C,D matrices are returned in block diagonal form.
 *
 *  SEE ALSO
 *      zp2tf, zp2tfm, zp2tfn, and ss2zp
 */

Func List zp2ss(z_, p_, k)
	CoMatrix z_, p_;
	Matrix k;
{
	Integer i,np,nz;
	CoMatrix z,p;
	Matrix A,B,C,D,a,b,c,d;
	Matrix de,nu,T;

	z = z_;
	p = p_;

	if (length(k) != Cols(z) && (! isempty(z))) {
		error("zp2ss(): Inconsistency in zeros 'z' or gains 'k'.\n");
	}

	// multiple output case
	if (Cols(z) > 1) {
		{nu,de} = zp2tf(z,p,k);
		{A,B,C,D} = tf2ss(nu,de);
		return {A,B,C,D};
	}

	// single output case

	if ( ! isempty(p)) {
		p = p(abs(Array(p)) != Inf);
	}
	if ( ! isempty(z)) {
		z = z(abs(Array(z)) != Inf);
	}

	// Make groups for complex pairs
	np = length(p);
	nz = length(z);
	
	if ( ! isempty(p)) {
		p = ccpair(p, 10*np*norm(p)*EPS);
	}
	if ( ! isempty(z)) {
		z = ccpair(z, 10*nz*norm(z)*EPS);
	}

	// The last element of p and z may be real number

	if (rem(np,2) && rem(nz,2)) {
		//         s - z(nz)   p(np) - z(nz)
		// G(s) = ---------- = ------------- + 1
		//         s - p(np)     s - p(np)
		A = [Re(p(np))];
		B = [1];
		C = [Re(p(np)) - Re(z(nz))];
		D = [1];
		np--;
		nz--;
 	} else if (rem(np,2)) {
		//           1
		// G(s) = ---------
		//        s - p(np)
		A = [Re(p(np))];
		B = [1];
		C = [1];
		D = [0];
		np--;
	} else if (rem(nz,2)) {
		//              (s - z(nz))
		// G(s) = ------------------------
		//        (s - p(np-1))*(s - p(np))
		nu = Re([1, -z(nz)]);
		de = Re([1, -(p(np-1)+p(np)), p(np-1)*p(np)]);
		A = [[-de(2), -de(3)]
			 [   1     0    ]];
		B = [[1][0]];
		C = [1, nu(2)];
		D = [0];
		nz--;
		np = np - 2;
	} else {
		A = B = C = D = [];
	}

	for (i = 1; i < nz; i = i + 2) {
		//         (s - z(i))*(s - z(i+1))
		// G(s) =  -----------------------
		//         (s - p(i))*(s - p(i+1))
		nu = Re([1, -(z(i)+z(i+1)), z(i)*z(i+1)]);
		de = Re([1, -(p(i)+p(i+1)), p(i)*p(i+1)]);

		a = [[-de(2), -de(3)]
			 [   1       0  ]];
		b = [[1][0]];
		c = [nu(2)-de(2), nu(3)-de(3)];
		d = [1];
		{A,B,C,D} = series(A,B,C,D,a,b,c,d);
	}

	for ( ; i < np; i = i + 2) {
		//                   1
		// G(s) =  -----------------------
		//         (s - p(i))*(s - p(i+1))

		de = Re([1, -(p(i)+p(i+1)), p(i)*p(i+1)]);
		a = [[-de(2), -de(3)]
			 [   1       0  ]];
		b = [[1][0]];
		c = [0 1];
		d = [0];
		{A,B,C,D} = series(A,B,C,D,a,b,c,d);
	}

	C = vec2diag(k) * C;
	D = vec2diag(k) * D;
	return {A,B,C,D};
}