function Robust_Parametrized_Comparison
% comparison of CL/OL and parametrized examples
%
% 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;
border = 0.4;
% create folder for storing figures
mkdir('figures');
figure_name = ['.',filesep,'figures',filesep,'CompFeasSet'];


%% 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 penalty


% 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);

% disturbance matrix
E = eye(nx);

Ncomp=3;

% Compute Colosed-Loop feasible set
disp('Computing CL- Feabile Set')
Omega=X;
iter=0;
while iter<Ncomp
    M=Omega-E*W;
    if ~isFullDim(M)
        return
    end
    % pre set
    Hpre = [M.A*A   M.A*B;
        zeros(2*nu,nx)  U.A];
    Kpre = [M.b;U.b];
    Xpre = projection(Polyhedron(Hpre,Kpre),[1,2]);
    Xpre = Xpre & X;
    iter=iter+1;
    Omega=Xpre;
end




% Parametrized  solution
for N=Ncomp  %N is the horizon according to book
    clear x
    clear u
    clear w
    clear x0
    clear F
    clear obj
    
    % States x(k), ..., x(k+N)
    %x = sdpvar(repmat(nx,1,N),repmat(1,1,N));
    % Uncertainty w(k), ..., w(k+N) (last one not used)
    w = sdpvar(N*nw,1);
    % Inputs U = L*W + V;
    V = sdpvar(N,1);
    L = sdpvar(N,N*nw,'full').*kron((tril(ones(N))-eye(N)),ones(1,nw));
    u = L*w + V;
    x0 = sdpvar(nx,1);
    % initialization;
    obj = 0;
    Xf{N}=X;
    x=x0;
    Hf = Xf{N}.A;
    Kf = Xf{N}.b;

    F=[ymin<= C*x0 <= ymax];
    for k = 1:N
        x=A*x + B*u(k,:) + E*w((k-1)*nw+1:k*nw);
        % Feasible region
        if k<N
            F = [F, umin <= u(k) <= umax, ymin<= C*x <= ymax];
            F = [F, uncertain(w((k-1)*nw+1:k*nw))];
            F = [F, dmin <= w((k-1)*nw+1:k*nw) <= dmax];
            obj = obj+ norm(Q*x,norm_type) + norm(R*u(k),norm_type);
        else
            F = [F, umin <= u(k) <= umax];
            if ~isempty(Hf)
                F = [F, Hf*x<= Kf];
                F = [F, uncertain(w((k-1)*nw+1:k*nw))];
                F = [F, dmin <= w((k-1)*nw+1:k*nw) <= dmax];
            end
            obj = obj+ norm(P*x,norm_type) + norm(R*u(k),norm_type);
        end
        % Cost in value iteration
    end
    %  Solve explicit problem
    disp('Solving for N=')
    N
    [F,obj] = robustify(F,obj,[],w);
    [solpar{N},diagnost,Uz,J,Optimizer] = solvemp(F,obj,[],x0,u(1));
    if isempty(solpar{N}{1})
        disp('Empty Feasible Set')
    end
end



% Open Loop Solution
for N=Ncomp
    clear x
    clear u
    clear w
    clear x0
    clear F
    clear obj
    
    % States x(k), ..., x(k+N)
    %x = sdpvar(repmat(nx,1,N),repmat(1,1,N));
    % 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));
    x0 = sdpvar(nx,1);
    % initialization;
    obj = 0;
    Xf{N}=X;
    x=x0;
    Hf = Xf{N}.A;
    Kf = Xf{N}.b;

    F=[ymin <= C*x0 <= ymax];
    for k = 1:N
        x=A*x + B*u{k} + E*w{k};
        % Feasible region
        if k<N
            F = [F, umin <= u{k} <= umax, ymin<= C*x <= ymax];
            F = [F, uncertain(w{k})];
            F = [F, dmin <= w{k} <= dmax];
            obj = obj+ norm(Q*x,norm_type) + norm(R*u{k},norm_type);
        else
            F = [F, umin <= u{k} <= umax];
            if ~isempty(Hf)
                F = [F, Hf*x<= Kf];
                F = [F, uncertain(w{k})];
                F = [F, dmin <= w{k} <= dmax];
            end
            obj = obj+ norm(P*x,norm_type) + norm(R*u{k},norm_type);
        end
        % Cost in value iteration
    end
    %[F,obj] = robustify(F,obj,[],w);
    %  Solve oexplicit problem
    disp('Solving for N=')
    N
    [sol{N},diagnost,Uz,J,Optimizer] = solvemp(F,obj,[],x0,u{1});
    if isempty(sol{N}{1})
        disp('Empty Feasible Set')
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Printing feasible set of the CROC-OL CROC-CL  and CROC- Parametrized 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fig1=figure;
hold on
%hx=plot(X,'linewidth',line_width,'wire',true,'linestyle',':','color',[1 1 1]);
ho=plot(Omega,'linewidth',line_width,'wire',true,'linestyle','-','color',[1 1 1]);
plot(solpar{N}{1}.Pfinal,struct('linewidth',line_width,'wire',true,'linestyle','-','color',[1 1 1]));
plot(sol{N}{1}.Pfinal,struct('linewidth',line_width,'wire',true,'linestyle','--','color',[1 1 1]));

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);
ht1=text(2,8,'$\mathcal{X}_0=\mathcal{X}_0^{Mv}$');
set(ht1, 'FontSize', label_font_size);
ht2=text(0,0,'$\mathcal{X}_0^{\textrm{OL}}$');
set(ht2, 'FontSize', label_font_size);
%ht3=text(2,2.5,'$\mathcal{X}_0^{Mv}$');
%set(ht3, 'FontSize', label_font_size);
%grid on    
set(gca,'XTickLabel',{''});
set(gca,'YTickLabel',{''});

%% save book figure
saveas(fig1, [figure_name,'_matlab'], 'fig');
laprint(fig1, [figure_name,'_tex'],'scalefonts','off','width',width);


end


