clear all;
close all;
clc;

Parameter_var;
alphMin=40000*g*a*b/(4*sqrt(100^2-(a/2)^2-(b/2)^2+a*b/2));
omegaMin=sqrt(alphMin/(40000*5.72^2));

% Zustände
%         [ phi_l          ]
%         | phi_lp         |
% x   =   |                |
%         | phi_c          |
%         [ phi_d          ]

A   = [ 0 1 0 0;...
        -omega^2 0 omega^2 omega^2;...
        0 0 0 0;...
        0 0 0 0 ];

B   = [ 0 0;
        0 0; ...
        1 0; ...
        0 1];
% C1  = [0 1 1 0];
% C2  = [0 0 0 1];

% Erster Versuch
% C1  = [1 0 0 -1];
% C2  = [1 0 0 0];
% C3  = [0 1 0 0]; 
% C4  = [0 0 0 0];
% D1  = [0 0];
% D2  = [-1 0];
% D3  = [0 0];
% D4  = [1 0];

%Zweiter Versuch
C  = [  0 1 0 0;...
        0 0 1 1;...
        1 0 1 1];
D  = 0;
G=ss(A,B,C,D);

% Festlegen der Gewichtungsblöcke
Wref    = 10^(1/120)*tf([0 1],[1/0.1 1]);
Wu      = 6.0606;   % 0.165^-1
Wgamma  = 4/pi;     % (pi/4)^-1
Wz      = 10^1.5*tf([0 1],[1 0]);   % pi^-1
Woffset = 0.016+5e-5;

systemnames     = 'G Wgamma Wu Wz Woffset Wref';
inputvar        = '[ dOffset; dref; du ]';
outputvar       = '[ Wgamma; Wu; Wz; G(1)+Woffset; G(2)]';
input_to_G      = '[ Wref; du ]';
input_to_Wref   = '[ dref ]';
input_to_Wgamma = '[ G(3) ]';
input_to_Wu     = '[ G(2) ]';
input_to_Wz     = '[ Wref-G(3)+G(2) ]';
input_to_Woffset= '[ dOffset ]';
sysout = sysic;

nmeas = 2;
ncont = 1;
[K1,Scl1,gam1] = hinfsyn(sysout,nmeas,ncont)
% CL1 = lft(G([1:4 ], 1),K1);