function PreReachNSteps
% controllable sets
%
% T. Besselmann, ETH Zurich, 18.08.2008
% F. Borrelli, UC Berkeley, 05.07.2009
% M. Herceg, Automatic Control Laboratory, 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,'PreSet_ex_12_2b'];
figure_name2 = ['.',filesep,'figures',filesep,'reachSet1_ex_12_2b'];
width = 6;
set(0,'defaulttextinterpreter','none');


%% core code
% Unstable System
% matrices of LTI dynamics 
% x(k+1) = A*x(k) + B*u(k)
A=[1.5 0;1 -1.5];
B=[1;0];

% create model in MPT3 interface
model = LTISystem('A',A,'B',B);

% constraints on inputs and states
model.u.min = -5;
model.u.max = 5;
model.x.min = [-10;-10];
model.x.max = [ 10; 10];

% constraint sets represented as polyhedra
X = Polyhedron('lb',model.x.min,'ub',model.x.max);
U = Polyhedron('lb',model.u.min,'ub',model.u.max);

% target set
S = Polyhedron('lb',[-1;-1],'ub',[1;1]);

% compute backward reachable set starting from the target set S
K(1) = model.reachableSet('X', S, 'U', U, 'direction', 'backward','N',1).intersect(X);
K(2) = model.reachableSet('X', K(1), 'U', U, 'direction', 'backward','N',1).intersect(X);
K(3) = model.reachableSet('X', K(2), 'U', U, 'direction', 'backward','N',1).intersect(X);
K(4) = model.reachableSet('X', K(3), 'U', U, 'direction', 'backward','N',1).intersect(X);


fig1=figure;
hold on
% plot the target set
plot(S,'color',[0.7 0.7 0.7],'linewidth',line_width);
% plot the controllable sets
for j=1:numel(K)
    % plot a shifted set such that there is no overlap
    Kshifted = K(j)-j*[10;0];    
    plot(Kshifted,'color',[0.7 0.7 0.7],'linewidth',line_width); 
    % put the labels
    ht2=text(-j*10,5,['$\mathcal{K}_',num2str(j),'(\mathcal{S})$']);
    set(ht2, 'FontSize', label_font_size);    
end

axis([-50,10,-6,6])

ht1=text(0,5,'$\mathcal{S}$');
set(ht1, 'FontSize', label_font_size);
xt = [];
yt = [];
set(gca,'YTickLabel',num2str(xt));
set(gca,'XTickLabel',num2str(yt));

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

hx1 = xlabel('');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('');
set(hy1, 'FontSize', label_font_size);
set(fig1, 'Position', [0 0 1200 420])
    
    
% compute forward reachable sets starting from S
R(1) = model.reachableSet('X', S, 'U', U, 'direction', 'forward','N',1).intersect(X);
R(2) = model.reachableSet('X', R(1), 'U', U, 'direction', 'forward','N',1).intersect(X);
R(3) = model.reachableSet('X', R(2), 'U', U, 'direction', 'forward','N',1).intersect(X);
R(4) = model.reachableSet('X', R(3), 'U', U, 'direction', 'forward','N',1).intersect(X);


fig2 = figure;
hold on
% plot the target set
plot(S,'color',[0.7 0.7 0.7],'linewidth',line_width);
% plot the reachable sets
for j=1:numel(R)
    % plot a shifted set such that there is no overlap
    Rshifted = R(j)+j*[60;0];    
    plot(Rshifted,'color',[0.7 0.7 0.7],'linewidth',line_width); 
    % put the labels
    ht2=text(j*60,24,['$\mathcal{R}_',num2str(j),'(\mathcal{X}_0)$']);
    set(ht2, 'FontSize', label_font_size);
end

axis([-50,300,-30,30])

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

hx1 = xlabel('');
set(hx1, 'FontSize', label_font_size);
hy1 = ylabel('');
set(hy1, 'FontSize', label_font_size);
    
ht1=text(0,24,'$\mathcal{X}_0$');
set(ht1, 'FontSize', label_font_size);

xt = [];
yt = [];
set(gca,'YTickLabel',num2str(xt));
set(gca,'XTickLabel',num2str(yt));
set(fig2, 'Position', [680 558 1200 420])
    

%% save book figures
% generate FIG file
disp('Hit any key to print the figures.');
pause
saveas(fig1, [figure_name1,'_matlab'], 'fig');

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


% generate FIG file
saveas(fig2, [figure_name2,'_matlab'], 'fig');

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


end
