function SMD_mpc
% explicit MPC control of a spring-mass system using 2-norm objective
%
% Francesco Borrelli Feb 2010
%
% revised by M. Herceg, ETH Zurich, Feb 2014
%
% requires MPT3


close all

%% printing parameters
label_font_size = 14;
tick_font_size = 10;
line_width = 0.8;
axeswidth=0.2;
width = 6;

% create folder for storing figures
mkdir('figures');
figure_name1 = ['.',filesep,'figures',filesep,'springmass_states'];
figure_name2 = ['.',filesep,'figures',filesep,'springmass_inputs'];
figure_name3 = ['.',filesep,'figures',filesep,'springmass_regions'];
figure_name4 = ['.',filesep,'figures',filesep,'springmass_states_tset'];
figure_name5 = ['.',filesep,'figures',filesep,'springmass_inputs_tset'];
figure_name6 = ['.',filesep,'figures',filesep,'springmass_regions_tset'];

%% core code
% values of symbolic parameters to be used when compiling the model in
% HYSDEL language
M=1;
b1=1;
b2=50;
k1=1;
k2=3;
d1=1;
d2=7.5;
S.xm=1;
Ts=0.5;

A1=[0 1;-k1/M -b1/M]; B1=[0 0;1/M -d1/M];
A2=[0 1;-k1/M -b2/M]; B2=[0 0;1/M -d1/M];
A3=[0 1;-k2/M -b1/M]; B3=[0 0;1/M -d2/M];
A4=[0 1;-k2/M -b2/M]; B4=[0 0;1/M -d2/M];

[Az1,Bz1]=c2d(A1,B1,Ts);
[Az2,Bz2]=c2d(A2,B2,Ts);
[Az3,Bz3]=c2d(A3,B3,Ts);
[Az4,Bz4]=c2d(A4,B4,Ts);

S.A111=Az1(1,1);S.A112=Az1(1,2);S.A121=Az1(2,1);S.A122=Az1(2,2);
S.A211=Az1(1,1);S.A212=Az2(1,2);S.A221=Az2(2,1);S.A222=Az2(2,2);
S.A311=Az1(1,1);S.A312=Az3(1,2);S.A321=Az3(2,1);S.A322=Az3(2,2);
S.A411=Az1(1,1);S.A412=Az4(1,2);S.A421=Az4(2,1);S.A422=Az4(2,2);
S.B111=Bz1(1,1);S.B112=Bz1(1,2);S.B121=Bz1(2,1);S.B122=Bz1(2,2);
S.B211=Bz1(1,1);S.B212=Bz2(1,2);S.B221=Bz2(2,1);S.B222=Bz2(2,2);
S.B311=Bz1(1,1);S.B312=Bz3(1,2);S.B321=Bz3(2,1);S.B322=Bz3(2,2);
S.B411=Bz1(1,1);S.B412=Bz4(1,2);S.B421=Bz4(2,1);S.B422=Bz4(2,2);

% get the MLD model
file = which('springmass.hys');
if isempty(file)
    error('Please add the folder with the file "springmass.hys" to Matlab path.');
end
if ~isequal(fileparts(file),pwd)
    % copy the hysdel file to local directory if not present
    copyfile(file,pwd);
end
mld_model = MLDSystem('springmass',S);

% convert to PWA model
model = mld_model.toPWA;

% CONTROL DESIGN with PWA model (no terminal penalties, no terminal set constrains)
model.x.penalty = QuadFunction(eye(2));
model.u.penalty = QuadFunction([0.2 0; 0 1]);
horizon = 3;

% MPC represents MIQP
online_ctrl = MPCController(model, horizon);

% compute the explicit solution
explicit_ctrl = online_ctrl.toExplicit();

% closed loop simulations
cl = ClosedLoop(explicit_ctrl, model);

Tend=25;
x0=[3;4];
sdata = cl.simulate(x0, Tend);
T=0:Ts:Tend*Ts;

