%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% See if the suspension wobble if it has assymmetry.
%
%%

%% Clear all

clear all;  % Clear workspace
close all;  % Close plot windows

addpath('../../utility');  % Add path to utilities

addpath('../../../sumcon/matlab');
wobbling_test20200805;
rmpath('../../../sumcon/matlab');

%%
sys1 = ss(ssA,ssB,ssC,ssD,'InputName',varinput,'OutputName',varoutput);

%%
freq=logspace(-2,2,1001);
bodesusplotopt(sys1,'azg','zTM',freq)
bodesusplotopt(sys1,'azg','xTM',freq)
bodesusplotopt(sys1,'azg','yTM',freq)

%%
t = linspace(0,30,3000);
input=[sin(0.9908*2*pi*t(1:1000)) zeros(1,2000)];
%%
inv='FzTM';
outv='pitchTM';
nin=strcmp(sys1.InputName,inv);
nout=strcmp(sys1.OutputName,outv);
sys_part = sys1(nout,nin);
[dispP,~] = lsim(sys_part,input,t);
%%
inv='FzTM';
outv='rollTM';
nin=strcmp(sys1.InputName,inv);
nout=strcmp(sys1.OutputName,outv);
sys_part = sys1(nout,nin);
[dispR,~] = lsim(sys_part,input,t);
%%
inv='FzTM';
outv='yawTM';
nin=strcmp(sys1.InputName,inv);
nout=strcmp(sys1.OutputName,outv);
sys_part = sys1(nout,nin);
[dispY,~] = lsim(sys_part,input,t);

%%
fig=figure();
subplot(3,1,1)
plot(t,dispP)
grid on
subplot(3,1,2)
plot(t,dispR)
grid on
subplot(3,1,3)
plot(t,dispY)
grid on

%%
fig=figure();
plot(dispP(2000:3000),dispR(2000:3000))
grid on
set(gca,'FontSize',12,'FontName','Times New Roman')
set(fig,'Color','white')
xlabel('PITCH','FontSize',12,'FontWeight','bold','FontName','Times New Roman')
ylabel('ROLL','FontSize',12,'FontWeight','bold','FontName','Times New Roman')
grid on;
saveas(fig,'./figs/Lissajous_PitYaw.jpg')