function DD_MPC_inftynorm_3Cases
% tree cases of computing explicit solution for a double integrator example
% linear cost function
%
% This example uses native MPT3 interface to formulate the MPC problem.
%
% 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_ninf_case'];
fig_str2 = ['.',filesep,'figures',filesep,'ddouble_int_ninf_case'];


%% core code
% use mpt2 interface to formulate the problem
% use native mpt3 interface to formulate the problem
A = [1 1; 0 1];
B = [0; 1];
model = LTISystem('A',A,'B',B);
model.x.min = [-5;-5];
model.x.max = [ 5; 5];
model.u.min = -1;
model.u.max = 1;

% penalties
model.x.penalty = InfNormFunction(eye(2));
model.u.penalty = InfNormFunction(20);

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

% Case 1
model.x.terminalPenalty = model.x.penalty;
% horizon = 6
ctrl1 = MPCController(model, 6);
% compute explicit solution
c{1} = ctrl1.toExplicit();
% simplify using merging
c{1}.simplify();


% Case 2
% compute the terminal penalty
model.x.without('terminalPenalty');
% horizon = 15
ctrl2 = MPCController(model, 15);
c{2} = ctrl2.toExplicit();
% simplify using merging
c{2}.simplify();

% pick the cost that contain the origin
[isin,inwhich]=c{2}.optimizer.contains([0;0]);
nrc=length(inwhich);
Ptcost=[];
for j=1:nrc
    Ptcost=[Ptcost;c{2}.optimizer.Set(j).Functions('obj').F];
end

% compute the controller
model.x.with('terminalPenalty');
model.x.terminalPenalty = InfNormFunction(Ptcost);
ctrl3 = MPCController(model, 6);
c{3} = ctrl3.toExplicit();
% simplify using merging
c{3}.simplify();


%% plot and save book figures
nsteps = 50;
for ii = 1:3,
    X0 = [-4,2; -2, 2; 0, 0.5; 4, -2; -1, -1; 2, -0.5];
    figure
    hold on
    plot(c{ii}.optimizer.Domain,'linewidth',line_width,'color','w');
    c{ii}.N=1;
    cl = ClosedLoop(c{ii},model);
    for k=1:size(X0,1)
        data = cl.simulate(X0(k,:)',nsteps);
        plot(data.X(1,:), data.X(2,:),'ko-','LineWidth',line_width,'MarkerSize',10),
    end
    
    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,'XTickLabel',{''});
    set(gca,'YTickLabel',{''});
    
    % print
    figure_name = [fig_str1 num2str(ii)];
    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

for ii = 1:3,
    figure
    hold on
    h=plot(c{ii}.optimizer,'LineWidth',line_width,'colormap','lightgray');

    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,'XTickLabel',{''});
    set(gca,'YTickLabel',{''});
    
    % print
    figure_name = [fig_str2 num2str(ii)];
    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




end