function [Kp,Ki,Kd,Tf] = convert(Num,Poles,Ts,IFormula,DFormula)
% Computes coefficients Kp,Ki,Kd,Tf of the Parallel Form given the
% numerator NUM and the poles of the controller transfer function.
% An error is thrown if the transfer function does not represent a
% PID controller.

%   Copyright 2009-2013 The MathWorks, Inc.
if any(isnan(Num)) || numel(Poles)>2
   error(message('Control:ltiobject:pidOperations2'))
end
Zeros = roots(Num);

% Robustly detect integral action
wp = damp(Poles,Ts);
wz = damp(Zeros,Ts);
wmax = max([NaN ; wp(wp<Inf,:) ; wz(wz<Inf,:)]); % max freq of system dynamics
pI = [];  pD = [];
if Ts==0
   % Look for a pole with small magnitude compared to zeros
   if any(wp==0) || any(wp<1e-8*wmax)
      % s=0 is a pole, or there is a pole close to s=0 relative to WMAX
      pI = 0;
   end
   if numel(Poles)==1+numel(pI)
      % Note: This approach has superior accuracy when we force a pole to s=0 
      % above because it adjusts the other pole to preserve the sum of the two
      pD = sum(Poles);
   end
else
   if abs(prod(Poles-1))<1e4*eps || any(wp<1e-8*wmax)
      % There is at least one pole near z=1
      pI = 1;
   end
   if numel(Poles)==1+numel(pI)
      % Note: This approach has superior accuracy when we force a pole to z=1 
      % above because it adjusts the other pole to preserve the product
      pD = prod(Poles);
   end
end

% Check number of poles
if numel(pI)+numel(pD)~=numel(Poles)
   error(message('Control:ltiobject:pidOperations2'))
end
wp = damp([pI;pD],Ts);

% Construct frequency grid to test accuracy of PID approximation
wzp = sort([wz;wp]);
wzp = wzp(wzp>0 & wzp<Inf,:);
nw = 20;
if Ts==0
   if isempty(wzp)
      w = logspace(-3,3,nw);
   else
      logwmax = log10(wmax);
      w = logspace(max(logwmax-12,log10(wzp(1))-2),logwmax+2,nw);
   end
   s = complex(0,w');
else
   % Note: Do not include pi/Ts because response can be zero there (zero at z=-1)
   lognf = log10(3);
   if isempty(wzp)
      w = logspace(lognf-8,lognf,nw);
   else
      w = logspace(max(lognf-12,log10(wzp(1)*Ts)-2),lognf,nw);
   end
   s = exp(complex(0,w'));
end

% Compute Tf and coefficients for I term and D term
[NumI,DenI] = getNumDenITerm(Ts,IFormula);
[NumD,DenD,Tf] = getNumDenDTerm(Ts,DFormula,pD);
if Tf<0 || isinf(Tf) || ~isreal(Tf)
   % Tf must be positive and finite
   error(message('Control:ltiobject:pidOperations2'))
end

% Form least-squares fit problem
h = zpkfresp(Zeros,[pI pD],Num(end-numel(Zeros)),s,true);
hI = polyval(NumI,s)./polyval(DenI,s);
hD = polyval(NumD,s)./polyval(DenD,s);
A = [1./h hI./h hD./h];
b = ones(nw,1);

% Solve for gains Ki,Kp,Kd
% Note: 1) Use frequency response fit for meaningful measure of fit error
%       2) Eliminate variables known to be zero to avoid rank-deficient A
rtol = 1e-4;  % max relative residual
inz = find([true ~isempty(pI) ~(isempty(pD) && numel(wz)<=numel(wp))]);
Gains = zeros(3,1);
Gains(inz) = fitGains(A(:,inz),b,rtol);
if max(abs(A*Gains-b))>rtol
   error(message('Control:ltiobject:pidOperations2'))
end
Kp = Gains(1);
Ki = Gains(2);
Kd = Gains(3);
end


%% local function --------------------------------------
function g = fitGains(A,b,rtol)
% Solves A*g=b in least-squares sense, seeking small-norm solution  
% compatible with max allowed residual RTOL.
% NOTE: Can't reliably estimate the rank of A because of wide-ranging column 
% norms. In addition, near pole/zero cancellations make the least-squares
% problem underspecified and give rise to large-gain solutions (g913671).
n = size(A,2);

% QR factorization with pivoting
[q,r,perm] = qr(A,0);
b = q'*b;

% Compute smallest gain values compatible with max residual RTOL
x0 = zeros(n,1);
for k=n:-1:1
   beta = b(k)-r(k,k+1:n)*x0(k+1:n,:);
   if abs(beta)>rtol
      % X0(K) is nonzero only when residual exceeds RTOL
      x0(k) = real(beta/r(k,k));  % could be Inf
   end
end
gmax = 1e3*max(abs(x0));  % computed gains should not exceed this value

% Repeat with stricter tolerance on residuals to improve fit without
% significantly increasing the PID gains
x = zeros(n,1);
for k=n:-1:1
   beta = b(k)-r(k,k+1:n)*x(k+1:n,:);
   tau = real(beta/r(k,k));  
   if abs(beta)>1e-8 && abs(tau)<gmax
      % X(K) is set to zero when it exceeds GMAX (amounts to dropping rank)
      % or when the residual is negligible (avoids tiny gain values)
      x(k) = tau;
   end
end

% Return gain vector
g = zeros(n,1);
g(perm(1:n)) = x;

end


function [Num,Den] = getNumDenITerm(Ts,Formula)
% Returns transfer function of integrator 1/s or its discrete-time equivalent.
if Ts==0
   Num = [0 1];
   Den = [1 0];
else
   switch Formula(1)
      case 'F'
         Num = [0 Ts];
      case 'B'
         Num = [Ts 0];
      case 'T'
         Num = [Ts/2 Ts/2];
   end
   Den = [1 -1];
end
end

function [Num,Den,Tf] = getNumDenDTerm(Ts,Formula,pD)
% Returns transfer function of derivative term s/(tau*s+1) or its discrete-time
% equivalent. PD specifies the root of tau*s+1.
if isempty(pD)
   % Pure derivative
   Tf = 0;
   if Ts==0
      Num = [1 0];
      Den = [0 1];
   else
      % Note: Will fail for 'B' and 'T', but don't just return Num,Den for 'F'
      switch Formula(1)
         case 'F'
            Num = [1 -1]/Ts;   Den = [0 1];
         case 'B'
            Num = [1 -1]/Ts;   Den = [1 0];
         case 'T'
            Num = (2/Ts)*[1 -1];   Den = [1 1];
      end
   end
else
   if Ts==0
      Tf = -1/pD;
      Num = [-pD 0];
      Den = [1 -pD];
   else
      alpha = (1-pD)/Ts;
      switch Formula(1)
         case 'F'
            Tf = 1/alpha;
         case 'B'
            Tf = pD/alpha;
         case 'T'
            Tf = (1+pD)/2/alpha;
      end
      Num = [1 -1]*alpha;
      Den = [1 -pD];
   end
end
end