% plot the state trajectories 
fig1=figure;
hold on;
grid on;
plot(T,sdata.X(1,:),'color','k','linewidth',line_width);
plot(T,sdata.X(2,:),'color','k','linewidth',line_width,'linestyle','--');

axis([0,Tend*Ts,-2,4]);

title('');
hx=xlabel('Time');
set(hx, 'FontSize', label_font_size);
hy=legend('$x_{1}$', '$x_{2}$');
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(hy, 'FontSize', label_font_size);
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);


% plot the control inputs
fig2=figure;
hold on;
grid on;
stairs(T(1:end-1),sdata.U(1,:),'color','k','linewidth',line_width);
stairs(T(1:end-1),sdata.U(2,:),'color','k','linewidth',line_width,'linestyle','--','Marker','*');

axis([0,Tend*Ts,-11,5])

title('')
hx=xlabel('Time');
set(hx, 'FontSize', label_font_size);
hy=legend('$u_{1}$', '$u_{2}$');
set(hy, 'FontSize', label_font_size);
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);


% plot the regions
fig3 = figure;
hold on;
grid on
explicit_ctrl.optimizer.plot('colormap','lightgray','linewidht',line_width);
axis([model.x.min(1), model.x.max(1), model.x.min(2),model.x.max(2)]);
xlabel('$x_{1}$','FontSize',label_font_size);
ylabel('$x_{2}$','FontSize',label_font_size);
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);


disp('Hit any key to continue MPC design with terminal equality constraint.');
pause

% CONTROL DESIGN WITH TERMINAL EQUALITY CONSTRAINT x_{N}=0
model.x.with('terminalSet');
model.x.terminalSet = Polyhedron('Ae',eye(2),'be',zeros(2,1));

online_ctrltset = MPCController(model, horizon);

% compute the explicit solution
explicit_ctrltset = online_ctrltset.toExplicit();

% closed loop simulations
cltset = ClosedLoop(explicit_ctrltset, model);
sdatatset = cltset.simulate(x0, Tend);

% plot state trajectories
fig4=figure;
hold on
grid on
plot(T,sdatatset.X(1,:),'color','k','linewidth',line_width);
plot(T,sdatatset.X(2,:),'color','k','linewidth',line_width,'linestyle','--');

axis([0,Tend*Ts,-2,4]);

title('');
hx=xlabel('Time');
set(hx, 'FontSize', label_font_size);
hy=legend('$x_{1}$', '$x_{2}$');
set(hy, 'FontSize', label_font_size);
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);



% plot state trajectories
fig5=figure;
hold on;
grid on;
stairs(T(1:end-1),sdatatset.U(1,:),'color','k','linewidth',line_width);
stairs(T(1:end-1),sdatatset.U(2,:),'color','k','linewidth',line_width,'linestyle','--','Marker','*');

axis([0,Tend*Ts,-11,5])

title('')
hx=xlabel('Time');
set(hx, 'FontSize', label_font_size);
hy=legend('$u_{1}$', '$u_{2}$');
set(hy, 'FontSize', label_font_size);
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);


% plot the regions
fig6 = figure;
hold on;
grid on
explicit_ctrltset.optimizer.plot('colormap','lightgray','linewidht',line_width);
axis([model.x.min(1), model.x.max(1), model.x.min(2),model.x.max(2)]);
xlabel('$x_{1}$','FontSize',label_font_size);
ylabel('$x_{2}$','FontSize',label_font_size);
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth);
set(gca,'FontSize', tick_font_size);


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

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

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

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

saveas(fig3, [figure_name3,'_matlab'], 'fig');

laprint(fig3, [figure_name3,'_tex'],'scalefonts','off','width',width);

saveas(fig4, [figure_name4,'_matlab'], 'fig');

laprint(fig4, [figure_name4,'_tex'],'scalefonts','off','width',width);

saveas(fig5, [figure_name5,'_matlab'], 'fig');

laprint(fig5, [figure_name5,'_tex'],'scalefonts','off','width',width);

saveas(fig6, [figure_name6,'_matlab'], 'fig');

laprint(fig6, [figure_name6,'_tex'],'scalefonts','off','width',width);

end

