function DD_minimumtime
% minimum-time explicit solution for double integrator
%
% Francesco Borrelli July 09 2009
% M. Herceg, ETH Zurich, 2014
%
% requires MPT3, mpt3lowcom package

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,'mintime_partition'];
figure_name2 = ['.',filesep,'figures',filesep,'mintime_ctrl'];


%% core code
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];
X = Polyhedron('lb',model.x.min,'ub',model.x.max);

%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);


% get the LQR feedback
K = model.LQRGain;
P = model.LQRPenalty;
% find the invariant set for the autonomous model x(k+1)=(A+B*K)x(k)
autnmodel = LTISystem('A',A+B*K);
autnmodel.x.min = model.x.min;
autnmodel.x.max = model.x.max;
Kx = Polyhedron([K;-K],[model.u.max;-model.u.min]);
X0 = X.intersect(Kx);
% Xf
Xf = autnmodel.invariantSet('X',X0);

% activate terminal constraint
model.x.with('terminalSet');
model.x.terminalSet = Xf;
model.x.with('terminalPenalty');
model.x.terminalPenalty = P;

% compute the minimum time controller
mptopt('pqpsolver','mpqp');
c = MinTimeController(model);
ctrl = c.toExplicit();
U = ctrl.optimizer;

% plot
fig1=figure;
hold on
grid on

nU = numel(U);
for i=1:nU
   U(nU-i+1).plot('color',[0.9 0.9 0.9],'linewidth',line_width);
end
plot(Xf,'color',[0.7 0.7 0.7],'linewidth',line_width);
axis([model.x.min(1),model.x.max(1),model.x.min(2),model.x.max(2)]);

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

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));

    

fig2=figure;
hold on
grid on
for i=1:nU
   U(i).fplot('primal','linewidth',line_width,'color',[0.9 0.9 0.9]);
end
view([-142.5 40]);

%axis([sysStruct.ymin(1),sysStruct.ymax(1),sysStruct.ymin(2),sysStruct.ymax(2), sysStruct.umin, sysStruct.umax])
axis tight
title('')
hx=xlabel('$x_{1}$');
set(hx, 'FontSize', label_font_size);
hy=ylabel('$x_{2}$');
set(hy, 'FontSize', label_font_size);
hz=zlabel('$u$');
set(hz, 'FontSize', label_font_size);

set(gca,'LineWidth',axeswidth)
set(gca,'FontSize', tick_font_size);
set(gca,'XTick',xl);
set(gca,'YTick',yl);
set(gca,'ZTick',zl);
set(gca,'XTickLabel',num2str(xl));
set(gca,'YTickLabel',num2str(yl));
set(gca,'ZTickLabel',num2str(zl));


%% save book figures
% print
saveas(fig1, [figure_name1,'_matlab'], 'fig');

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


% print
saveas(fig2, [figure_name2,'_matlab'], 'fig');

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

end
