function DD_inftynorm_infinite
% explicit solution with infinite horizon on double integrator with infinity-norm
%
% 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');
fig_str1 = ['.',filesep,'figures',filesep,'dd_ninfty_infinite_Nr_'];
fig_str2 = ['.',filesep,'figures',filesep,'dd_ninfty_infinite_reduced_Nr_'];


%% 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 = InfNormFunction(eye(2));
model.u.penalty = InfNormFunction(0.8);

% activate terminal penalty
model.x.with('terminalPenalty');
model.x.terminalPenalty = InfNormFunction(eye(2));

% horizon
horizon = 30;

for N = 1:horizon,
    % increase horizon by one
    c = MPCController(model, N);
    ctrl{N} = c.toExplicit();    
    if N>1
        % if two subsequant domains are equal, break, the infinite horizon
        % solution found
        if ctrl{N}.optimizer.Domain==ctrl{N-1}.optimizer.Domain,
           break
        end
    end
end

% pick the last optimizer
U = ctrl{N};

% plot
fig1 = figure;
U.partition.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);

xl = transpose(-10:5:10);
yl = transpose(-10:5:10);
set(gca,'XTick',xl);
set(gca,'YTick',yl);
set(gca,'XTickLabel',num2str(xl));
set(gca,'YTickLabel',num2str(yl));


title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);
        

% simplify the controller 
U.simplify('orm');

% plot the simplified controller
fig2 = figure;
U.partition.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}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);

set(gca,'XTick',xl);
set(gca,'YTick',yl);
set(gca,'XTickLabel',num2str(xl));
set(gca,'YTickLabel',num2str(yl));


%% save book figures

% print
figure_name = [fig_str1, num2str(U.optimizer.Num)];
saveas(fig1, [figure_name,'_matlab'], 'fig');

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


% print
figure_name = [fig_str2 num2str(U.optimizer.Num)];
saveas(fig2, [figure_name,'_matlab'], 'fig');

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

end
