% Example derived from
% https://ch.mathworks.com/help/robust/ug/simultaneous-stabilization-using-robust-control.html
%
% Design a digital controller for a continious (and unstable) system.

%addpath ../toolbox/
%%
Pnom = tf(2,[1 -2]);                  % Nominal model

p1 = Pnom*tf(1,[.06 1]);              % extra lag
p2 = Pnom*tf([-.02 1],[.02 1]);       % time delay
p3 = Pnom*tf(50^2,[1 2*.1*50 50^2]);  % high frequency resonance
p4 = Pnom*tf(70^2,[1 2*.2*70 70^2]);  % high frequency resonance
p5 = tf(2.4,[1 -2.2]);                % pole/gain migration
p6 = tf(1.6,[1 -1.8]);                % pole/gain migration

Parray = stack(1,p1,p2,p3,p4,p5,p6);
%%
Ts = 1e-3; % Sample fast enough such that continious model is a good approxiamtion of the model using zero order hold
z = tf('z',Ts);

Kinit = 2+0.01/(z-1); % Initial stabilizing controller

w = logspace(-1,log10(pi/Ts),300);

%% Set-up problem

SYS = datadriven.system('G',Parray,'W',w,'Kinit',Kinit,'order', 20);
OBJ = datadriven.objectives('two.W1',.1/(tf('z',Ts)-1));
CON = datadriven.constraints('W1',1/2,'W2',1/2,'W3',db2mag(-30));
PAR = datadriven.parameters('tol',1e-5,'maxIter',100,'radius',0.95);

%% SOVLE

% !! REQUIRES MOSEK + MOSEK FUSION !!
% Make sure the license is avalible, and Fusion installed. 
% type
% >> mosekdiag
% to check if mosek read to go

[SYS.controller,obj] = datadriven.siso(SYS,OBJ,CON,PAR); % implementation for SISO controllers

FB = datadriven.getController(SYS); 

%%

Gf = frd(Parray,w);
Gf.Ts = Ts; 
% !!! Gf (ie Parray) is NOT sampled at Ts. The different sampling time is
% changed only for the plots (can't mix continious and discrete systems)

figure
S = feedback(1,Gf*FB); % compute sensitivity

bodemag(S,1-S,FB*S, ...
        tf(1/CON.W1),'-k',tf(1/CON.W2),'-k',tf(1/CON.W3),'-k',...
        w)

legend('sensitivity','complementary sensitivity','input sensitivity','1/constraints')

%%
nyquist(Gf*FB)
shg