function DD_MPC_3Cases_old
% tree cases of computing explicit solution for a double integrator example
% quadratic function
%
% use mpt2 interface to formulate the 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_str = ['.',filesep,'figures',filesep,'double_int_case'];


%% core code
% use mpt2 interface to formulate the problem
sysStruct.A = [1 1; 0 1];
sysStruct.B = [0; 1];
sysStruct.C = eye(2);
sysStruct.D = [0;0];
sysStruct.xmin = [-10;-10];
sysStruct.xmax = [ 10; 10];
sysStruct.umin = -1;
sysStruct.umax = 1;
probStruct.Q = eye(2);
probStruct.R = 0.01;
probStruct.norm = 2;
probStruct.subopt_lev = 0;
probStruct.N = 2; % horizon
nu = size(sysStruct.B,2);

% convert to mpt3
model = mpt_import(sysStruct,probStruct);

% compute LQR terminal cost
PN = model.LQRPenalty;
probStruct.P_N = PN.H;


% Case 1
probStruct.N = 2; % horizon
probStruct.Tset = polytope([eye(2);-eye(2)],[0*sysStruct.xmax+1e-5;0*sysStruct.xmin+1e-5]); % terminal constraint
probStruct.Tconstraint = 2;
M1 = mpt_constructMatrices(sysStruct,probStruct);

% explicit solution
problem1 = Opt(M1);
c{1} = problem1.solve();
% simplify using merging
c{1}.xopt.trimFunction('primal',nu);
c{1}.xopt.merge('primal');


% Case 2
K = model.LQRGain();
X = Polyhedron([eye(2);-eye(2); K; -K],[model.x.max;-model.x.min;model.u.max; -model.u.min]);
aut_model = LTISystem('A',sysStruct.A+sysStruct.B*K);
Oinf = aut_model.invariantSet('X',X);
% use Oinf as terminal set
probStruct.Tset = polytope(Oinf.A,Oinf.b);
M2 = mpt_constructMatrices(sysStruct,probStruct);
% explicit solution
problem2 = Opt(M2);
c{2} = problem2.solve();
% simplify using merging
c{2}.xopt.trimFunction('primal',nu);
c{2}.xopt.merge('primal');

% Case 3
probStruct.N = 6; % horizon = 6
probStruct = rmfield(probStruct,'Tset');
probStruct.Tconstraint = 0;
M3 = mpt_constructMatrices(sysStruct,probStruct);
% explicit solution
problem3 = Opt(M3);
c{3} = problem3.solve();
% simplify using merging
c{3}.xopt.trimFunction('primal',nu);
c{3}.xopt.merge('primal');


%% plot and save book figures
for ii = 1:3,
    figure;
    h=plot(c{ii}.xopt,'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,'XTickLabel',{''});
    set(gca,'YTickLabel',{''});
    
    % print
    figure_name = [fig_str, 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