% function [A0,A1,A2,A3,A4,A5,Kp_MSP1,Ti_MSP1,Kp_MSP2,Ti_MSP2,Km_MSP,a1_MSP, a2_MSP, L_MSP] = msptune (y,u,t,T0,T1,Tint,Tfin,Ts,Lx);
%
% function msptune.m calculates areas from the process step response. 
% It also calculates two PI controller parameters based on chosen time Lx.
%
% y - process response (one column!)
% u - process input (one column!)
% t - time vector (one column!)
% T0 - a start of experiment before the change of control variable 
%      (for the calculation of starting mean value)
% T1 - time instant when control variable changes
% Tint - integration time
% Tfin - a time till when the final mean value is calculated
% Ts - sampling time
% Lx - Time delay of partially delayed process model
%
% Kp, Ti, Td - controller parameters
% A0..A5 - areas calculated from the process response
% alphad - an intermediate variable
% 
% For more details about parameters see paper Vrancic, Peng and Strmcnik:
% A new PID controller tuning method based on multiple integrations, 
% Control Enginnering Practice, Vol. 7, pp. 623-633 (1999) and
% Vrancic, Vrecko, Strmcnik and Juricic : 
% Automatic tuning of the flexible Smith predictor controller. 
% 1999 American Control Conference, June 2-4, 1999, 
% Hyatt Regency San Diego, San Diego, pp. 3853-3857


function [A0,A1,A2,A3,A4,A5,Kp_MSP1,Ti_MSP1,Kp_MSP2,Ti_MSP2,Km_MSP,a1_MSP, a2_MSP, L_MSP] = msptune (y,u,t,T0,T1,Tint,Tfin,Ts,Lx);

a = max(size(y));

i2 = a;
while (t(i2) > Tfin)
  i2 = i2-1;
end

i1 = i2;
while (t(i1) > Tint)
  i1 = i1-1;
end

j1 = 1;
while (t(j1) < T0)
  j1 = j1+1;
end

j2 = j1;
while (t(j2) < T1)
  j2 = j2+1;
end

[j1 j2 i1 i2];

ya0=mean(y(j1:j2));
ya1=mean(y(i1:i2));
ua0=mean(u(j1:j2-1));
ua1=mean(u(i1:i2));

DU = ua1-ua0;

A0 = (ya1-ya0)/DU;
Kpr = A0;
y = (y-ya0)/DU;
u = (u-ua0)/DU;

% Integration by using trapezoidal rule

Iu1 = [0;Ts*cumsum((u(j2:i1-1)+u(j2+1:i1))/2)];
Iu2 = [0;Ts*cumsum((Iu1(1:i1-j2)+Iu1(2:i1-j2+1))/2)];
Iu3 = [0;Ts*cumsum((Iu2(1:i1-j2)+Iu2(2:i1-j2+1))/2)];
Iu4 = [0;Ts*cumsum((Iu3(1:i1-j2)+Iu3(2:i1-j2+1))/2)];
Iu5 = [0;Ts*cumsum((Iu4(1:i1-j2)+Iu4(2:i1-j2+1))/2)];

Iy1 = [0;Ts*cumsum((y(j2:i1-1)+y(j2+1:i1))/2)];
Iy2 = [0;Ts*cumsum((Iy1(1:i1-j2)+Iy1(2:i1-j2+1))/2)];
Iy3 = [0;Ts*cumsum((Iy2(1:i1-j2)+Iy2(2:i1-j2+1))/2)];
Iy4 = [0;Ts*cumsum((Iy3(1:i1-j2)+Iy3(2:i1-j2+1))/2)];
Iy5 = [0;Ts*cumsum((Iy4(1:i1-j2)+Iy4(2:i1-j2+1))/2)];

y1 = (A0*Iu1-Iy1);
A1 = y1(i1-j2);
y2 = (A1*Iu1-A0*Iu2+Iy2);
A2 = y2(i1-j2);
y3 = (A2*Iu1-A1*Iu2+A0*Iu3-Iy3);
A3 = y3(i1-j2);
y4 = (A3*Iu1-A2*Iu2+A1*Iu3-A0*Iu4+Iy4);
A4 = y4(i1-j2);
y5 = (A4*Iu1-A3*Iu2+A2*Iu3-A1*Iu4+A0*Iu5-Iy5);
A5 = y5(i1-j2);

L_MSP = min(real(roots([1/6,-A1/(2*Kpr),A1^2/Kpr^2-A2/Kpr,2*A1*A2/Kpr^2-A3/Kpr-A1^3/Kpr^3])));

a1_MSP = A1/Kpr-L_MSP;
a2_MSP = -A2/Kpr+A1*a1_MSP/Kpr+L_MSP^2/2;

if (abs(a2_MSP) < 1e-6)
   a2_MSP = 0;
end;

Lm = L_MSP;

A1m = A1-Lm*Kpr;
A2m = A2-A1*Lm+Kpr*Lm*Lm/2;
A3m = A3-A2*Lm+A1*Lm*Lm/2-Kpr*Lm*Lm*Lm/6;
 
alpha = (A1m*A2m)/(Kpr*A3m)-1;

Kpmax = 2;

if (alpha < 0.5/Kpmax) 
   alpha = 0.5/Kpmax;
end
 
Kp_MSP1 = 0.5/(Kpr*alpha);
Ti_MSP1 = A1m/(Kpr*(1+alpha));

Lm = L_MSP-Lx;

A1m = A1-Lm*Kpr;
A2m = A2-A1*Lm+Kpr*Lm*Lm/2;
A3m = A3-A2*Lm+A1*Lm*Lm/2-Kpr*Lm*Lm*Lm/6;

alpha = (A1m*A2m)/(Kpr*A3m)-1;

if (alpha < 0.5/Kpmax) 
   alpha = 0.5/Kpmax;
end
 
Kp_MSP2 = 0.5/(Kpr*alpha);
Ti_MSP2 = A1m/(Kpr*(1+alpha));

Km_MSP = Kpr;