function K=loop_shape(Ge,lambda,tauc)

num = Ge.num{1};
den = Ge.den{1};

b1 = num(2);
b0 = num(3);

a1 = -den(2);
a0 = den(3);

A = [a1 1 ; -a0 0];
B = [b1 b0]';
C = [1 0];
D = 0;
sys=ss(A,B,C,D);
set(sys,'inputname','Z','outputname','P1','statename',{'x1','x2'});
present(sys);
%% IMC Controller
[Kc,Ki,Kd,Tf,C] = PID_IMC(Ge,tauc);
disp('IMC controller')
zpk(C)



%% H-infinity loop-shaping
sys_s =  -C*sys; % Shaped plant by IMC Controller
% Finds the controller which optimally robustifies a given
% shaped plant in terms of tolerating maximum coprime uncertainty.
% RANGE TUNING: > 1  && < 1.2  (smaller the better)
[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);
K = minreal(C*Ks);
%K3 = balancmr(K3,3);
K=tf(K);


% omega = logspace(-3,2,100);
% [mag,phase] = bode(K,omega);
% mag1 = reshape(mag(1,1,:),100,1);
% phase1 = reshape(phase(1,1,:),100,1);
% 
% [mag,phase] = bode(C,omega);
% mag2 = reshape(mag(1,1,:),100,1);
% phase2 = reshape(phase(1,1,:),100,1);
% 
% [mag,phase] = bode(K3,omega);
% mag3 = reshape(mag(1,1,:),100,1);
% phase3 = 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])
% loglog(omega,mag2,'b',omega,mag1,'r',omega,mag3,'g','Linewidth',1);
% ylabel('Mag [-]','FontSize',12,'FontName','Times')
% %xlabel('\omega [Rad/s]','FontSize',10,'FontName','Times')
% xlim([1e-2 10])
% legend('IMC','H_{\infty} Mixed-sensitivity','H_{\infty} Loop-shaping',1,'orientation','horizontal')
% 
% subplot(2,1,2)
% set(gca,'FontSize',10,'OuterPosition',[0  0  1  1/2])
% semilogx(omega,phase2,'b',omega,phase1,'r',omega,phase3,'g','Linewidth',1);
% ylabel('Phase [deg]','FontSize',12,'FontName','Times')
% xlabel('\omega [Rad/s]','FontSize',12,'FontName','Times')
% legend('IMC','H_{\infty} Mixed-sensitivity','H_{\infty} Loop-shaping',4,'orientation','horizontal')
% xlim([1e-2 10])
% 
% %print -depsc C:\Users\jahansha\Documents\Esmaeil_Thesis\Figures_Chapter5\Three_Cont
% %%
% L1 = sys*(K);
% S1 = 1/(1+L1);
% T1 = 1 - S1;
% R1 = K*S1;
% SG1 = S1*sys;
% disp('Poles of sensitivity transfer function of H-inf Mix-synth for P1')
% pole(S1)
% 
% [mag,phase] = bode(L1,omega);
% mag_L1 = reshape(mag,length(omega),1);
% phase_L1 = reshape(phase,length(omega),1);
% 
% allmargin(L1)
% 
% 
% L2 = sys*C;
% S2 = 1/(1+L2);
% T2 = 1 - S2;
% R2 = C*S2;
% disp('Poles of sensitivity transfer function of IMC for P1')
% pole(S2)
% 
% [mag,phase] = bode(L2,omega);
% mag_L2 = reshape(mag,length(omega),1);
% phase_L2 = reshape(phase,length(omega),1);
% 
% allmargin(L2)
% 
% 
% L3 = sys*K3;
% S3 = 1/(1+L3);
% T3 = 1 - S3;
% R3 = K3*S3;
% disp('Poles of sensitivity transfer function of H-inf L-shaping for P1')
% pole(S3)
% 
% [mag,phase] = bode(L3,omega);
% mag_L3 = reshape(mag,length(omega),1);
% phase_L3 = reshape(phase,length(omega),1);
% 
% allmargin(L3)
% 
% 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)));
%     
%     S3f(i) = abs(freqresp(S3,omega(i)));
%     T3f(i) = abs(freqresp(T3,omega(i)));
%     R3f(i) = abs(freqresp(R3,omega(i)));
%     
%     Wp_inv(i) = abs(freqresp(GAM/W1,omega(i)));
%     Wt_inv(i) = abs(freqresp(GAM/W3,omega(i)));
%     %Wu_inv(i) = abs(freqresp(GAM/Wu,omega(i)));
%     %Wt_inv(i) = GAM/Wt;
%     Wu_inv(i) = GAM/W2;
% end
% disp('H_{infty} Mix-synth peaks')
% disp('Peak of S:')
% disp(max(S1f))
% disp('Peak T:')
% disp(max(T1f))
% disp('Peak of KS:')
% disp(max(R1f))
% 
% disp('IMC controller peaks')
% disp('Peak of S:')
% disp(max(S2f))
% disp('Peak T:')
% disp(max(T2f))
% disp('Peak of KS:')
% disp(max(R2f))
% 
% disp('H_{infty} L-shaping peaks')
% disp('Peak of S:')
% disp(max(S3f))
% disp('Peak T:')
% disp(max(T3f))
% disp('Peak of KS:')
% disp(max(R3f))
% %%
% 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])
% loglog(omega,S2f,'b',omega,S1f,'r',omega,S3f,'g',omega,Wp_inv,'--r','Linewidth',1)
% legend('IMC','H_{\infty} Mixed-sensitivity','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])
% loglog(omega,T2f,'b',omega,T1f,'r',omega,T3f,'g',omega,Wt_inv,'--r','Linewidth',1)
% legend('IMC','H_{\infty} Mixed-sensitivity','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])
% loglog(omega,R2f,'b',omega,R1f,'r',omega,R3f,'g',omega,Wu_inv,'--r','Linewidth',1);
% title('Input usage','FontSize',12,'FontWeight','normal','FontName','Times');
% legend('IMC','H_{\infty} Mixed-sensitivity','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])
% 

%print -depsc C:\Users\jahansha\Documents\Esmaeil_Thesis\Figures_Chapter5\\Three_Sens
