% MDC1 Conversion to acceleration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test script for the MDC1 conversion to acceleration of IFO data
% 
% $Id: test_ao_ltp_ifo2acc.m,v 1.6 2009/12/10 15:35:38 luigi Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Clear

clear all

%% Loading data

currPath = cd;
cd 'C:\Users\Luigi\Dati\mock_data\interferometer\';

j=1;
fs = 10;

disp(sprintf('*** Reading data %d', j));
in = load(sprintf('mockdata_16_48_17_11_2007_%d.dat', j));
d1  = in(:,2);
d12 = in(:,3);

% Build O1
eval('ts = tsdata(d1, fs);');
eval(sprintf('o1_%d = ao(ts);', j));
eval(sprintf('o1_%d = setName(o1_%d, ''o1'');', j, j));
eval(sprintf('o1_%d = setXunits(o1_%d, ''s'');', j, j));
eval(sprintf('o1_%d = setYunits(o1_%d, ''m'');', j, j));

% Build O12
eval('ts = tsdata(d12, fs);');
eval(sprintf('o12_%d = ao(ts);', j));
eval(sprintf('o12_%d = setName(o12_%d, ''o12'');', j, j));
eval(sprintf('o12_%d = setXunits(o12_%d, ''s'');', j, j));
eval(sprintf('o12_%d = setYunits(o12_%d, ''m'');', j, j));

cd(currPath)

eval(sprintf('ch1 = o1_%d;',j));
eval(sprintf('ch2 = o12_%d;',j));

%% Save

% save(ch1, 'C:\Dati\mock_data\interferometer\mdc1_ch1.mat')
% save(ch2, 'C:\Dati\mock_data\interferometer\mdc1_ch12.mat')

%% Check data spectra

ch1xx = ch1.psd;
ch2xx = ch2.psd;
iplot(ch1xx,ch2xx)

%% Convert to acceleration

% default settings
[a1,ad] = ltp_ifo2acc(ch1,ch2);

%% check spectra

a1xx = a1.psd;
adxx = ad.psd;
iplot(a1xx,adxx)


%% ************************************************************************

%% Set useful params
% corresponding to default values

fs = ch1.fs; % Hz - sampling frequency

TAU_th = 0.1; % s - time constant of thrusters actuation
DELTAT_Th = 0.315; % s - delay introduced to command the force to thrusters system

TAU_el = 0.01; % s - time constant of electrostatic actuation
DELTAT_el = 0.305; % s - delay introduced to command the force to electrostatic actuators

w1 = 0.00114i; % TM1 stiffness
w2 = 0.00141i; % TM2 stiffness

S11 = 1; % Calibration of IFO CH1
S1D = 0; % Cross-talk diff channel to channel 1
SD1 = -1e-4; % Cross-talk channel 1 to diff channel
SDD = 1; % Calibration of IFO diff channel

m1 = 1.96; % TM1 mass
m2 = 1.96; % TM2 mass
msc = 422.7; % SC mass
Gamma = 4.92e-9; % Gravitational gradient


%% Do step-by-step Checks

% get mdc1 ifo noise psd models
currPath = cd;
cd 'C:\Users\Luigi\ltp_data_analysis\MDCs\MDC1_UTN\';


So = matrix([cd '\CSD_mod.mat']);

cd(currPath)

% frequency range for TFs calculation
f = So.objs(1,1).x;

% set units for So
for ii=1:numel(So.objs)
  So.objs(ii).setYunits('m^2 Hz^-1');
end


%% Get intermediate steps

% get just Free Dynamics
[a1fd,adfd] = ltp_ifo2acc(ch1,ch2,plist('Hdf',0,'Hsus',0,'Adf',0,'Asus',0));

% get actuated force per unit of mass
[a1c,adc] = ltp_ifo2acc(ch1,ch2,plist('FreeDyn_1',0,'FreeDyn_Delta',0));


%% IFO TFs ***

% Getting models
IFO11 = smodel('S11');
IFO11.setParams({'S11'},{S11});
IFO11.setXvar('f');
IFO11.setXvals(f);
IFO11.setXunits('Hz');
IFO11.setName;

IFO1D = smodel('S1D');
IFO1D.setParams({'S1D'},{S1D});
IFO1D.setXvar('f');
IFO1D.setXvals(f);
IFO1D.setXunits('Hz');
IFO1D.setName;

IFOD1 = smodel('SD1');
IFOD1.setParams({'SD1'},{SD1});
IFOD1.setXvar('f');
IFOD1.setXvals(f);
IFOD1.setXunits('Hz');
IFOD1.setName;

IFODD = smodel('SDD');
IFODD.setParams({'SDD'},{SDD});
IFODD.setXvar('f');
IFODD.setXvals(f);
IFODD.setXunits('Hz');
IFODD.setName;

