% MDC1 Conversion to acceleration
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test script for the MDC1 conversion to acceleration of IFO data
% 
% $Id: test_mdc1_conv2acc_utn.m,v 1.2 2009/03/02 15:53:46 luigi Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Clear

clear all

%% Loading data

currPath = cd;
cd 'C:\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));
  
global LTPDAinvar

LTPDAinvar = { o1_1  , 1 , 'From file';...
               o12_1 , 1 , 'From file'};

cd(currPath)

            
%% =============================================================

channel1 = LTPDAinvar{1,1};
channel2 = LTPDAinvar{2,1};

%%

CH1 = channel1.lpsd;
CH2 = channel2.lpsd;
iplot(CH1)
iplot(CH2)

%% DRAG FREE

rat1pl    = plist('NUM', [0.0004659;0.1349;4.37;0.8304;0.07449;0.002978;4.403e-005], 'DEN', [1;5.046;9.609;11.05;0.01221;3.401e-006;0]);
control1 = mdc1_ifo2cont_utn(channel1,rat1pl);

%% retarded actuaction - thrusters:

retard1pl = plist('TAU', 0.1, 'DELTAT', 0.315, 'FS', 10, 'TOL', 1e-007, 'AMP', 1);
actuation1   = mdc1_cont2act_utn(control1,retard1pl);

%% LOW FREQ SUSP

rat2pl    = plist('NUM', [-2.726e-007;1.665e-005;1.303e-007;8.381e-010], 'DEN', [1;0.2189;0.01922;0.0007803;0]);
control2 = mdc1_ifo2cont_utn(channel2,rat2pl);

%% retarded actuaction - electrical actuators:

retard2pl = plist('TAU', 0.01, 'DELTAT', 0.305, 'FS', 10, 'TOL', 1e-007, 'AMP', 1);
actuation2   = mdc1_cont2act_utn(control2,retard2pl);

%%  Free dynamics:

plfd = plist('pstiff1', -1.3e-6,'pstiff2', -2e-6,'cross_talk', -1e-4,'METHOD', 'PARFIT');
freeDynamics = mdc1_ifo2acc_fd_utn(channel1,channel2,plfd);

%%  Output

output1 = freeDynamics.index(1) + actuation1;
output2 = freeDynamics.index(2) + actuation2;
% output1 = freeDynamics(1) + control1;
% output2 = freeDynamics(2) + control2;

%% ******************************************
% plotting output psd

psd1 = output1.lpsd;
psd2 = output2.lpsd;
iplot(psd1)
iplot(psd2)

%% ====================== More checks =====================================

%% Extracting filters

miir1Obj = find(control1.procinfo,'CONT_FILTER');
retard1 = find(actuation1.procinfo,'ACT_FILTER');
miir2Obj = find(control2.procinfo,'CONT_FILTER');
retard2 = find(actuation2.procinfo,'ACT_FILTER');

%% Checking controllers

% frequencies
f = logspace(-6,log10(5),300);
% f = logspace(-6,0,300);
f = f.';

% response of continuous drag free
Rcdf = resp(rat1Obj,f);
Rcdf.setName('Continuous DF');

% response of discrete drag free
tRddf = resp(miir1Obj,plist('f',f));
Rddf = tRddf(1);
for ii = 2:numel(tRddf)
  Rddf = Rddf + tRddf(ii);
end
Rddf.setName('Discrete DF');

% response of continuous low freq susp
Rclfs = resp(rat2Obj,f);
Rclfs.setName('Continuous LFS');

% response of discrete low freq susp
tRdlfs = resp(miir2Obj,plist('f',f));
Rdlfs = tRdlfs(1);
for ii = 2:numel(tRdlfs)
  Rdlfs = Rdlfs + tRdlfs(ii);
end
Rdlfs.setName('Discrete LFS');

% response of actuation filter 1
Ract1 = resp(retard1,plist('f',f));
Ract1.setName('Thrusters');

% response of actuation filter 2
Ract2 = resp(retard2,plist('f',f));
Ract1.setName('Actuators');

%% Plotting reposnse

% Continuous and discrete controllers
pl = plist('Legends', {'cH_{df}','dH_{df}','cH_{lfs}','dH_{lfs}'},...
  'LineStyles', {'', '--', '', '--'});
iplot(Rcdf,Rddf,Rclfs,Rdlfs,pl)

% Ratio between discrete and continuous
pl = plist('Legends', {'dH_{df}/cH_{df}','dH_{lfs}/cH_{lfs}'},...
  'YScales',{'All','lin'});
iplot(Rddf./Rddf,Rdlfs./Rclfs,pl)

% Actuators
pl = plist('Legends', {'Thrusters','El. Act.'},...
  'YScales',{'All','lin'});
iplot(Ract1,Ract2,pl)

% discrete controllers + actuation
pl = plist('Legends', {'dH_{df}','dH_{df} + Tht','dH_{lfs}','dH_{lfs} + Act'},...
  'LineStyles', {'', '--', '', '--'});
iplot(Rddf,Rddf.*Ract1,Rdlfs,Rdlfs.*Ract2,pl)

%% Calculating transfer functions

% controller ***
pl  = plist('Nfft', fs*1000);
tfdf = tfe(channel1,control1,pl);
tflfs = tfe(channel2,control2,pl);
plpl = plist('Legends', {'H_{df}','H_{df} data','H_{lfs}','H_{lfs} data'},...
  'LineStyles', {'', '--', '', '--'});
iplot(Rddf,tfdf(1,2),Rdlfs,tflfs(1,2),plpl)

% Actuation ***
pl  = plist('Nfft', fs*1000);
tfact1 = tfe(control1,actuation1,pl);
tfact2 = tfe(control2,actuation2,pl);
plpl = plist('Legends', {'Tht','Tht data','El Act','El Act data'},...
  'LineStyles', {'', '--', '', '--'});
iplot(Ract1,tfact1(1,2),Ract2,tfact2(1,2),plpl)

% Free Dynamics TFs ***
pl  = plist('Nfft', fs*10000);
tffd1 = tfe(channel1,freeDynamics(1),pl);
tffd2 = tfe(channel2,freeDynamics(2),pl);

% Theorethical
% w1 = -1.3e-6;
% w2 = -2e-6;
% D1 = -1e-4;
% s = 2.*pi.*1i.*f;
% FD1 = s.^2 + w1;
% FD2 = -D1.*s.^2 + w2 - w1 - D1*w2 + s.^2 + w2;
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));

plpl = plist('Legends', {'FD1','FD1 data','FD2','FD2 data'},...
  'LineStyles', {'', '--', '', '--'});
iplot(abs(FD1),abs(tffd1(1,2)),abs(FD2),abs(tffd2(1,2)),plpl)