function DD_2norm_finite
% explicit solution on double integrator with 2-norm
%
% Francesco Borrelli July 05 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');
fig_str1 = ['.',filesep,'figures',filesep,'double_int_ft_h'];
fig_str2 = ['.',filesep,'figures',filesep,'double_int_ft_reduced_h'];


%% core code
%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 = [-10; -10];
model.x.max = [10; 10];

%set constraints on input
model.u.min =-1;
model.u.max = 1;

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

% compute LQR terminal cost
PN = model.LQRPenalty;

% activate penalty
model.x.with('terminalPenalty');
model.x.terminalPenalty = PN;

% horizon
horizon = 6;

xl = transpose(-10:5:10);
yl = transpose(-10:5:10);

% compute explicit controllers for increasing horizon
c = cell(1,horizon);
for N = 1:horizon,
    ctrl = MPCController(model, N);
    % explicit solution stored as a cell
    c{N} = ctrl.toExplicit();
    
    % plot
    figure 
    h=plot(c{N}.optimizer,'colormap','lightgray','linewidth',line_width);

    axis([model.x.min(1),model.x.max(1),model.x.min(2),model.x.max(2)]);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);

    title('')
    hx=xlabel(['$x_{1}(' num2str(horizon-N) ')$']);
    set(hx, 'FontSize', label_font_size);
    hy=ylabel(['$x_{2}(' num2str(horizon-N) ')$']);
    set(hy, 'FontSize', label_font_size);
    
    set(gca,'XTick',xl);
    set(gca,'YTick',yl);
    set(gca,'XTickLabel',num2str(xl));
    set(gca,'YTickLabel',num2str(yl));
        
    % print    
    figure_name = [fig_str1, num2str(N), '_Nr_', num2str(c{N}.optimizer.Num)];
    disp('Hit any key to print the figure and continue.');
    pause
    saveas(gcf, [figure_name,'_matlab'], 'fig');
    
    laprint(gcf, [figure_name,'_tex'],'scalefonts','off');
end

% repeat the plot for the control law where polyhedra with the same controllers are merged
close all
for N = 1:horizon,
    % call optimal region merging
    c{N}.simplify('orm');  
    
    % plot
    figure 
    c{N}.optimizer.plot('colormap','lightgray','linewidth',line_width);   

    axis([model.x.min(1),model.x.max(1),model.x.min(2),model.x.max(2)]);
    set(gca,'LineWidth',axeswidth)
    set(gca,'FontSize', tick_font_size);

    title('')
    hx=xlabel(['$x_{1}(' num2str(horizon-N) ')$']);
    set(hx, 'FontSize', label_font_size);
    hy=ylabel(['$x_{2}(' num2str(horizon-N) ')$']);
    set(hy, 'FontSize', label_font_size);       

    set(gca,'XTick',xl);
    set(gca,'YTick',yl);
    set(gca,'XTickLabel',num2str(xl));
    set(gca,'YTickLabel',num2str(yl));   
    
    % print    
    figure_name = [fig_str2, num2str(N), '_Nr_', num2str(c{N}.optimizer.Num)];
    disp('Hit any key to print the figure and continue.');
    pause
    saveas(gcf, [figure_name,'_matlab'], 'fig');
    
    laprint(gcf, [figure_name,'_tex'],'scalefonts','off');
end
