function PropagationSets
% propagation of feasible sets for double integrator
%
% Francesco Borrelli July 09 2009
% M. 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;
% create new folder for storing figures
mkdir('figures');
figure_name1 = ['.',filesep,'figures',filesep,'ex_feas_0'];
figure_name2 = ['.',filesep,'figures',filesep,'ex_feas_1'];



%% core code
% System Definiton 
%x(k+1)=Ax(k)+Bu(k)
A = [1 1; 0 1];
B = [0; 1];
model = LTISystem('A',A,'B',B);

%set constraints on output
model.x.min = [-5; -5];
model.x.max = [5; 5];
X = Polyhedron('lb',model.x.min,'ub',model.x.max);

%set constraints on input
model.u.min =-1;
model.u.max = 1;
U = Polyhedron('lb',model.u.min,'ub',model.u.max);

% quadratic penalties
model.x.penalty = QuadFunction(eye(2));
model.u.penalty = QuadFunction(1);


% get the LQR feedback
K = model.LQRGain;
% find the invariant set for the autonomous model x(k+1)=(A+B*K)x(k)
autnmodel = LTISystem('A',A+B*K);
autnmodel.x.min = model.x.min;
autnmodel.x.max = model.x.max;
Kx = Polyhedron([K;-K],[model.u.max;-model.u.min]);
X0 = X.intersect(Kx);
% Xf
Xf = autnmodel.invariantSet('X',X0);

% use scaled set for better graphics
Xf = 0.5*Xf;

% case 1
% compute reachable sets outside from Xf
maxIterations = 50;
Pn = []; % save intermediate set during computation
X0 = Xf; % initial set constraint
for i = 1:maxIterations
    % compute forward reachable set
    R = model.reachableSet('X', X0, 'U', U, ...
        'direction', 'backward');
    % intersect with the state constraints
    R = R.intersect(X).minHRep();
    Pn = [Pn, R];
    if R==X0
        break
    else
        X0 = R;
    end
end


% case 2
% compute the control invariant set starting from X
Piter = []; % save intermediate set during computation
X0 = X; % initial set constraint
for i = 1:maxIterations
    % compute backward reachable set
    P = model.reachableSet('X', X0, 'U', U, ...
        'direction', 'backward');
    % intersect with the state constraints
    P = P.intersect(X0).minHRep();
    Piter = [Piter, P];
    if P==X0
        break
    else
        X0 = P;
    end
end


% plot the sets
fig1 = figure;
hold on
grid on
R.plot('color',[0.9 0.9 0.9],'linewidth',line_width);
Xf.plot('color',[0.7 0.7 0.7],'linewidth',line_width);
h=Pn.plot('wire',true,'linewidth',line_width,'linestyle','--');
for j=1:numel(h)
    set(h(j),'FaceColor','none');
end
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);

axis([model.x.min(1)-1,model.x.max(1)+1,model.x.min(2)-1,model.x.max(2)+1])



fig2 = figure;
hold on
grid on
P.plot('color',[0.9 0.9 0.9],'linewidth',line_width);
f=X.plot('wire',true,'linewidth',line_width,'linestyle','--');
set(f,'FaceColor','none');
g=Piter.plot('wire',true,'linewidth',line_width,'linestyle','--');
for j=1:numel(g)
    set(g(j),'FaceColor','none');
end

set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);
hx1 = xlabel('$x_1$');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('$x_2$');
set(hy1, 'FontSize', label_font_size);

axis([model.x.min(1)-1,model.x.max(1)+1,model.x.min(2)-1,model.x.max(2)+1])


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

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


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

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


end