function springmass_openloop
% openloop prediction of a spring-mass system
%
% 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,'nonlinear_spring'];
figure_name2 = ['.',filesep,'figures',filesep,'spring-mass-u2=0'];
figure_name3 = ['.',filesep,'figures',filesep,'spring-mass-u2=1'];



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

% plot the nonlinearity
fig1=figure;
x1=-10:.1:10;
k=(k1*x1+d1).*(x1<=S.xm)+(k2*x1+d2).*(x1>S.xm);
plot(x1,k,'k','linewidth',line_width)
grid on
hx1=xlabel('$x_1$');
hy1=ylabel('$k(x_1)$');
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth)
set(gca, 'FontSize', tick_font_size);
set(hx1, 'FontSize', label_font_size);
set(hy1, 'FontSize', label_font_size);


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
model = MLDSystem('springmass',S);

% PWA model
pwa_model = model.toPWA;


% Open Loop Simulations
x0=[0;0];
Tend=10;
TT=Tend/Ts;
T=0:Ts:Tend;
u1=3;
% step change in the first input
U1=[u1*ones(1,TT); zeros(1,TT)];

% simulate
pwa_model.initialize(x0);
data1 = pwa_model.simulate(U1);

% identify the modes
xu1 = [data1.X(:,1:size(data1.U,2));data1.U];
mode1 = pwa_model.domain.contains(xu1);
[index_mode1,c]=find(mode1);

% plot the state trajectories
fig2=figure;
subplot(2,1,1)
plot(T,data1.X(1,:),'k','linewidth',line_width);
axis([T(1),T(end),pwa_model.x.min(1),pwa_model.x.max(1)])
grid on
hy2=ylabel('position $x_1$');
set(gca,'YTickLabel',{''});
set(gca,'XTickLabel',{''});
set(gca,'LineWidth',axeswidth)
set(gca, 'FontSize', tick_font_size);
set(hy2, 'FontSize', label_font_size);

% plot the modes
subplot(2,1,2)
stairs(T,[index_mode1(1); index_mode1],'k','linewidth',line_width);
axis([T(1),T(end),0,5]);
grid on
hx3=xlabel('time');
hy3=ylabel('mode');
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth)
set(gca, 'FontSize', tick_font_size);
set(hx3, 'FontSize', label_font_size);
set(hy3, 'FontSize', label_font_size);



% step change in the second input
U2=[u1*ones(1,TT); ones(1,TT)];

% simulate
pwa_model.initialize(x0);
data2 = pwa_model.simulate(U2);

% identify the modes
xu2 = [data2.X(:,1:size(data2.U,2));data2.U];
mode2 = pwa_model.domain.contains(xu2);
[index_mode2,c]=find(mode2);


% plot the state trajectories
fig3=figure;
subplot(2,1,1)
plot(T,data2.X(1,:),'k','linewidth',line_width);
grid on
axis([T(1),T(end),pwa_model.x.min(1),pwa_model.x.max(1)])
hy4=ylabel('position $x_1$');
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth)
set(gca, 'FontSize', tick_font_size);
set(hy4, 'FontSize', label_font_size);

% plot the modes
subplot(2,1,2)
stairs(T,[index_mode2(1); index_mode2],'k','linewidth',line_width);
grid on
axis([T(1),T(end),0,5])
hx5=xlabel('time');
hy5=ylabel('mode');
set(gca,'YTickLabel',{''})
set(gca,'XTickLabel',{''})
set(gca,'LineWidth',axeswidth)
set(gca, 'FontSize', tick_font_size);
set(hx5, 'FontSize', label_font_size);
set(hy5, 'FontSize', label_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);


end