% Build the matrix
IFO = matrix([IFO11 IFO1D;IFOD1 IFODD]);


%% Free Dynamics TFs ***

% Getting models
FD11 = smodel('(2.*pi.*1i.*f).^2 + (1+m1/msc)*(w1^2) + (m2/msc)*(w2^2)');
FD11.setParams({'m1','m2','msc','w1','w2'},{m1,m2,msc,w1,w2});
FD11.setXvar('f');
FD11.setXvals(f);
FD11.setYunits('s^-2');
FD11.setXunits('Hz');
FD11.setName;

FD12 = smodel('(m2/msc)*(w2^2) + Gamma');
FD12.setParams({'m2','msc','w2','Gamma'},{m2,msc,w2,Gamma});
FD12.setXvar('f');
FD12.setXvals(f);
FD12.setYunits('s^-2');
FD12.setXunits('Hz');
FD12.setName;

FD21 = smodel('(w2^2) - (w1^2)');
FD21.setParams({'w1','w2'},{w1,w2});
FD21.setXvar('f');
FD21.setXvals(f);
FD21.setYunits('s^-2');
FD21.setXunits('Hz');
FD21.setName;

FD22 = smodel('(2.*pi.*1i.*f).^2 + (w2^2) - 2*Gamma');
FD22.setParams({'w2','Gamma'},{w2,Gamma});
FD22.setXvar('f');
FD22.setXvals(f);
FD22.setYunits('s^-2');
FD22.setXunits('Hz');
FD22.setName;

% Build the matrix
FD = matrix([FD11 FD12;FD21 FD22]);

% Transfer function from IFO output --> total acceleration
FDO = FD*inv(IFO);

FDO11 = eval(FDO.objs(1,1));
FDO12 = eval(FDO.objs(1,2));
FDO21 = eval(FDO.objs(2,1));
FDO22 = eval(FDO.objs(2,2));

FDOe = matrix([FDO11 FDO12;FDO21 FDO22]);

% expected PSD for total acceleration
Sfd = FDOe*So*conj(transpose(FDOe));

% eval results
Sfd11 = (2/fs).*abs(Sfd.objs(1,1));
Sfd12 = (2/fs).*Sfd.objs(1,2);
Sfd22 = (2/fs).*abs(Sfd.objs(2,2));

% get PSD from data
plpsd = plist('nfft',1e5);
Pfd11 = psd(a1fd,plpsd);
Pfd22 = psd(adfd,plpsd);
% plcsd = plist('nfft',5e3);
% Pfd12 = csd(a1fd,adfd,plcsd);

