%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test ao/eqmotion for the solution of a torsion pendulum equation of the
% motion
%
% 25-03-2009 L. Ferraioli
%     CREATION
% 
% $Id: test_ao_eqmotion.m,v 1.1 2009/03/27 12:25:15 luigi Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Generate data

% generate signal + white noise
a = ao(plist('tsfcn','20.*sin(2.*pi.*t./800)+randn(size(t))','fs',1,'nsecs',1e5,'yunits','rad'));

%% Solve equation of Motion for the torsion pendulum

% Set pendulum parameters
I = 4.31e-5; % Moment of Inertia
T0 = 563; % Oscillatory period
Q = 2880; % Quality factor
Gam = I*(2*pi/T0)^2;

% building coefficients AO
alpha2 = cdata(I);
alpha2.setYunits(unit('kg').*unit('m').^2./unit('rad'));
alpha2 = ao(alpha2);

alpha1 = cdata(Gam./(2.*pi.*Q./T0));
alpha1.setYunits(unit('kg').*unit('m').^2./unit('rad')./unit('s'));
alpha1 = ao(alpha1);

alpha0 = cdata(Gam);
alpha0.setYunits(unit('kg').*unit('m').^2./unit('rad')./unit('s').^2);
alpha0 = ao(alpha0);

% Calculate torque output units are defined by the coefficients units
pl1 = plist('ALPHA2',alpha2,'ALPHA1',alpha1,'ALPHA0',alpha0);
b1 = eqmotion(a,pl1);
b1.simplifyYunits;
b1.setName;

% Alternative torque calculation set the targunits to get appropiate output
% units
pl2 = plist('ALPHA2',I,'ALPHA1',Gam./(2.*pi.*Q./T0),'ALPHA0',Gam,'TARGETUNITS',unit('kg').*unit('m').^2./unit('s').^2);
b2 = eqmotion(a,pl2);
b2.simplifyYunits;
b2.setName;

% plotting
iplot(b1,b2)

%% Extract TF

tf1 = tfe(a,b1,plist('Nfft',1e4));

tf2 = tfe(a,b2,plist('Nfft',1e4));

% Theorethical TF
f = logspace(-4,log10(0.5),300);
f = f.';
s = 1i.*2.*pi.*f;
I = 4.31e-5;
T0 = 563;
Q = 2880;
Gam = I*(2*pi/T0)^2;
TF = I.*s.^2 + (Gam./(2.*pi.*Q./T0)).*s + Gam;
TF = ao(plist('xvals',f,'yvals',TF,'dtype','fsdata','fs',a.fs));
TF.setYunits(tf1.yunits);
TF.setName;

% plot
iplot(TF,tf1,tf2)