function [Kc,Ki,Kd,Tf,C] = PID_IMC(G,Tauc)
%PID_IMC Summary of this function goes here
% This function give PID tuning in the form of C = Kc + Ki/s + Kd*s/(Tf*s+1)
% The model must be second-order and minimum-phase

% Inputs
% G: Unstable minimum-phase model
% Tauc: Tuning parameter, time constant of the closed-loop system

% Outputs
% Kc: Proportional gain
% Ki: Integral gain
% Kd: Derivative gain
% Tf: Filter time constant
% C: IMC controller

% Author: R. Chen
% Modified by: Esmaeil Jahanshahi
% For a general TF use [C, q, tauc] = TuningIMC(Ge,Tauc), but here is for
% a minimum-phase second-order process that gives a PID controller

% Plan Information
[Zero,Pole,Gain,Ts] = zpkdata(G,'v');

indRHPzero = (real(Zero)>0);                        % indices of open RHP zeros
indRHPpole = (real(Pole)>0);                        % indices of open RHP poles
RHPpoles = Pole(indRHPpole);                        % RHP poles
NumRHPzeros = sum(indRHPzero);                      % number of open RHP zeros
NumRHPpoles = sum(indRHPpole);                      % number of open RHP poles

% for MP systems
q_tilde = zpk(Pole,Zero,1/Gain);

k = NumRHPpoles+1; % since Vm always contains an pole at origin for step input
m = max(length(zero(q_tilde))-length(pole(q_tilde)),1); % make sure q=q_tilde*f is proper
filterOrder = m+k-1;
% 3. calculate filter as sum(a(k)s^k)/(tau*s+1)^filterOrder
coefficients = ones(1,k);

if NumRHPpoles>0
    A = zeros(NumRHPpoles,NumRHPpoles);
    for ctRHPpole = 1:length(RHPpoles)
        A(ctRHPpole,:) = RHPpoles(ctRHPpole).^(1:NumRHPpoles);
    end
    b = (Tauc*RHPpoles+1).^filterOrder-coefficients(1);
    coefficients(2:end) = (real(A\b))';
end
% compute f
num = fliplr(coefficients);
den = fliplr(poly(repmat(-Tauc,1,filterOrder)));
f = tf(num,den);

q = minreal(q_tilde*f);
C = feedback(q,G,+1);
C = minreal(C);

alpha1 = coefficients(2);
alpha2 = coefficients(3);

Tf = 1/abs(Zero);
Ki = Tf/(Gain*Tauc^3);
Kc = Ki*alpha1 - Ki*Tf;
Kd = Ki*alpha2 - Kc*Tf;

end

