%% ME - 221, ss, initial, lsim and step functions - Example
% run sections one by one and read the comments
% Initiate the script
clc; clear; close all

%% Assume that we obtained the following state-space equations: 
% dx1 = x3;
% dx2 = x4;
% dx3 = -5/58*x1 - 1/116*x2 - 60/23*x3 - 5/23*x4  + 2*u1 - u2;
% dx4 = -1/58*x1 - 3/58*x2  - 10/23*x3 - 20/23*x4 + u2;
% y1 = x1;
% y2 = x1 + 2*x2;

% Then, A,B,C and D matrices are:
Ass = [0 0 1 0; 0 0 0 1; -5/58 -1/116 -60/23 -5/23; -1/58 -3/58 -10/23 -20/23];
Bss = [0 0; 0 0; 2 -1; 0 1];
Css = [1 0 0 0; 1 2 0 0];
Dss = [0 0; 0 0];

% Building a linear system in matlab using the ss function: (read documentation for exta information)
Linear_system = ss(Ass,Bss,Css,Dss);

% There are many functions that can be used to study linear systems, for now we focus on two:
%% 1) initial - simulates the response to initial conditions 
x0 = [1 -2 1 0].';
tt = linspace(0,100,1e3).';
Y = initial(Linear_system,x0,tt);

figure(1);
plot(tt,Y,'LineWidth',2)
xlabel('$t$(s)','interpreter','Latex')
ylabel('$Y$(units)','interpreter','Latex')
title('Linear system response to intial conditions','interpreter','Latex')
hl = legend('$Y_1$','$Y_2$');
set(hl,'interpreter','Latex')
set(gca,'FontSize',12)
set(gcf,'color','w')

%% 2) lsim - simulates the response to an input (forced response)
U = [2*sin(10*tt) exp(-0.1*tt).*(sin(0.5*tt))].';
Yf = lsim(Linear_system,U,tt,x0);

figure(2);
plot(tt,Yf,'LineWidth',2)
xlabel('$t$(s)','interpreter','Latex')
ylabel('$Y$(units)','interpreter','Latex')
title('Linear system forced response','interpreter','Latex')
hl = legend('$Y_1$','$Y_2$');
set(hl,'interpreter','Latex')
set(gca,'FontSize',12)
set(gcf,'color','w')

%% 3) step - simulates the response to a step force
Ys = step(Linear_system,tt); 
SIZE_Ys = size(Ys) ;
% Notice that the size is 1000 by 2 by 2: (length?of?t)×(number?of?outputs)×(number?of?inputs)
% If we want to plot the response of the two outputs to the first input
Y12_1 = squeeze(Ys(:,:,1));
% If we want to plot the response of the two outputs to the second input
Y12_2 = squeeze(Ys(:,:,2));
% If we want to plot the response of the two outputs to 3u1 - 2u2
Y12_12 = 3*Y12_1-2*Y12_2;

figure(4);
plot(tt,Y12_1,'LineWidth',2)
xlabel('$t$(s)','interpreter','Latex')
ylabel('$Y$(units)','interpreter','Latex')
title('Linear system step response to $u_1$','interpreter','Latex')
hl = legend('$Y_1$','$Y_2$');
set(hl,'interpreter','Latex')
set(gca,'FontSize',12)
set(gcf,'color','w')

figure(5);
plot(tt,Y12_2,'LineWidth',2)
xlabel('$t$(s)','interpreter','Latex')
ylabel('$Y$(units)','interpreter','Latex')
title('Linear system step response to $u_2$','interpreter','Latex')
hl = legend('$Y_1$','$Y_2$');
set(hl,'interpreter','Latex')
set(gca,'FontSize',12)
set(gcf,'color','w')

figure(6);
plot(tt,Y12_12,'LineWidth',2)
xlabel('$t$(s)','interpreter','Latex')
ylabel('$Y$(units)','interpreter','Latex')
title('Linear system step response to $3u_1-2u_2$','interpreter','Latex')
hl = legend('$Y_1$','$Y_2$');
set(hl,'interpreter','Latex')
set(gca,'FontSize',12)
set(gcf,'color','w')
