%% Tuning calculations for the three different techniques gathered in this matlab script.

clc
clear all
close all
warning off

global num
global den
global theta

step_tid=5;

% Defining the identified model
% "Actual model" - simple model jahanshahi 
% b1 = -0.01085;
% b0 = -0.002689;
% a1 = -0.02717;
% a0 = 0.02611;
% 
% 
% 
% % % Identified optimisation lab
% b1 = -0.009762;
% b0 = - 0.004175;
% a1 = 1e-08 ;
% a0 =   0.01178;

theta=0;

% sys=tf(num,den);
sys=model;
[num, den]=tfdata(sys);
num=num{1};
den=den{1};

b1=num(2);
b0=num(3);
a1=den(2);
a0=den(3);
%  num = [b1 b0];
%  den = [1 a1 a0];

%% Jahanshahi
disp('#########################');
disp('JAHANSHAHI TUNING CALCULATION:');
disp('  ')
disp('Controller structure:')
disp('C=Kc+Ki/s+((Kd/Tf)/(1+1/(Tf*s))')
disp('  ')


[z,p,K_p] = tf2zpk(num,den);
fi=(b0/b1);

syms h
F=h^2+a1*h+a0;

d = solve(F,h);%,'MaxDegree',4)
h = double(d);
pi_1=h(1);
pi_2=h(2);

% Defining filter equations 
% f1, f2 and f0 are per (my) definition the filter coefficients (would be confusing with use of the symbol a)
% Lambda shall be as large as possible and still have negative gain
lambda=8;


%5;%0.051;%5;   %(For aCTUAL MODEL LAMBDA=5)
n=3;  % Parameter dependent on the order of the model

syms f1 f2 

F1 = pi_1*f1 + (pi_1^2)*f2 - ((lambda*pi_1+1)^n) +1;
F2 = pi_2*f1 + (pi_2^2)*f2 - ((lambda*pi_2+1)^n) +1;

b=solve(F1,F2,f1,f2);

f1 = double(b.f1);
f2 = double(b.f2);

% Designing the controller

s=tf('s');

% Innsatt i likning nyeste artikkelen
 num_cont = (1/(K_p*lambda^3))*(f2*s^2+(f1)*s+(1));
 den_cont = s*(s+fi);


contr=num_cont/den_cont;   % COntroller transfer function

[Kc,Ki,Kd,Tf]= piddata(contr);

% val(1)=Kc;
% val(2)=(Kc/Ki)/60;
% val(3)=(Kd/Kc)/60;
% val(4)=Tf/(Kd/Kc);

str1_es = ['Controller Gain: ' num2str(Kc)];
disp(str1_es)
str2_es = ['Integral gain: ' num2str(Ki)];
disp(str2_es)
str3_es = ['Derivative gain: ' num2str(Kd)];
disp(str3_es)
str4_es = ['Tf= ' num2str(Tf) ];
disp(str4_es)
disp(['Lambda equals:   Lambda=' num2str(lambda)])
disp('  ')
disp('#########################');





% str1_es = ['Controller Gain: ' num2str(val(1))];
% disp(str1_es)
% str2_es = ['Integral Time  min: ' num2str(val(2))];
% disp(str2_es)
% str3_es = ['Derivative Time min: ' num2str(val(3))];
% disp(str3_es)
% str4_es = ['Alpha: ' num2str(val(4)) ];
% disp(str4_es)
% disp('  ')
% disp('#########################');
s = tf('s');
G_mod = (b1*s+b0)/(s^2 + a1*s+a0); 
C_es=Kc+(Ki/s)+((Kd/Tf)/(1+(1/(Tf*s))));

L=G_mod*C_es;

disp('Diverse:')
DD=allmargin(L)



sim('IMC_test_esmaeil2')

load yvanlig.mat
load Rvanlig.mat

% figure(1)
% plot(y_imc_es(1,:),y_imc_es(2,:))
% hold on
% plot(R_es(1,:),R_es(2,:),'r')

% %% Optimisation based on jahanshahi
% X0= [Kc Ki Kd Tf];
% 
% % Formulating the constraints as the matrix inequality A·x <= b, where
% A=[];%diag([-1 -1 -1 -1 -1 -1 -1]);
% b=[];%[0 0 0 0 0 0 0]';
% Aeq=[];
% beq=[];
% lb=[ -Inf -Inf -Inf -Inf];%[0 0.00001 0.00001 0 0.00001 0.00001 0];%0.001*ones(1,7);%zeros(1,7);
% ub=[100 10001 100 100];
% nonlcon = [];
% 
% % Optimization function
% options = optimset('Algorithm','sqp','TolFun',1e-8,'TolX',1e-8);%,'Display','iter'); % run interior-point algorithm
% %bruk 10e-5
% %X = fminunc(@cost_function_PID,X0,options); % Unconstrained optimization
% X=fmincon(@cost_function_PID,X0,A,b,Aeq,beq,lb,ub,nonlcon,options)
% %[X,iaeD, exitFlagD]    =  fmincon(@cost_function, X0, [],[],[],[],[0;0],[],nonlcon,options); % tilsvarer x,fval,exitflag] = fmincon(...)
% 
% Kc_opt = X(1); 
% Ki_opt = X(2); 
% Kd_opt = X(3); 
% Tf_opt = X(4);
% 
% sim('IMC_test_esmaeil_optimisation')
% 
% load y_es_tun_opt.mat
% load R_es_tun_opt.mat
% 
% % figure(1)
% % plot(y_imc_es_opt(1,:),y_imc_es_opt(2,:))
% % hold on
% % plot(R_es_opt(1,:),R_es_opt(2,:),'r')
% % 
% % return
% 

