Ts = 5e-2; % sampling time 
% Specify damping coefficient.
c = 5;   
% Specify stiffness.
k = 300; 
% Specify load command.
u = 1:10;
% Specify mass.
m = 10*u + 0.1*u.^2;
% Compute linear system at a given mass value.
clearvars sys
for i = 1:length(u)
   A = [0 1; -k/m(i), -c/m(i)];
   B = [0; 1/m(i)];
   C = [1 0];
   sys(:,:,i) = c2d(ss(A,B,C,0),Ts,'least-squares'); 
end
%The variable u is the scheduling input. Add this information to the model.

sys.SamplingGrid = struct('LoadCommand',u);

bode(sys)
shg



%%
% Simultanous stabilization using a PID controller

C0 = tunablePID('C','pid',Ts); %

z = tf('z',Ts);
C0.u = 'e';   C0.y = 'u';

sys.y = 'y';
sys.u = 'u';
Sum1 = sumblk('e = r - y');
T0 = connect(sys,C0,Sum1,{'r'},{'u','e','y'});


W1 = 1/(z-1);
W2 = 1/makeweight(2,0.05*pi/Ts,0.1,Ts); % CL bandwidth off ~0.05*pi/Ts

softReq =  TuningGoal.WeightedGain('r','e',W1,[]);
hardReq =  TuningGoal.WeightedGain('r','y',W2,[]);;


opts = systuneOptions('RandomStart',0);
[CL,fSoft,gHard,f] = systune(T0,softReq,hardReq,opts);

C = getBlockValue(CL,'C');
S0 = feedback(1,sys*C);

%%
w = logspace(-2,log10(pi/Ts), 1000);
w(end) = pi/Ts;

SYS = datadriven.system('G',sys,'W',w,'Kinit',C,'order', 2, 'theta', @THETA);
OBJ = datadriven.objectives('two.W1',10/(tf('z',Ts)-1));
CON = datadriven.constraints('W1',1/2,'W2',1/2,'W3',db2mag(-60));
PAR = datadriven.parameters('tol',1e-5,'maxIter',20,'radius',0.99);

%% 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.lpv(SYS,OBJ,CON,PAR); % implementation for SISO controllers

FB = datadriven.getController(SYS); 

%%


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

step(S0,S);

legend('Non-LPV','LPV')


function theta = THETA(sys)

q = sys.SamplingGrid.LoadCommand;

theta = [1;q;q^2]; % schedule controller with 1,LoadCommand,LoadCommand^2
end