% MDC1 Conversion to acceleration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test script for the MDC1 conversion to acceleration of IFO data
% 
% $Id: test_mdc1_ifo2acc.m,v 1.2 2009/09/24 15:17:36 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));

%% Check data spectra

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

%% Set useful params

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

%% Load built in model for Drag Free

pl = plist('built-in','mdc1_Cdf','gain',1);
Cdf = rational(pl);

%% Load Built in model for Thrusters

pl = plist('built-in','mdc1_actuation','fs',fs,'tau',TAU_th,'DeltaT',DELTAT_Th);
ThAct = miir(pl);

%% Load built in model for Suspension controller

pl = plist('built-in','mdc1_Csus','gain',1);
Csus = rational(pl);

%% Load Built in model for Electrostatic actuators

pl = plist('built-in','mdc1_actuation','fs',fs,'tau',TAU_el,'DeltaT',DELTAT_el);
ElAct = miir(pl);

%% Convert to acceleration

pl = plist('w1',w1,'w2',w2,'S11',S11,'S1D',S1D,'SD1',SD1,'SDD',SDD,...
  'Cdf',Cdf,'Csus',Csus,'ThAct',ThAct,'ElAct',ElAct,'method','parfit');
[a1,ad] = ltp_ifo2acc(ch1,ch2,pl);

%% plotting output psd

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

%% plotting output lpsd

a1xx = a1.lpsd;
adxx = ad.lpsd;
iplot(a1xx,adxx,plist('LineWidths',{'All',3}))

%% Do step-by-step Checks

% frequency range for TFs calculation
f = [logspace(-6,log10(5),300)]';


%% Get intermediate steps

%%% IFO2POS conversion
ifod = S11*SDD - S1D*SD1;
x1 = (ch1.*SDD - ch2.*S1D)./ifod;
xd = (ch2.*S11 - ch1.*SD1)./ifod;


%%% Free Dynamics
sw1 = w1^2;
sw2 = w2^2;

D0COEFF = -3/35;
D1COEFF = -1/5;
D2COEFF = 2/7;

pldyn = plist('ALPHA0',sw1,'ALPHA1',0,'ALPHA2',1,...
  'D0COEFF',D0COEFF,'D1COEFF',D1COEFF,'D2COEFF',D2COEFF,...
  'X0',0,'TARGETUNITS',x1.yunits./(unit('s').^2));
a1fd = eqmotion(x1,pldyn);

% Total Differential force per unit of mass
pldyn = plist('ALPHA0',sw2,'ALPHA1',0,'ALPHA2',1,...
  'D0COEFF',D0COEFF,'D1COEFF',D1COEFF,'D2COEFF',D2COEFF,...
  'X0',0,'TARGETUNITS',x1.yunits./(unit('s').^2));
adfd = eqmotion(xd,pldyn);

% calculate dynamical cross talk
adct = x1.*(sw2 - sw1);
adct.setYunits(adfd.yunits);

% Total differential force
adfd = adfd + adct;

%%% DF Controller

parfrCdf = parfrac(Cdf);
filtCdf = miir(parfrCdf,plist('FS',fs));
ac1 = filter(ch1, filtCdf);

%%% Thrusters actuation
acomm1 = filter(ac1, ThAct);

%%% Sus controller
parfrCsus = parfrac(Csus);
filtCsus = miir(parfrCsus,plist('FS',fs));
acd = filter(ch2, filtCsus);

%%% Electrostatic actuation
acommd = filter(acd, ElAct);


%% Free Dynamics TFs ***

% get transfer functions from data
tffd1 = ltfe(ch1,a1fd);
tffd2 = ltfe(ch2,adfd);

% Get expected transfer functions
FD1 = ao(plist('fsfcn', '(2.*pi.*1i.*f).^2 + -1.3e-6', 'f', f));
FD2 = ao(plist('fsfcn', '-(-1e-4).*(2.*pi.*1i.*f).^2 + (-2e-6) - (-1.3e-6) - (-1e-4)*(-2e-6) + (2.*pi.*1i.*f).^2 + (-2e-6)', 'f', f));

% plot
plpl = plist('Legends', {'Dyn ch1','Dyn ch1 data','Dyn ch12','Dyn ch12 data'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(abs(FD1),abs(tffd1),abs(FD2),abs(tffd2),plpl)

%% Controllers TFs ***

% get transfer functions from data
tffd1 = ltfe(ch1,ac1);
tffd2 = ltfe(ch2,acd);

% Get expected transfer functions
pl = plist('built-in','mdc1_Cdf','gain',1);
Cdf = rational(pl);
Cdfr = resp(Cdf,plist('f',f));

pl = plist('built-in','mdc1_Csus','gain',1);
Csus = rational(pl);
Csusr = resp(Csus,plist('f',f));

% plot
plpl = plist('Legends', {'Cdf','Cdf data','Csus','Csus data'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(Cdfr,tffd1,Csusr,tffd2,plpl)

%% Actuation TFs

% get transfer functions from data
tffd1 = ltfe(ac1,acomm1);
tffd1.simplifyYunits;
tffd2 = ltfe(acd,acommd);
tffd2.simplifyYunits;

% 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

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

% plot
plpl = plist('Legends', {'Thrusters','Thrusters data','Electrostatic','Electrostatic data'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(ThActr,tffd1,ElActr,tffd2,plpl)


%% Full model of acceleration noise

% get mdc1 ifo noise psd models
CSD = get_2D_test_obj_tf_psd(f);

% free dynamics
FD11 = ao(plist('fsfcn', '(2.*pi.*1i.*f).^2 + -1.3e-6', 'f', f));
FD21 = ao(plist('fsfcn', '-(-1e-4).*(2.*pi.*1i.*f).^2 + (-2e-6) - (-1.3e-6) - (-1e-4)*(-2e-6)', 'f', f));
FD22 = ao(plist('fsfcn', '-(-1e-4).*(2.*pi.*1i.*f).^2 + (-2e-6) - (-1.3e-6) - (-1e-4)*(-2e-6) + (2.*pi.*1i.*f).^2 + (-2e-6)', 'f', f));

Cdfr = resp(Cdf,plist('f',f));
Csusr = resp(Csus,plist('f',f));
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));


TF11 = FD11 + Cdfr.*ThActr;

TF22 = FD22 + Csusr.*ElActr;

Acc1 = (2/fs).*TF11.*CSD(1,1).*conj(TF11);
Accd = (2/fs).*(FD21.*CSD(2,1).*conj(FD21) + TF22.*CSD(2,2).*conj(TF22));

% plot
plpl = plist('Legends', {'Ch1 data','Ch1','Ch12 data','Ch12'},...
  'LineStyles', {'', '--', '', '--'},'LineWidths',{'All',3});
iplot(a1xx,abs(Acc1),adxx,abs(Accd),plpl)