% PFALLPSYMZ all pass filtering in order to stabilize TF poles and zeros.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DESCRIPTION:
% 
%     All pass filtering in order to stabilize transfer functions poles. 
%     It inputs a partial fraction expanded discrete model and outpu
%     residues, poles direct terms and frequency response of the stabilized
%     model. Function can handle multiple models with common poles.
% 
% CALL:
% 
%     [nr,np,nd,resp] = pfallpsymz(r,p,d,f,fs)
% 
% INPUTS:
% 
%     r: are residues. (Npx1) or (NpxM) vector
%     p: are poles. (Npx1) vector
%     d: is direct term (1x1) or (1xM) vector
%     mresp: input model response. (Nx1) or (NxM) vector
%     f: is the frequancies vector in (Hz). (Nx1) vector
%     fs: is the sampling frequency in (Hz). (1x1)
%     
% OUTPUTS:
% 
%     nr: new residues. (Npx1) or (NpxM) vector
%     np: new stable poles. (Npx1) vector
%     nd: new direct term. (1x1) or (1xM) vector
%     nmresp: new model response. (Nx1) or (NxM) vector
% 
% NOTE:
% 
%     This function make use of symbolic math toolbox functions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% VERSION: $Id: pfallpsymz.m,v 1.5 2008/11/10 15:40:11 luigi Exp $
%
% HISTORY:     12-09-2008 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [nr,np,nd,nmresp] = pfallpsymz(r,p,d,mresp,f,fs)

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

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

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

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

  [a,b] = size(d);
  if a > b
    d = d.'; % reshape as a row
    d = d(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

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  f = sym(f,'f');
  fs = sym(fs,'f');
  syms z
  p = sym(p,'f');

  % stabilize poles
  sp = p;
  unst = abs(double(sp)) > 1;
  sp(unst) = 1./conj(sp(unst));
  skp = prod(1./conj(p(unst)));

  [Na,Nb] = size(r);

  for nn = 1:Nb
    
%     pp = p(unst);
%     psp = sp(unst);
%     tterm = 1;
%     for ii = 1:length(pp)
%       tterm = tterm.*(z-pp(ii))./(z-psp(ii));
%     end
%     
%     s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
%     allresp = subs(tterm,z,s);
%     phs = angle(allresp);
%     nmresp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
%     np = double(sp);

    % digits(100)
    % Defining inputs as symbolic variable
    rt = sym(r(:,nn),'f');
  %   p = sym(p,'f');
    dt = sym(d(1,nn),'f');
  %   f = sym(f,'f');
  %   fs = sym(fs,'f');
  %   syms z

    % Function gain coefficient
    k = sum(rt)+dt;

    Np = length(p);

    % Defining the symbolic transfer function
    vter = rt.*z./(z-p);
    vter = [vter; dt];
    Mter = diag(vter);
    H = trace(Mter);

    % Factorizing the transfer function
    HH = factor(H);

    % Extracting numerator and denominator
    [N,D] = numden(HH);

    % Symbolci calculation of function zeros
    cN = coeffs(expand(N),z);
    n = length(cN);
    cN2 = -1.*cN(2:end)./cN(1);
    A = sym(diag(ones(1,n-2),-1));
    A(1,:) = cN2;
    zrs = eig(A);
    if double(d) == 0
      zrs = [zrs; sym(0,'f')];
    end

    % % stabilize zeros
    % szrs = zrs;
    % unst = abs(double(szrs)) > 1;
    % szrs(unst) = 1./conj(szrs(unst));
    % skz = prod(conj(zrs(unst)));

  %   % stabilize poles
  %   sp = p;
  %   unst = abs(double(sp)) > 1;
  %   sp(unst) = 1./conj(sp(unst));
  %   skp = prod(1./conj(p(unst)));

    % Correcting for some special cases
    % if isempty(skz)
    %   skz = sym(1,'f');
    % end
    if isempty(skp)
      skp = sym(1,'f');
    end
    if isempty(k)
      k = sym(1,'f');
    end

    % Calculating new gain
    % sk = real(k*skz*skp);
    sk = real(k*skp);

    HHHn = sym(1,'f');

    for jj = 1:Np
      HHHn = HHHn.*(z-zrs(jj));
      tsp = sp;
      tsp(jj) = [];
      tHHHd = sym(1,'f');
      for kk = 1:Np-1
        tHHHd = tHHHd.*(z-tsp(kk));
      end
      HHHd(jj,1) = tHHHd;

    end

    for jj = 1:Np
      sr(jj,1) = subs(sk*HHHn/(z*HHHd(jj,1)),z,sp(jj));
    end

    np = double(sp);
    for kk = 1:Np
      if imag(np(kk)) == 0
        sr(kk) = real(sr(kk));
      end
    end

    nr(:,nn) = double(sr);
    nd(:,nn) = double(dt);

    % Model evaluation
    pfparams.type = 'disc';
    pfparams.freq = f;
    pfparams.fs = fs;
    pfparams.res = nr(:,nn);
    pfparams.pol = np;
    pfparams.dterm = nd(:,nn);
    pfr = utils.math.pfresp(pfparams);
    resp = pfr.resp;

    ratio = mean(abs(mresp(:,nn))./abs(resp));
    resp = resp.*ratio;
    nr(:,nn) = nr(:,nn).*ratio;
    nd(:,nn) = nd(:,nn).*ratio;
    nmresp(:,nn) = resp;

  end