%% Cho

disp('CHO TUNING CALCULATION:');
disp('  ')
disp('Controller structure:')
disp('  ')
disp('C=Kc(1 +( 1/(tau_i* s))+ tau_d* s)*((lambda *s +1)^2/(beta_2 * s^2 + beta_1*s +1))')


lambda = 5;%2.1709e-07;%1;  %(For aCTUAL MODEL LAMBDA=1)
syms beta1
syms beta2

F1_cho = (3*(lambda^2)+theta*beta1-beta2)/((lambda^3)+theta*beta2)-a1;
F2_cho = (3*lambda+theta-beta1)/(lambda^3+theta*beta2)-a0;

s = solve(F1_cho,F2_cho,beta1,beta2);
beta1 = double(s.beta1);
beta2 = double(s.beta2);


%s=tf('s');
%C=(beta1/(b0*(lambda^3+theta*beta2)))*(1+(1/(beta1*s))+((beta2*s)/beta1));
% Filter

%sys=feedback(F,C);

% PID - Tuning values

kc=beta1/(b1*(lambda^3+theta*beta2));
tau_i=beta1;
tau_d=beta2/beta1;
I=kc/tau_i;
D=kc*tau_d;

% Set-point filter

%numf = [lambda 1];
numf = [lambda^2 2*lambda 1];
denf = [beta2 beta1 1];

%F=(lambda*s+1)/(beta2*s^2+beta1*s+1)


val(5)=kc;
val(6)=I;
val(7)=D;
%val(8)=tf(numf,denf);
filter_transfer_function_cho_tuning=tf(numf,denf);
s = tf('s');
C_cho =kc*(1 +( 1/(tau_i*s))+ tau_d*s)*((lambda*s +1)^2/(beta2*s^2 + beta1*s+1));
L_cho = sys*C_cho;
allmargin(L_cho)

disp('  ')
str1_es = ['Controller Gain: ' num2str(val(5))];
disp(str1_es)
str2_es = ['Integral Gain: ' num2str(val(6))];
disp(str2_es)
str3_es = ['Derivative Gain: ' num2str(val(7))];
disp(str3_es)
disp('Filter transfer function: ')
filter_transfer_function_cho_tuning
str324_es = ['Lambda: ' num2str(lambda)];
disp(str324_es)
str3241_es = ['beta1: ' num2str(beta1)];
disp(str3241_es)
str3242_es = ['BETA2: ' num2str(beta2)];
disp(str3242_es)




disp('#########################');
sim('IMC_cho_tun')

load y_cho_tun.mat
load R_cho_tun.mat


% figure(2)
% plot(y_imc_cho(1,:),y_imc_cho(2,:),'k','LineWidth',1)
% hold on
% plot(R_cho(1,:),R_cho(2,:),'r','LineWidth',1)
% 
% legend('Process Output','Set-point','Location','NorthEast')




%% Chidambaram

disp('CHIDAMBARAM TUNING CALCULATION:');
disp('  ')
disp('Controller structure:')
disp('  ')
disp('C=Kc(1 +( 1/(tau_i* s))+ tau_d* s)*((alpha*s +1)/(beta*s +1))')

b_0=-b0;
b_1=-b1;
alpha_0=a0;
alpha_1=-a1;
d_m=theta+0.1;
% Beregning av tuning parametre under her
% USing the method from chidabram...:

Kp  = -b_0/alpha_0;%-0.2613;
p   = -b_1/b_0;%-2.7021;
a_2 = 1/alpha_0;%29.825;
a_1 = -alpha_1/alpha_0;%-6.0424;

y_1  = -a_2^2+0.5*d_m*p*a_2;
y_2  = -0.5*d_m*p*a_1-a_2*p-0.5*d_m*a_2;
y_3  = -a_1*a_2-a_2*p-0.5*d_m*a_2;
y_4  =-0.5*d_m*p+a_2;

% Initial values for the while loop
lambda=10;   %Tuning parameter  %(For aCTUAL MODEL LAMBDA=3)3 for lab , 5 greit ellers



%while d_1<0 & d_2<0 & c_4<0 & c_3<0  & c_0<0 

