%   function [abpz, seq] = allbpz(yp,p,z,lv)
%
%   The ``All-Bp(z)'' allbpz-function calculates all possible filters
%   Bp(z) as described in Havre and Skogestad (1995) by manipulating 
%   the sequence of factorization of the indvidual filters.
%   Note! that the function is recursive.
%   
%   Inputs:  yp   - output directions of RHP-poles stored as a column 
%                   vectors, yp(:,i) corresponds to pole p(i).
%            p    - poles stores in a singel vector p, p(i) is expected 
%                   to corrrespond to yp(:,i).
%            z    - single zero where Bp(z).
%            lv   - Input argument only used by the function itself.
% 
%   Outputs: abpz - Bp(z) given as one large matrix of size (ny x np!*ny)
%                   where ny - number outputs, np - numper of poles in p.
%            seq  - vector with variables describing the sequence of 
%                   factorizations of the poles p(i).
%
%   Written by: Kjetil Havre, 24/11-1995, e-mail: kjetil@ife.no
%
%   Reference: Havre K. and S. Skogestad, 1995, ``Effect of RHP Zeros and
%              Poles on Performance in Multivariable Systems''.
%

function [abpz, seq] = allbpz(yp,p,z,lv)

   [n, np] = size(p);
   if n > 1
      p = vtp(p);
      [n, np] = size(p);
   end

   if isempty(z)
      disp('Error: z is empty.');
      return
   end
   
   [nr,nc] = size(yp);
   if nc ~= np
      disp(['Error: Number of poles:', int2num(np),...
            ' is not equal to number of output zero directions: ',...
	    int2num(nc)] );
      return
   end

   if nargin < 4
      lv = 1:np;
   end

   if np==1;
      abpz = eye(nr) + 2*real(p)/(z-p)*yp*yp';
      seq = lv(1);
      return;
   end
   
   abpz = []; seq = [];
   sif = 10^(np-1);
   for i=1:np
      bi      = eye(nr) + 2*real(p(i))/(z-p(i))*yp(:,i)*yp(:,i)';
      [Bl sl] = allbpz( [yp(:,1:i-1) yp(:,i+1:np)], [p(1:i-1) p(i+1:np)], z, [lv(1:i-1) lv(i+1:np)] );
      B    = bi*Bl;
      abpz = [abpz B];
      seq  = [seq lv(i)*sif*ones(size(sl))+sl];
   end
end

