function Robust_CL
% min-max control example
%
% Francesco Borrelli July 05 2009
% Revised by Martin Herceg, ETH Zurich, 2014
% 
% requires MPT3

close all

%% printing parameters
label_font_size = 14;
tick_font_size = 10;
line_width = 0.8;
axeswidth=0.2;
width = 6;
% create folder for storing figures
mkdir('figures');
name = 'ex_14_minmax';
figure_name1 = ['.',filesep,'figures',filesep,'Sol_',name];
figure_name2 = ['.',filesep,'figures',filesep,'partdd_rob_cl'];
figure_name3 = ['.',filesep,'figures',filesep,'Feasset_',name];

%% core code
%x(k+1)=Ax(k)+Bu(k)
A= [1 0.1; -0.2 1];
B = [-0.5;1];

%y(k)=Cx(k)+Du(k)
C= [1 0; 0 1];
D= [0;0];

%set constraints on output
ymin = [-10;-10];
ymax = [ 10; 10];

%set constraints on state
xmin = [-10;-10];
xmax = [ 10; 10];

%set constraints on input
umin =   -1;
umax =   1;

%set constraints on disturbace
dmin = 2*[-1;-1];
dmax = 2*[ 1; 1];

% problem setup
norm_type=inf;
Q=[1 1;0 1]; % state penalty
P=Q; % terminal state penalty
R=1.8;% input weight
N=3; % horizon

% Polyhedral sets
nx = size(A,1); % Number of states
nu = size(B,2); % Number of inputs
nw = nx;
X = Polyhedron('lb',xmin,'ub',xmax);
W = Polyhedron('lb',dmin,'ub',dmax);
U = Polyhedron('lb',umin,'ub',umax);


% Compute Min MAx robust Control
disp('Computing Min-Max Control')

% Model data
E = eye(nx);
% States x(k), ..., x(k+N)
x = sdpvar(repmat(nx,1,N+1),repmat(1,1,N+1));
% Inputs u(k), ..., u(k+N) (last one not used)
u = sdpvar(repmat(nu,1,N),repmat(1,1,N));
% Uncertainty w(k), ..., w(k+N) (last one not used)
w = sdpvar(repmat(nw,1,N),repmat(1,1,N));


% initialization;
disp('Step N') 
disp('Initialization') 
J{N+1} = norm(P*x{N+1},norm_type);
Xf{N+1}=X;

for k = N:-1:1   
    disp('Step') 
    k-1
    Hf = Xf{k+1}.A;
    Kf = Xf{k+1}.b;
    
    % Feasible region
    F = [umin <= u{k} <= umax, ymin<= C*x{k} <= ymax];
    if ~isempty(Hf)
        F = [F, Hf*(A*x{k} + B*u{k} + E*w{k}) <= Kf];
    end

    % Bounded exploration space
    F = [F, -100 <= x{k} <= 100];

    % Uncertain value function
    Jw = replace(J{k+1},x{k+1},A*x{k} + B*u{k} + E*w{k});

    % Declare uncertainty model
    F = [F, uncertain(w{k})];
    F = [F, dmin <= w{k} <= dmax];

    % Cost in value iteration
    obj = norm(Q*x{k},norm_type) + norm(R*u{k},norm_type) + Jw;

    % Solve one-step problem   
    [sol{k},diagnost{k},Uz{k},J{k},Optimizer{k}] = solvemp(F,obj,[],x{k},u{k});
    Xf{k}=Polyhedron('H',double(sol{k}{1}.Pfinal));
    

end

% Generate Plots
fig1=figure;
hold on
h=plot(sol{1}{1}.Pn,struct('colormap','lightgray','linewidth',line_width)); 
border = 0.4;
axis([xmin(1)-border,xmax(1)+border,xmin(2)-border,xmax(2)+border]);

set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);

title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);

set(gca,'XTickLabel',{''});
set(gca,'YTickLabel',{''});


% simplify the last controller
Pn(1,length(sol{N}{1}.Pn))=Polyhedron;
for i=1:length(Pn)
   Pn(i) = Polyhedron('H',double(sol{N}{1}.Pn(i)));
   Pn(i).addFunction(AffFunction(sol{N}{1}.Fi{i},sol{N}{1}.Gi{i}),'primal');
   Pn(i).addFunction(AffFunction(sol{N}{1}.Bi{i},sol{N}{1}.Ci{i}),'obj');
end
PU = PolyUnion(Pn);
PU.merge('primal');

fig2=figure;
hold on
h=plot(PU,'colormap','lightgray','linewidth',line_width); 
axis([xmin(1)-border,xmax(1)+border,xmin(2)-border,xmax(2)+border]);

set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);

title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);

set(gca,'XTickLabel',{''});
set(gca,'YTickLabel',{''});

% feasible set
fig3=figure;
hold on
for j=2:length(Xf)
    h=plot(Xf{j},'linewidth',line_width,'linestyle','--','wire',true);
    set(h,'FaceColor','none');
end
plot(Xf{1},'color',[0.5 0.5 0.5],'linewidth',line_width);
axis([xmin(1)-border,xmax(1)+border,xmin(2)-border,xmax(2)+border]);

set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);

title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);

set(gca,'XTickLabel',{''});
set(gca,'YTickLabel',{''});



%% save book figures
saveas(fig1, [figure_name1,'_matlab'], 'fig');

laprint(fig1, [figure_name1,'_tex'],'scalefonts','off','width',width);

saveas(fig2, [figure_name2,'_matlab'], 'fig');

laprint(fig2, [figure_name2,'_tex'],'scalefonts','off','width',width);

saveas(fig3, [figure_name3,'_matlab'], 'fig');

laprint(fig3, [figure_name3,'_tex'],'scalefonts','off','width',width);


end