%% plot
plpl1 = plist('Legends', {'S_{a1a1} data','S_{a1a1}','S_{aDaD} data','S_{aDaD}'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(sqrt(Pfd11),sqrt(Sfd11),sqrt(Pfd22),sqrt(Sfd22),plpl1)
% iplot(Pfd11,Sfd11,Pfd22,Sfd22,plpl1)

% plpl2 = plist('Legends', {'S_{a1aD} data','S_{a1aD}'},...
%   'LineStyles', {'', '--'},'LineWidths',{'All',3});
% iplot(Pfd12,Sfd12,plpl2)


%% Controllers TFs ***

% Get expected transfer functions
Cdf = miir(plist('built-in','DFACS_x_Hdf'));
Cdfr = resp(Cdf,plist('f',f,'bank','parallel'));

Csus = miir(plist('built-in','DFACS_M3_x_Hsus'));
Csusr = resp(Csus,plist('f',f,'bank','parallel'));

% Control matrix
H = matrix([Cdfr ao(0);ao(0) Csusr]);

%% Actuation TFs ***
% Get expected transfer functions
% ThActr = ao(plist('fsfcn', '10.*(1./(1 + (2.*pi.*1i.*f)*0.1)).*(1./(2.*pi.*1i.*f)).*(exp(-(2.*pi.*1i.*f).*0.315)-exp(-(2.*pi.*1i.*f).*(0.315+0.1)))', 'f', f));
% ElActr = ao(plist('fsfcn', '10.*(1./(1 + (2.*pi.*1i.*f)*0.01)).*(1./(2.*pi.*1i.*f)).*(exp(-(2.*pi.*1i.*f).*0.305)-exp(-(2.*pi.*1i.*f).*(0.305+0.1)))', 'f', f));

% Get expected transfer functions
pl = plist('built-in','mdc1_Adf');
Adf = miir(pl);
Adfr = resp(Adf,plist('f',f));

pl = plist('built-in','mdc1_Asus');
Asus = miir(pl);
Asusr = resp(Asus,plist('f',f));

% Actuation matrix
A = matrix([Adfr ao(0);ao(0) Asusr]);

%% Get expected spectra and measured

C = matrix([ao(1) ao(-m2/msc);ao(0) ao(-1)]);
Hnorm = matrix([ao(1/msc) ao(0);ao(0) ao(1/m2)]);
tfc = C*A*Hnorm*H;
for ii =1:numel(tfc.objs)
  tfc.objs(ii).setYunits(tfc.objs(ii).yunits/unit('kg'));
  tfc.objs(ii).simplifyYunits;
end
Sc = tfc*So*conj(transpose(tfc));

% get PSD from data
plpsd = plist('nfft',1e5);
Pc11 = psd(a1c,plpsd);
Pc22 = psd(adc,plpsd);
% plcsd = plist('nfft',5e3);
% Pc12 = csd(a1c,adc,plcsd);

% plot
plpl1 = plist('Legends', {'S_{a1a1} data','S_{a1a1}','S_{aDaD} data','S_{aDaD}'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
% iplot(Pc11,(2/fs).*abs(Sc.objs(1,1)),Pc22,(2/fs).*abs(Sc.objs(2,2)),plpl1)
iplot(sqrt(Pc11),sqrt((2/fs).*abs(Sc.objs(1,1))),sqrt(Pc22),sqrt((2/fs).*abs(Sc.objs(2,2))),plpl1)

% plpl2 = plist('Legends', {'S_{a1aD} data','S_{a1aD}'},...
%   'LineStyles', {'', '--'},'LineWidths',{'All',3});
% iplot(Pc12,Sc.objs(1,2),plpl2)


%% Full model of acceleration noise

TF = FDOe + tfc;
Stot = TF*So*conj(transpose(TF));

% plot
plpl = plist('Legends', {'Ch1 data','Ch1','Ch12 data','Ch12'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(sqrt(a1xx),sqrt((2/fs).*abs(Stot.objs(1,1))),sqrt(adxx),sqrt((2/fs).*abs(Stot.objs(2,2))),plpl)
% iplot(a1xx,(2/fs).*abs(Stot.objs(1,1)),adxx,(2/fs).*abs(Stot.objs(2,2)),plpl)



%% Test with zero input

nsecs = ch1.nsecs;

ch1_0 = ao(plist('tsfcn', 'zeros(size(t))', 'fs', 10, 'nsecs', nsecs, 'yunits','m'));
ch2_0 = ao(plist('tsfcn', 'zeros(size(t))', 'fs', 10, 'nsecs', nsecs, 'yunits','m'));

%% %%% %%% %%%

[a1_0,ad_0] = ltp_ifo2acc(ch1_0,ch2_0);

% output must be zero
plpsd = plist('nfft',1e5);
a1_0xx = psd(a1_0,plpsd);
ad_0xx = psd(ad_0,plpsd);

% plot
plpl = plist('Legends', {'Ch1 data','Ch12 data'},...
  'LineStyles', {'', ''},'LineWidths',{'All',3});
iplot(sqrt(a1_0xx),sqrt(ad_0xx),plpl)

%% %%% %%% %%%

[a1_1,ad_1] = ltp_ifo2acc(ch1,ch2_0);

TF = FDOe + tfc;
So1 = copy(So,1);
for ii = 2:numel(So1.objs)
  So1.objs(ii).setY(zeros(size(So1.objs(ii).x)));
end
Stot1 = TF*So1*conj(transpose(TF));

plpsd = plist('nfft',1e5);
a1_1xx = psd(a1_1,plpsd);
ad_1xx = psd(ad_1,plpsd);

% plot
plpl = plist('Legends', {'Ch1 data','Ch1','Ch12 data','Ch12'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(sqrt(a1_1xx),sqrt((2/fs).*abs(Stot1.objs(1,1))),sqrt(ad_1xx),sqrt((2/fs).*abs(Stot1.objs(2,2))),plpl)

%% %%% %%% %%%

[a1_2,ad_2] = ltp_ifo2acc(ch1_0,ch2);

TF = FDOe + tfc;
So2 = copy(So,1);
for ii = 1:numel(So2.objs)-1
  So2.objs(ii).setY(zeros(size(So2.objs(ii).x)));
end
Stot2 = TF*So2*conj(transpose(TF));

plpsd = plist('nfft',1e5);
a1_2xx = psd(a1_2,plpsd);
ad_2xx = psd(ad_2,plpsd);

% plot
plpl = plist('Legends', {'Ch1 data','Ch1','Ch12 data','Ch12'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(sqrt(a1_2xx),sqrt((2/fs).*abs(Stot2.objs(1,1))),sqrt(ad_2xx),sqrt((2/fs).*abs(Stot2.objs(2,2))),plpl)