
function res_cho = plotLee
clc
clear all
% close all




%%
% s = tf('s');
 G=model;
MKS =[];
MT=[];
Jvec=[];
stable=[];
i=1;
lambda_vec = 3:0.1:10;
iae_weights=[   1.1651    0.0046];
%  iae_weights=[  1  1];
for i=1:length(lambda_vec)
    Tauc=lambda_vec(i);
% Tuning for IMC design
%for Tauc = 
[C] = pid_lee(G,Tauc);
%G = Gn;%*exp(-1*s);
C
%
S = 1/(1+C*G);
marg = allmargin(G*C);
stable(i) =  marg.Stable;


minreal(C*S);

MKS(i) = getPeakGain(C*S,1e-8);

MT(i) = getPeakGain(G*C*S,1e-8);


    
     GS = G*S;
     sys = [S;GS];
     [e,t]=step(sys,150);
     iae_tuning = trapz(t,(e).^2)';
     


     J = [0.5 0.5]*abs((iae_tuning./iae_weights'));
     Jvec(i)=J;



end


%%
% Tauc_space=[ 8 9 10 ];
% MKSr =[];
% MTr=[];
% Jvecr=[];
% stable=[];
% i=1;


% iae_weights=[   1.1651    0.0046];

% for Tauc=Tauc_space
%     
% % Tuning for IMC design
% %for Tauc = 
% [Kc,Ki,Kd,Tf,C] = PID_IMC(G,Tauc);
% C = tf(C);
% % K=ctrl([ -15.4758    0.0686    8.5541    4.0000]);
% S = 1/(1+C*G);
% marg = allmargin(G*C);
% stable(i) =  marg.Stable;
% 
% 
% MKSr(i) = getPeakGain(C*S,1e-8);
% MTr(i) = getPeakGain(G*C*S,1e-8);
% 
% 
%     
%     
%      S = 1/(1+G*C);
%      GS = G/(1+G*C);
%      sys = [S;GS];
%      [e,t]=step(sys,150);
%      iae_tuning = trapz(t,(e).^2)';
% 
%      J = [0.5 0.5]*abs((iae_tuning./iae_weights'));
%        Jvecr(i)=J;
% 
% 
% i=i+1;
% 
% end


figure(3)
plot3(MKS,MT,Jvec,'b')
res_cho.ks=MKS;
res_cho.mt=MT;
res_cho.cost=Jvec;

return
%bode((1/(1+G*C)+G*C/(1+G*C)-1)/(2/(1+G*C)*(G*C/(1+G*C)-1)))

%combined_peak = (ms_peak+mt_peak-1)/(2*ms_peak*(mt_peak-1))
%%
% lambda = 1.01; % Tuning for loop-shaping design
% sys_s =  -C_PID*sys4; % Shaped plant by IMC Controller
% [Ac,Bc,Cc,Dc,gammin] = coprimeunc(sys_s.a,sys_s.b,sys_s.c,sys_s.d,lambda);
% Ks = ss(Ac,Bc,Cc,Dc);
% C_Hinf = minreal(C_PID*Ks);
% zpk(C_Hinf)

%%
% L1 = sys4*C_PID;
% S1 = 1/(1+L1);
% T1 = 1 - S1;
% R1 = C_PID*S1;
% 
% disp('Margins of PID:')
% allmargin(L1)
% 
% L2 = sys4*C_Hinf;
% S2 = 1/(1+L2);
% T2 = 1 - S2;
% R2 = C_Hinf*S2;
% disp('Margins of Hinf:')
% allmargin(L2)

%%

% omega = logspace(-3,2,100);
% [mag,phase] = bode(C_PID,omega);
% mag1 = reshape(mag(1,1,:),100,1);
% phase1 = reshape(phase(1,1,:),100,1);
% 
% [mag,phase] = bode(C_Hinf,omega);
% mag2 = reshape(mag(1,1,:),100,1);
% phase2 = reshape(phase(1,1,:),100,1);
% 
% figure(1)
% clf
% rect = [0, 0, 18, 12];
% set(gcf,'Color',[1,1,1],'PaperUnits','centimeters','PaperSize',[18 12],'PaperPosition',rect)
% subplot(2,1,1)
% set(gca,'FontSize',10,'OuterPosition',[0  1/2  1  1/2])
% semilogx(omega,mag1,'b',omega,mag2,'r','Linewidth',1);
% ylabel('Mag [-]','FontSize',12,'FontName','Times')
% %xlabel('\omega [Rad/s]','FontSize',10,'FontName','Times')
% xlim([1e-2 10])
% legend('PID','H_{\infty} Loop-shaping',1,'orientation','horizontal')
% 
% subplot(2,1,2)
% set(gca,'FontSize',10,'OuterPosition',[0  0  1  1/2])
% semilogx(omega,phase1,'b',omega,phase2,'r','Linewidth',1);
% ylabel('Phase [deg]','FontSize',12,'FontName','Times')
% xlabel('\omega [Rad/s]','FontSize',12,'FontName','Times')
% legend('PID','H_{\infty} Loop-shaping',4,'orientation','horizontal')
% xlim([1e-2 10])
%%

% for i=1:length(omega)
%     S1f(i) = abs(freqresp(S1,omega(i)));
%     T1f(i) = abs(freqresp(T1,omega(i)));
%     R1f(i) = abs(freqresp(R1,omega(i)));
%     
%     S2f(i) = abs(freqresp(S2,omega(i)));
%     T2f(i) = abs(freqresp(T2,omega(i)));
%     R2f(i) = abs(freqresp(R2,omega(i)));
%     
% end
% disp('PID peaks')
% disp('Peak of S:')
% disp(max(S1f))
% disp('Peak T:')
% disp(max(T1f))
% disp('Peak of KS:')
% disp(max(R1f))
% 
% disp('Hinf peaks')
% disp('Peak of S:')
% disp(max(S2f))
% disp('Peak T:')
% disp(max(T2f))
% disp('Peak of KS:')
% disp(max(R2f))
% 
% figure(2)
% clf
% rect = [0, 0, 18, 18];
% set(gcf,'Color',[1,1,1],'PaperUnits','centimeters','PaperSize',[18 18],'PaperPosition',rect)
% subplot(3,1,1)
% set(gca,'FontSize',10,'OuterPosition',[0  2/3  1  1/3])
% semilogx(omega,S1f,'b',omega,S2f,'r','Linewidth',1)
% legend('PID','H_{\infty} Loop-shaping',4,'orientation','horizontal')
% title('Sensitivity transfer function','FontSize',12,'FontWeight','normal','FontName','Times');
% ylabel('|\itS\rm|','FontSize',12,'FontName','Times')
% %xlabel('\omega [Rad/s]','FontSize',10,'FontName','Times')
% xlim([1e-2 10])
% 
% subplot(3,1,2)
% set(gca,'FontSize',10,'OuterPosition',[0  1/3  1  1/3])
% semilogx(omega,T1f,'b',omega,T2f,'r','Linewidth',1)
% legend('PID','H_{\infty} Loop-shaping',3,'orientation','horizontal')
% title('Complementary sensitivity transfer function','FontSize',12,'FontWeight','normal','FontName','Times');
% ylabel('|\itT\rm|','FontSize',12,'FontName','Times')
% %xlabel('\omega [Rad/s]','FontSize',10,'FontName','Times')
% xlim([1e-2 10])
% 
% subplot(3,1,3)
% set(gca,'FontSize',10,'OuterPosition',[0  0  1  1/3])
% semilogx(omega,R1f,'b',omega,R2f,'r','Linewidth',1);
% title('Input usage','FontSize',12,'FontWeight','normal','FontName','Times');
% legend('PID','H_{\infty} Loop-shaping',4,'orientation','horizontal')
% xlabel('\omega [Rad/s]','FontSize',12,'FontName','Times')
% ylabel('|\itKS\rm|','FontSize',12,'FontName','Times')
% xlim([1e-2 10])