%   function [abzp, seq] = allbzp(yz,z,p,lv)
%
%   The ``All-Bz(p)'' allbzp-function calculates all possible filters
%   Bz(p) as described in Havre and Skogestad (1995) by manipulating 
%   the sequence of factorization of the indvidual filters.
%   Note! that the function is recursive.
%   
%   Inputs:  yz   - output directions of RHP-zeros stored as a column 
%                   vectors, yz(:,i) corresponds to zero z(i).
%            z    - zeros stores in a singel vector z, z(i) is expected 
%                   to corrrespond to yz(:,i).
%            p    - single pole where Bz(p).
%            lv   - Input argument only used by the function itself.
% 
%   Outputs: abzp - Bz(p) given as one large matrix of size (nz!*ny x ny)
%                   where ny - number outputs, nz - numper of zeros in z.
%            seq  - vector with variables describing the sequence of 
%                   factorizations of the zeros z(i).
%
%   Written by: Kjetil Havre, 30/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 [abzp, seq] = allbzp(yz,z,p,lv)

   [n, nz] = size(z);
   if n > 1
      z = vtp(z);
      [n, nz] = size(z);
   end

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

   if nargin < 4
      lv = 1:nz;
   end

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

