%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PHILIPPE SCHUCHERT            %
% SCI-STI-AK, EPFL              %
% philippe.schuchert@epfl.ch    %
% March 2021                    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Example use of the datadriven command
clc; close all; clear

%% Things we know / have measured

Ts = 10e-3;
z = tf('z',Ts);

G1 = 1/(z-1.02);
G2 = 0.9/(z-1);
Garray = stack(1,G1,G2);

w = utils.logspace2(0.01,pi/Ts,400);

K = pidtune(G2,'pidf',2); % Inital controller that is known to be stabilizing
Sinit = feedback(1,Garray*K);

disp(['Eigenvalues close-loop using initial controller: ', num2str(max(abs(eig(Sinit)),[],'all')), ' (stable CL)']) % < 1 --> Stable Closed-loop
disp('')
%%

% Initial controller of order 1, but we want a final controller of order 3:
% -> zero padding to get the correct order.
[num,den] = tfdata(K,'v'); % get numerator/denominator of PID controller
orderK = 3;
den(orderK+1) = 0; % zero padding
num(orderK+1) = 0; % zero padding

% Poles on unit cicle of the controller have to be in the fixed parts
Fy = [1 -1]; % fixed parts in denominator, that well be keps throughout iterations.
den_new = deconv(den,Fy); % den = conv(den_new,Fy).
% The final controller den is den = conv(den_new, Fy)

% Denominator has only 2 tunable coefficients (first one is fixed to 1, and fixed part).
% Numerator has 4 tunable coefficients,  Fx = 1. No need to change num.

%% SET-UP system info

[SYS, OBJ, CON, PAR] = utils.emptyStruct(); % load empty structure

ctrl = struct('num',num,'den',den_new,'Ts',Ts,'Fx',1,'Fy',Fy); % assemble controller

SYS.controller = ctrl;
SYS.model = Garray; % <- simultaneous satabilizations

SYS.W = w; % specify frequency grid where problem is solved

%% Objective(s)
% See different fields of OBJ
W1 = 1/(tf('z',Ts)-1);
OBJ.o2.W1 = W1; % Only minimize || W1 S ||_2, freqresp(W1,w) must exist

%% Constraints(s)
% See different fields of CON
W2 =  1/c2d(makeweight(2,0.1*pi/Ts,0),Ts);
CON.W2 = W2; % Only constraint || W2 T ||_\infty ≤ 1, freqresp(W2,w) must exist
%% Solve problem
% See different fields of PAR
PAR.tol = 1e-4; % stop when change in objective < 1e-4. 
%PAR.maxIter = 10; % max Number of iterations
PAR.radius = 0.98; % max abs eigenvalue pole controller

tic
[controller,obj] = datadriven(SYS,OBJ,CON,PAR);
toc

% Other solver can be used as last additional argument:
% [controller,obj] = datadriven(SYS,OBJ,CON,PAR,'sedumi'); to force YALMIP
% to use sedumi as solver.
% If mosek AND mosek Fusion are installed, you can use
% [controller,obj] = datadriven(SYS,OBJ,CON,PAR,'fusion');
% (much faster, no need for YALMIP as middle-man)

%% Analysis using optimal controller

Kdd = utils.toTF(controller); % construct controller from datadriven output
if ~isempty(PAR.radius)
    polestr = [' (constrained <', num2str(PAR.radius), ')'];
else
    polestr = '';
end

disp(' ')
disp(['Max abs pole (variable part) controller: ', num2str(max(abs(roots(controller.den)))), polestr]) 
S = feedback(1,Garray*Kdd);

bodemag(Sinit,'-.r',S,'-r',1-S,1/W2,'--k',w)
ylim([-20 10])
grid
legend('Initial $\mathcal{S}$','Final $\mathcal{S}$','$\mathcal{T}$','$\overline{W_2}^{-1}$','interpreter','LaTeX')
%legend('Initial $\mathcal{S}$','Final $\mathcal{S}$','$\mathcal{T}$','$\overline{W_2}^{-1}$','interpreter','LaTeX')
% Need to plot legend two times (bug with MATLAB 2020).
shg

figure(2);
step(Sinit,S,0:Ts:10)
legend('Initial controller','Final controller')
% Notice how bad the intial controller was ( but still stabilizing)
disp(['H2 computed using trapz integration: ', num2str(obj.H2)])