% PFALLPZ all pass filtering to stabilize TF poles and zeros.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DESCRIPTION:
% 
%     All pass filtering in order to stabilize transfer function poles and
%     zeros. It inputs a partial fraction expanded discrete model and
%     outputs a pole-zero minimum phase system
% 
% CALL:
% 
%     [resp,np] = pfallpz(ir,ip,id,mresp,f,fs)
%     [resp,np] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
%     [resp,np,nz] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
% 
% INPUTS:
% 
%     ir: are residues
%     ip: are poles
%     id: is direct term
%     f: is the frequancies vector in (Hz)
%     fs: is the sampling frequency in (Hz)
%     minphase: is a flag assuming true (output a minimum phase system) or
%     false (output a stable non minimum phase system) values. Default,
%     false
%     
% OUTPUTS:
% 
%     resp: is the functions phase frequency response
%     np: are the new stable poles
%
% NOTE:
% 
%     This function make use of signal analysis toolbox functions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VERSION: $Id: pfallpz.m,v 1.6 2009/06/10 15:47:00 luigi Exp $
% 
% HISTORY:     12-09-2008 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = pfallpz(ir,ip,id,mresp,f,fs,varargin)

  % Reshaping
  [a,b] = size(ir);
  if a<b
    ir = ir.'; % reshape as a column vector
  end

  [a,b] = size(ip);
  if a<b
    ip = ip.'; % reshape as a column vector
  end

  [a,b] = size(f);
  if a<b
    f = f.'; % reshape as a column vector
  end

  [a,b] = size(id);
  if a > b
    id = id.'; % reshape as a row
    id = id(1,:); % taking the first row (the function can handle only simple constant direct terms)
  end

  if isempty(fs)
    fs = 1;
  end
  [a,b] = size(fs);
  if a ~= b
    disp(' Fs has to be a number. Only first term will be considered! ')
    fs = fs(1);
  end
  
  if nargin == 7
    minphase = varargin{1};
  else
    minphase = false;
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  [Na,Nb] = size(ir);
  np = zeros(Na,Nb);
  nz = zeros(Na,Nb);
  for nn = 1:Nb

    r = ir(:,nn);
    d = id(1,nn);
    p = ip;
%     iresp = mresp(:,nn);

%     k = sum(r)+d;

    % stabilizing poles
    sp = p;
    unst = abs(p) > 1;
    sp(unst) = 1./conj(sp(unst));
    
    s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
    
    pp = p(unst);
    psp = sp(unst);
    for ii = 1:length(s)
      nterm = 1;
      for jj = 1:length(sp(unst))
        nterm = nterm.*(s(ii)-pp(jj))./(s(ii)-psp(jj));
      end
      phs(ii,1) = angle(nterm);
    end
      
    resp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
    
    % output stable poles
    np(:,nn) = sp;
    
    if minphase
      if d~=0
        error('!!!Minimum phase filers can be obtained only when direct term is zero')
      end
      % finding zeros
%       N = Na+1;
%       [mults, idx] = mpoles(p,1e-15,0);  % checking for poles multiplicity
%       p = p(idx);     % re-arrange poles & residues
%       r = r(idx);
%       den = poly(p);
%       num = conv(den,d);
%       for i=1:Na
%          temp = poly( p([1:(i-mults(i)), (i+1):Na]) );
%          num = num + [r(i)*temp zeros(1,N-length(temp))];
%       end
%       zrs = roots(num);
      D = 0; % direct term of sigma
  
      A = diag(p);
      B = ones(Na,1);
      C = zeros(1,Na);
      % Real poles have real residues, complex poles have comples residues

      cindex=zeros(1,Na);
      for m=1:Na 
        if imag(p(m))~=0  
          if m==1 
            cindex(m)=1;
          else
            if cindex(m-1)==0 || cindex(m-1)==2
              cindex(m)=1; cindex(m+1)=2; 
            else
              cindex(m)=2;
            end
          end 
        end
      end


      for kk = 1:Na
        if cindex(kk) == 1
          A(kk,kk)=real(p(kk));
          A(kk,kk+1)=imag(p(kk));
          A(kk+1,kk)=-1*imag(p(kk));
          A(kk+1,kk+1)=real(p(kk));
          B(kk,1) = 2;
          B(kk+1,1) = 0;
          C(1,kk) = real(r(kk,1));
          C(1,kk+1) = imag(r(kk,1));
        elseif cindex(kk) == 0 % real pole
          C(1,kk) = r(kk,1);
        end
      end
      
      [zrs,p2,k] = ss2zp(A,B,C,D,1);
      
      % stabilizing zeros
      szrs = zrs;
      % willing to work with columns
      [a,b] = size(szrs);
      if a<b
        szrs = szrs.'; % reshape as a column vector
        zrs = zrs.';
      end
      % adding the zero at the origin
      zrs = [0;zrs];
      szrs = [0;szrs];
      % do stabilization
      zunst = abs(zrs) > 1;
      szrs(zunst) = 1./conj(zrs(zunst));
      
      zzrs = zrs(zunst);
      zszrs = szrs(zunst);
      for ii = 1:length(s)
        nterm = 1;
        for jj = 1:length(szrs(zunst))
          nterm = nterm.*(s(ii)-zszrs(jj))./(s(ii)-zzrs(jj));
        end
        zphs(ii,1) = angle(nterm);
      end

      resp(:,nn) = resp(:,nn).*(cos(zphs)+1i.*sin(zphs));
      
      % output stable zeros
      nz(:,nn) = szrs;
    end
    
  end
  
  
  % output
  if nargout == 1
    varargout{1} = resp;
  elseif nargout == 2
    varargout{1} = resp;
    varargout{2} = np;
  elseif (nargout == 3) && minphase
    varargout{1} = resp;
    varargout{2} = np;
    varargout{3} = nz;
  else
    error('Too many output arguments!')
  end
end