%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Sous-Programme calcul_marges_1.m %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Mmi,Mmo,Mmci,Mmco]=calcul_marges_1(A,B,C,Kp,Ki,n,m,p)

matrice_A=A;
matrice_B=B;
matrice_C=C;
nombre_n=n;
nombre_m=m;
nombre_p=p;
matrice_Kp=Kp;
matrice_Ki=Ki;

% Fonction de transfert en boucle ouverte Li(s)
s=tf('s');
Li_1=ss(matrice_A,matrice_B,matrice_Kp,zeros(nombre_m,nombre_m)); 
Li_1=tf(Li_1);
Li_2=ss(matrice_A,matrice_B,matrice_Ki*matrice_C,zeros(nombre_m,nombre_m));
Li_2=tf(Li_2);
Li_2=Li_2/s;
Li=Li_1+Li_2;

% Fonction de transfert en boucle ouverte Lo(s)
Lo_1=ss(matrice_A-matrice_B*matrice_Kp,matrice_B*matrice_Ki,matrice_C,zeros(nombre_p,nombre_p)); 
Lo_1=tf(Lo_1); 
Lo=Lo_1/s;

% Lieux de Bode de Li(s) et Lo(s)
if nombre_m+nombre_p==2 
    bode(Li);grid;title('Bode de Li - pour les marges: click droit --> Characteristics --> All stability margins');
    figure;bode(Lo);grid;title('Bode de Lo - pour les marges: click droit --> Characteristics --> All stability margins');
else
end

% Fonctions de sensibilité Si(s) et So(s)
Si=inv(eye(nombre_m,nombre_m)+Li);
So=inv(eye(nombre_p,nombre_p)+Lo);

% Fonctions de sensibilité complémentaire Ti(s) et To(s)
Ti=eye(nombre_m,nombre_m)-Si;
To=eye(nombre_p,nombre_p)-So;

% Diagrammes de Bode (éventuellement multivariables) de Si, So, Ti et To
if nombre_m+nombre_p==2 
    figure; bodemag(Si,So,'--'); grid; title('Bode de Si (trait plein) et de So (trait pointillé)');
    figure; bodemag(Ti,To,'--'); grid; title('Bode de Ti (trait plein) et de To (trait pointillé)');
else
    sigma(Si,So,'--'); grid; title('Bode multivariable de Si (trait plein) et de So (trait pointillé)');
    figure; sigma(Ti,To,'--'); grid; title('Bode multivariable de Ti (trait plein) et de To (trait pointillé)');
end

% Calcul des marges de module
Mmi=1/norm(Si,inf);
Mmo=1/norm(So,inf);
Mmci=1/norm(Ti,inf);
Mmco=1/norm(To,inf);