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)
alfa=pi/3;
A = 1.2*[cos(alfa) -sin(alfa); sin(alfa) cos(alfa)];
B = [1;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 = -0.5;
umax = 1;

%set constraints on disturbace

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

r1=[0.0176,0.0655];
r2=[-0.0428,0.0428];
r3=[0.0747,0.0200];
Ax=[r1;-r1;r2;-r2;r3;-r3]
%V=[-1.8,-4.2;-2.7 -3.7;2.73 -3.73;1.8 -4.2;0 4.73;4 2.3;4.5 0.5;4.5 -0.5;1.8 4.2;-1.8 4.2];
V=[-1.5,-4.5;-2 -3; 1.1, 1.7;-1.6, 2.5;2.0, -3];
X = Polyhedron(Ax,ones(6,1));
W = Polyhedron(V);
U = Polyhedron('lb',umin,'ub',umax);

% disturbance matrix
E = eye(nx);

Ncomp=2;

% 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=[X.A*x0<=X.b];
    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, X.A*x<=X.b];
            F = [F, uncertain(w((k-1)*nw+1:k*nw))];
            F = [F, W.A*w((k-1)*nw+1:k*nw) <= W.b];
            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,  W.A*w((k-1)*nw+1:k*nw) <= W.b];
            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=[X.A*x0<=X.b];
    for k = 1:N
        x=A*x + B*u{k} + E*w{k};
        % Feasible region
        if k<N
            F = [F, umin <= u{k} <= umax, X.A*x<=X.b];
            F = [F, uncertain(w{k})];
            F = [F, W.A*w{k} <= W.b];
            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, W.A* w{k} <= W.b];
            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
plot(X,'linewidth',line_width,'wire',true,'linestyle',':','color',[1 1 1]);
%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 0]));
%plot(sol{N}{1}.Pfinal,struct('linewidth',line_width,'wire',true,'linestyle','--','color',[1 1 1]));
plot(Omega,'linewidth',line_width,'linestyle','-','color','r');
plot(solpar{N}{1}.Pfinal,struct('linewidth',line_width,'linestyle','-','color','g'));
plot(sol{N}{1}.Pfinal,struct('linewidth',line_width,'linestyle','--','color','b'));

%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,6,'$\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     
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


