%%% Find the constant FF filters:

%2018-08-20. Edgard Bonilla:
%This function loads Transfer function mearements form ST2L -> M0 and M0 ->
%M0 to find the value of the QUAD ISIFF filters, as described in T1800301.

%All the filters here are in SI units, and will need to be calibrated.
% The L2L [cts/nm]=(L2L [N/m])*10^-3/1.2851; %the calibration factor is defined in G1100968 
% The L2P filter is mostly to compensate for the M0 L drive cross coupling to P
% below ~ 0.15 Hz. 

% As such, the correct value for the filter should be found by obtaining the effective lever arm l
%l [m]=L2P/L2L. And the calibration for the L2P filter follows. 
%
%IMPORTANT NOTE: If l is almost a pure imaginary number then the effective lever arm is
%actually ~0 and the noise is eating our measurements for it. 
% A possible workaround is to take the real part of l as the lever arm, but
% it is unclear if it will be good
%___________________________________________________________________________________




%read the measurements
%ETMX
% quadname='ETMX';
% ST2L_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-18_H1SUSETMX_M0_ISIFF_FilterDesign_ISIST2L_2_M0L_tf.txt');
% ST2L_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-18_H1SUSETMX_M0_ISIFF_FilterDesign_ISIST2L_2_M0P_tf.txt');
% 
% M0P_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-19_H1SUSETMX_M0_ISIFF_FilterDesign_M0P_2_M0P_tf.txt');
% M0P_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-19_H1SUSETMX_M0_ISIFF_FilterDesign_M0P_2_M0L_tf.txt');
% 
% M0L_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-19_H1SUSETMX_M0_ISIFF_FilterDesign_M0L_2_M0P_tf.txt');
% M0L_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ETMX/Common/Data/2018-08-19_H1SUSETMX_M0_ISIFF_FilterDesign_M0L_2_M0L_tf.txt');

% %ITMX
quadname='ITMX';
ST2L_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-18_H1SUSITMX_M0_ISIFF_FilterDesign_ISIST2L_2_M0L_tf.txt');
ST2L_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-18_H1SUSITMX_M0_ISIFF_FilterDesign_ISIST2L_2_M0P_tf.txt');

M0P_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-19_H1SUSITMX_M0_ISIFF_FilterDesign_M0P_2_M0P_tf.txt');
M0P_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-19_H1SUSITMX_M0_ISIFF_FilterDesign_M0P_2_M0L_tf.txt');

M0L_2_M0P=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-19_H1SUSITMX_M0_ISIFF_FilterDesign_M0L_2_M0P_tf.txt');
M0L_2_M0L=dlmread('/ligo/svncommon/SusSVN/sus/trunk/QUAD/H1/ITMX/Common/Data/2018-08-19_H1SUSITMX_M0_ISIFF_FilterDesign_M0L_2_M0L_tf.txt');
%% calibrate the tfs: (after this step everything is in SI units) 
calibration =1.2851; %Taken from G1100968
ST2L_2_M0L(:,2)=ST2L_2_M0L(:,2)*10^3;%[um/nm]->[m/m]
ST2L_2_M0P(:,2)=ST2L_2_M0P(:,2)*10^3; %[urad/nm]->[rad/m]

M0P_2_M0P(:,2)=M0P_2_M0P(:,2)/calibration;
M0P_2_M0L(:,2)=M0P_2_M0L(:,2)/calibration;

M0L_2_M0P(:,2)=M0L_2_M0P(:,2)/calibration;
M0L_2_M0L(:,2)=M0L_2_M0L(:,2)/calibration;

%% Get the complex amplitude/phase:
Gll=ST2L_2_M0L(:,2).*exp(1j*ST2L_2_M0L(:,3))+1;
Gpl=ST2L_2_M0P(:,2).*exp(1j*ST2L_2_M0P(:,3));

Tpp=M0P_2_M0P(:,2).*exp(1j*M0P_2_M0P(:,3));
Tlp=M0P_2_M0L(:,2).*exp(1j*M0P_2_M0L(:,3));

Tpl=M0L_2_M0P(:,2,:).*exp(1j*M0L_2_M0P(:,3));
Tll=M0L_2_M0L(:,2,:).*exp(1j*M0L_2_M0L(:,3));
%% Invert a matrix for getting k and l
FF_l=zeros(length(M0L_2_M0P),2);
FF_p=zeros(length(M0L_2_M0P),2);
for ii=1:length(Tll)
    FF_l(ii,1)=M0L_2_M0P(ii,1);
    FF_p(ii,1)=M0L_2_M0P(ii,1);
    A=inv([Tll(ii) Tlp(ii); Tpl(ii) Tpp(ii)]);
    FF_l(ii,2)=-A(1,:)*[Gll(ii); Gpl(ii)];
    FF_p(ii,2)=-A(2,:)*[Gll(ii); Gpl(ii)];
end

%% Plot the Result:
set(0,'DefaultAxesFontSize',16);
set(0,'DefaultAxesXGrid','on');
set(0,'DefaultAxesYGrid','on');

% % L to L filter
figure,
subplot(4,1,1:3) %3/4 of the plot for magnitude
magnitude_ll=loglog(FF_l(:,1),abs(FF_l(:,2)),'linewidth',3);

xlim([0.01 4])
grid on 
title([quadname ' L2L ISIFF filter'])
ylabel('Magnitude [N/m]')
subplot(4,1,4) %1/4 of the plot for phase
phase_ll=semilogx(FF_l(:,1),180/pi*angle(FF_l(:,2)),'linewidth',3);

ylim([-185 185])
set(gca,'YTick',-180:45:180)
xlim([0.01 4])
grid on 
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')

% P to P filter
figure,
subplot(4,1,1:3) %3/4 of the plot for magnitude
magnitude_ll=loglog(FF_p(:,1),abs(FF_p(:,2)),'linewidth',3);

xlim([0.01 4])
grid on 
title([quadname ' L2P ISIFF filter'])
ylabel('Magnitude [N.m/m]')
subplot(4,1,4) %1/4 of the plot for phase
phase_ll=semilogx(FF_p(:,1),180/pi*angle(FF_p(:,2)),'linewidth',3);

ylim([-185 185])
set(gca,'YTick',-180:45:180)
xlim([0.01 4])
grid on 
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')

%lever arm factor (L2P/L2L)
figure,
subplot(4,1,1:3) %3/4 of the plot for magnitude
magnitude_ll=loglog(FF_p(:,1),abs(FF_p(:,2)./FF_l(:,2)),'linewidth',3);

xlim([0.01 4])
grid on 
title([quadname ' ISIFF Lever arm factor (L2P/L2L)'])
ylabel('Magnitude [m]')
subplot(4,1,4) %1/4 of the plot for phase
phase_ll=semilogx(FF_p(:,1),180/pi*angle(FF_p(:,2)./FF_l(:,2)),'linewidth',3);

ylim([-185 185])
set(gca,'YTick',-180:45:180)
xlim([0.01 4])
grid on 
ylabel('Phase [deg]')
xlabel('Frequency [Hz]')