z_1=a_2*lambda^3+1.5*d_m*a_2*lambda^2-0.5*d_m*a_1*lambda^3-3*lambda*a_2^2-d_m*a_2^2-p*a_2^2;
z_2=3*a_2*lambda^2+1.5*d_m*a_2*lambda-0.5*p*d_m*a_2-0.5*d_m*lambda^3-3*a_1*a_2*lambda-a_1*a_2*d_m-a_1*a_2*p;

n_2=(y_3*z_1-y_1*z_2)/(y_2*y_3-y_1*y_4);
n_1=(y_2*z_2-z_1*y_4)/(y_2*y_3-y_1*y_4);

h=3*lambda+d_m-n_1+p;
x_1=(0.5*d_m*lambda^3-0.5*d_m*n_2*p)/h;
x_2=(lambda^3+1.5*d_m*lambda^2+n_2*p+0.5*d_m*n_2-0.5*d_m*n_1*p)/h;
x_3=(3*lambda^2+1.5*d_m*lambda-n_2+n_1*p+0.5*d_m*n_1-0.5*p*d_m)/h;

beta=x_1/a_2;

% Defining controller transfer function

s=tf('s');

num_imc_in=(a_2*s^2 +a_1*s +1)*(n_2*s^2+n_1*s+1)*(1+0.5*d_m*s);
den_imc_in=Kp*h*s*(x_1*s^3+x_2*s^2+x_3*s+1);

% Simulering av IMC i simulink

%pid parameteres
kc_imc=n_1/(Kp*h);
tau_i=n_1;
tau_d=n_2/n_1;
alpha=0.5*d_m;
Integral=kc_imc/tau_i;
Derivative=kc_imc*tau_d;


% Stability requirements
%disp('Alle de følgende parametrene skal være større enn null')
c_4 = a_2*beta*tau_i+0.5*d_m*tau_i*tau_d*kc_imc*Kp*p;
c_3 = a_1*beta*tau_i+a_2*tau_i-0.5*d_m*tau_i*tau_d*Kp*kc_imc+0.5*d_m*tau_i*Kp*kc_imc*p-tau_i*tau_d*Kp*kc_imc*p;
c_2 =tau_i*beta+a_1*tau_i-0.5*d_m*tau_i*Kp*kc_imc+tau_i*tau_d*Kp*kc_imc+0.5*d_m*Kp*kc_imc*p-tau_i*Kp*kc_imc*p;
c_1 =tau_i-0.5*d_m*Kp*kc_imc+tau_i*Kp*kc_imc-Kp*kc_imc*p;
c_0 =Kp*kc_imc;

d_1 = ((c_1*c_4)-(c_2*c_3))/c_3;
d_2 = ((c_0*(c_3^2)) - ((c_1^2)*c_4)+(c_1*c_2*c_3))/((c_1*c_4)-(c_2*c_3));

%lambda=lambda+0.01;


%disp('lambda are then given as:')
%disp(lambda)
val(8)=kc_imc;
val(9)=Integral;
val(10)=Derivative;

disp('  ')
str1_es = ['Controller Gain: ' num2str(val(8))];
disp(str1_es)
str2_es = ['Integral Gain: ' num2str(val(9))];
disp(str2_es)
str3_es = ['Derivative Gain: ' num2str(val(10))];
disp(str3_es)
str3_es = ['alpha: ' num2str(alpha)];
disp(str3_es)
str3_es = ['beta: ' num2str(beta)];
disp(str3_es)

s = tf('s');
C_chi = kc_imc*(1 +( 1/(tau_i*s))+ tau_d* s)*((alpha*s +1)/(beta*s +1));
L_chi = sys*C_chi;
allmargin(L_chi)

% disp('Filter transfer function: ')
% tf([alpha 1],[beta 1])
disp('#########################');



% New simulation
sim('IMC_chidambaram')

load y_ch_tun.mat   
load R_ch.mat   

% plot
% figure(3)
% plot(y_imc_ch_tun(1,:),y_imc_ch_tun(2,:),'LineWidth',0.5)
% hold on
% plot(R_ch_tun(1,:),R_ch_tun(2,:), 'r','LineWidth',0.5)


%% Plot fil

figure(1)
hold on
plot(R_ch_tun(1,:),R_ch_tun(2,:), 'k','LineWidth',1)
plot(y_imc_ch_tun(1,:),y_imc_ch_tun(2,:),'r--','LineWidth',3)
plot(y_imc_cho(1,:),y_imc_cho(2,:),'g-.','LineWidth',3)
plot(y_imc_es(1,:),y_imc_es(2,:),'b:','LineWidth',3)
%plot(y_imc_es_opt(1,:),y_imc_es_opt(2,:),'c','LineWidth',3)
ylabel('Step Change','FontSize',16,'FontName','Baskerville Old Face')
xlabel('Time','FontSize',16,'FontName','Baskerville Old Face')
legend('Set-point Change','Chidambaram','Cho','Jahanshahi','Location','NorthEast')
xlim([0 150])
ylim([0 1.4])


%% Quality control




