clear all
%% get data


addpath /ligo/svncommon/NbSVN/aligonoisebudget/trunk/Externals/SimulinkNb/Utils/
addpath /ligo/svncommon/SeiSVN/seismic/Common/MatlabTools/


gwd = GWData();
gwd.make_kerberos_ready;
gwd.site_info(2).port = 31200;
gwd.site_info(2).server = 'nds.ligo-wa.caltech.edu';

startTime1 = 1150459291; % 20 June 2016 ~17:01 UTC, Moderate winds for 5 hours
duration = 3600*5; 
channelList = {...
    'H1:ISI-GND_STS_ETMX_X_DQ',...1
    'H1:ISI-GND_BRS_ETMX_RY_OUT_DQ',...2
    'H1:ISI-GND_BRS_ETMY_RX_OUT_DQ',...3
    'H1:ISI-GND_STS_ETMY_Y_DQ',...4
    'H1:ISI-GND_STS_ITMY_X_DQ',...5
    'H1:ISI-GND_STS_ITMY_Y_DQ',...6
    };
dataRate = 256;

[dataIn, tIn,~] =gwd.fetch(startTime1,duration,channelList,dataRate);
dataIn = 1e-9*dataIn; % Converting nm/s and nrad into m/s and rad


% % % Saving data to reduce NDS transactions
% % save('data1150459291.mat', 'tIn', 'dataIn')

% % load 'data1150459291.mat'
% % dataRate = 256;


%% Data decimation, naming
data = dataIn;
t = tIn;

R1 = 8;  % Decimation factor 1
datax = zeros(size(data,1)/R1, size(data,2));
for n = 1:size(data,2)
    datax(:,n) = decimate(data(:,n),R1);
end
R2 = 8;  % Decimation factor
data1 = zeros(size(datax,1)/R2, size(datax,2));
for n = 1:size(datax,2)
    data1(:,n) = decimate(datax(:,n),R2);
end
t = downsample(t,R1*R2);


%
ts = t(2) - t(1);

gndex = data1(:,1);  
brsex = data1(:,2); 
brsey = data1(:,3); 
gndey = data1(:,4);  
gndix = data1(:,5);  
gndiy = data1(:,6);  

%% Filter extraction and definition for sensor correction

% Extracting BRS filters from ETMX ROTVEL 1,2,3 6,7
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_ROTVEL',1);
fm1 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_ROTVEL',2);
fm2 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_ROTVEL',3);
fm3 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_ROTVEL',6);
fm6 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_ROTVEL',7);
fm7 = zpk(z,p,k);
rotvelex = fm1*fm2*fm3*fm6*fm7;

% Extracting BRS filters for ETMX STS2 Torque correction
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_TORQUE',1);
fm1 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMX.txt','GND_SENSCOR_ETMX_STS_X_TORQUE',2);
fm2 = zpk(z,p,k);
torqex = fm1*fm2;


% Extracting BRS filters from ETMY ROTVEL 1,2,3
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMY.txt','GND_SENSCOR_ETMY_STS_Y_ROTVEL',1);
fm1 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMY.txt','GND_SENSCOR_ETMY_STS_Y_ROTVEL',2);
fm2 = zpk(z,p,k);
[z, p, k] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIETMY.txt','GND_SENSCOR_ETMY_STS_Y_ROTVEL',3);
fm3 = zpk(z,p,k);
rotveley = fm1*fm2*fm3;

% Extracting current Sensor-correction filter zpk system from Foton filters, creating zpks
[z, p, k, name1] = readFilterzpk('/opt/rtcds/userapps/release/isi/h1/filterfiles/H1ISIITMY.txt','ITMY_ST1_SENSCOR_GND_STS_X_FIR',4);
Warn_SC_v3 = zpk(z,p,k);

% Removing ~1Hz roll-off from Warn_SC_v3, adding extra zero at DC (displacement -> velocity)
z2 = [0 0 0 0 0.004 0.01 0.085 pairQ(0.08, 0.778) ];
p2 = [0.02 0.04 0.06 pairQ(0.04, 1) pairQ(0.06, 1.462) pairQ(0.007, 1.932)];
scHp = zpkgf(z2,p2,1,100); 

% Setting the High-pass gain to 1 at the micro-seism, recalculating Lowpass
scHp = minreal(sysgf(scHp,1,0.16));
scLp = minreal(1 - scHp);

% Filters for displacement to velocity (and vice versa)
d2v = zpkgf([0],[1e3],1,1/(2*pi));
v2d = zpkgf([],[0.001],1,1/(2*pi));

%% Applying BRS Sensor-correction filters
nav = 71;    % Number of averages
fcut = 0.13; % Cutoff frequency


brsexSc = c2dfilt(brsex,rotvelex,ts); 
gndexTorq = c2dfilt(gndex,torqex,ts);

brseySc = c2dfilt(brsey,rotveley,ts); 

% Removing filter transients by throwing away the first ~500s of data.
ftcut = find(t>500,1);   % Index of the 500s point for removing filter transients
t1 = t; t1(1:ftcut) = []; 
gndex(1:ftcut) = []; brsex(1:ftcut) = [];
gndey(1:ftcut) = []; brsey(1:ftcut) = [];
brsexSc(1:ftcut) = []; gndexTorq(1:ftcut) = [];
brseySc(1:ftcut) = [];
gndix(1:ftcut) = []; gndiy(1:ftcut) = [];

% Calculating spectra, summing RMS from f<0.1
[gndexAsd, f] = asd2(gndex,ts,nav);
df = f(2) - f(1);
ncut = find(f>0.1,1);  % Cutouff index for RMS calculations to remove microseism
fRms = f(1:ncut);
gndexRms = asdrms(gndexAsd(1:ncut),df);

gndeyAsd = asd2(gndey,ts,nav);
gndeyRms = asdrms(gndeyAsd(1:ncut),df);

gndixAsd = asd2(gndix,ts,nav);
gndixRms = asdrms(gndixAsd(1:ncut),df);

gndiyAsd = asd2(gndiy,ts,nav);
gndiyRms = asdrms(gndiyAsd(1:ncut),df);

% Applying sensor correction with new filters
gndexSc = gndex + gndexTorq + brsexSc;
gndexScAsd = asd2(gndexSc,ts,nav);
gndexScRms = asdrms(gndexScAsd(1:ncut),df);

% Applying sensor correction with new filters
gndeySc = gndey + brseySc;
gndeyScAsd = asd2(gndeySc,ts,nav);
gndeyScRms = asdrms(gndeyScAsd(1:ncut),df);

%% Applying CPS sensor-correction filters

% Applying CPS sensor correction filter to tilt-corrected STS data
gndex2cps = c2dfilt(gndex,scHp,ts);
gndexSc2cps = c2dfilt(gndexSc,scHp,ts);

gndey2cps = c2dfilt(gndey,scHp,ts);
gndeySc2cps = c2dfilt(gndeySc,scHp,ts);

gndix2cps = c2dfilt(gndix,scHp,ts);
gndiy2cps = c2dfilt(gndiy,scHp,ts);

% Removing filter transients by throwing away the first ~20s of data.
ftcut = find(t>20,1);   % Index of the 500s point for removing filter transients
t2 = t; t2(1:ftcut) = []; 
gndex2cps(1:ftcut) = []; gndexSc2cps(1:ftcut) = [];
gndey2cps(1:ftcut) = []; gndeySc2cps(1:ftcut) = [];
gndix2cps(1:ftcut) = []; gndiy2cps(1:ftcut) = [];
 

[gndex2cpsAsd, f2] = asd2(gndex2cps,ts,nav);
df2 = f2(2) - f2(1); 
ncut = find(f2>0.2,1); 
f2Rms = f2(1:ncut);
gndex2cpsRms = asdrms(gndex2cpsAsd(1:ncut),df2);
gndexSc2cpsAsd = asd2(gndexSc2cps,ts,nav);
gndexSc2cpsRms = asdrms(gndexSc2cpsAsd(1:ncut),df2);

gndey2cpsAsd = asd2(gndey2cps,ts,nav);
gndey2cpsRms = asdrms(gndey2cpsAsd(1:ncut),df2);
gndeySc2cpsAsd = asd2(gndeySc2cps,ts,nav);
gndeySc2cpsRms = asdrms(gndeySc2cpsAsd(1:ncut),df2);

gndix2cpsAsd = asd2(gndix2cps,ts,nav);
gndix2cpsRms = asdrms(gndix2cpsAsd(1:ncut),df2);

gndiy2cpsAsd = asd2(gndiy2cps,ts,nav);
gndiy2cpsRms = asdrms(gndiy2cpsAsd(1:ncut),df2);


xdiff = gndex2cps - gndix2cps; 
ydiff = gndey2cps - gndiy2cps;  
xdiffAsd = asd2(xdiff,ts,nav); xdiffRms = asdrms(xdiffAsd(1:ncut),df2);
ydiffAsd = asd2(ydiff,ts,nav); ydiffRms = asdrms(ydiffAsd(1:ncut),df2);

xdiffSc = gndexSc2cps - gndix2cps; 
ydiffSc = gndeySc2cps - gndiy2cps;  
xdiffScAsd = asd2(xdiffSc,ts,nav); xdiffScRms = asdrms(xdiffScAsd(1:ncut),df2);
ydiffScAsd = asd2(ydiffSc,ts,nav); ydiffScRms = asdrms(ydiffScAsd(1:ncut),df2);


[yyCoh, fCoh] = coh2(gndeySc2cps,gndiy2cps,ts,nav);
[sig, ydiffresid, fsccs, fitvec] = sccs2(gndeySc2cps,gndiy2cps,ts,nav);
ydiffresidRms = asdrms(ydiffresid(1:ncut),df2);


figure(1)
clf
% hold on
loglog(...
...    f,gndexAsd,'k',f2,gndexRms,'k--',...
...    f,gndeyAsd,'b',f2,gndeyRms,'b--',...
...    f,gndexScAsd,'k',f2,gndexScRms,'k--',...
...    f,gndeyScAsd,'b',f2,gndeyScRms,'b--',...
    f2,gndex2cpsAsd,'g',f2Rms,gndex2cpsRms,'g--',...
    f2,gndexSc2cpsAsd,'k',f2Rms,gndexSc2cpsRms,'k--',...
    f2,gndey2cpsAsd,'c',f2Rms,gndey2cpsRms,'c--',...
    f2,gndeySc2cpsAsd,'m',f2Rms,gndeySc2cpsRms,'m--',...
    f2,gndix2cpsAsd,'b',f2Rms,gndix2cpsRms,'b--',...
    f2,gndiy2cpsAsd,'r',f2Rms,gndiy2cpsRms,'r--',...
    f,f*0 + 1e-7,'k:')
axis([5e-3 0.5 1e-8 5e-6])
set(gca,'FontSize',18)
xlabel('Frequency [Hz]')
ylabel('Inertial-equivalent velocity [m/s/\surdHz]')
legend(...
    'GND-EX-2CPS','GND-EX-2CPS rms'...
    ,'Tilt-corrected GND-EX-2CPS','Tilt-corrected GND-EX-2CPS rms'...
    ,'GND-EY-2CPS','GND-EY-2CPS rms'...
    ,'Tilt-corrected GND-EY-2CPS','Tilt-corrected GND-EY-2CPS rms'...
    ,'GND-IX-2CPS','GND-IX-2CPS rms'...
    ,'GND-IY-2CPS','GND-IY-2CPS rms'...
    )
title('Ground-to-CPS Sensor correction')


figure(2)
clf
% hold on
loglog(...
    f2,gndeySc2cpsAsd,'k',f2Rms,gndeySc2cpsRms,'k--',...
    f2,gndiy2cpsAsd,'m',f2Rms,gndiy2cpsRms,'m--',...
    f2,ydiffAsd,'r',f2Rms,ydiffRms,'r--',...
    f2,ydiffScAsd,'b',f2Rms,ydiffScRms,'b--',...
...    f2,ydiffresid,'g',f2Rms,ydiffresidRms,'g--',...
    f,f*0 + 1e-7,'k:')
axis([5e-3 0.5 1e-8 5e-6])
set(gca,'FontSize',18)
xlabel('Frequency [Hz]')
ylabel('Inertial-equivalent velocity [nm/s/\surdHz]')
legend(...
    'Tilt-corrected GND-EY-2CPS','Tilt-corrected GND-EY-2CPS rms'...
    ,'GND-IY-2CPS','GND-IY-2CPS rms'...
    ,'Y-arm no tilt-correction', 'rms'...
    ,'Y-arm corrected', 'rms'...
    )
title('Ground-to-CPS Sensor correction')

% figure(4)
% semilogx(...
%     fCoh,xxCoh,'b'...
%     )
% axis([2e-3 0.5 0 1])
% set(gca,'FontSize',20)
% xlabel('Frequency [Hz]')
% ylabel('Inertial-equivalent velocity [m/s/\surdHz]')




% f7 = logspace(-3,3,1000);
% figure(3)
% subplot(211)
% loglog(f7,abs(fresp(Warn_SC_v3*d2v,f7)),'b',f7,abs(fresp(1 - Warn_SC_v3*d2v,f7)),'b--'...
%     ,f7,abs(fresp(scHp,f7)),'r',f7,abs(fresp(1 - scHp,f7)),'r--')
% set(gca,'FontSize',20)
% axis([min(f) 10 1e-3 3e1])
% subplot(212)
% semilogx(f7,deg(fresp(Warn_SC_v3*d2v,f7)),'b',f7,deg(fresp(1 - Warn_SC_v3*d2v,f7)),'b--'...
%     ,f7,deg(fresp(scHp,f7)),'r',f7,deg(fresp(1 - scHp,f7)),'r--')
% set(gca,'FontSize',20)
% axis([min(f) 10 -200 200])



% Money plot versions of previous
figure(10)
loglog(...
    f2,gndex2cpsAsd,'g',...
    f2,gndey2cpsAsd,'c',...
    f2,gndix2cpsAsd,'b',...
    f2,gndiy2cpsAsd,'r',...
    f2,gndexSc2cpsAsd,'k',...
    f2,gndeySc2cpsAsd,'m',...
    f2Rms,gndex2cpsRms,'g--',...
    f2Rms,gndexSc2cpsRms,'k--',...
    f2Rms,gndey2cpsRms,'c--',...
    f2Rms,gndeySc2cpsRms,'m--',...
    f2Rms,gndix2cpsRms,'b--',...
    f2Rms,gndiy2cpsRms,'r--'...
    )
axis([5e-3 0.5 1e-8 5e-6])
set(gca,'FontSize',18)
xlabel('Frequency [Hz]')
ylabel('Inertial-equivalent velocity [m/s/\surdHz]')
legend(...
    'GND-EX to CPS'...
    ,'GND-EY to CPS'...
    ,'GND-IX to CPS'...
    ,'GND-IY to CPS'...
    ,'Tilt-corrected GND-EX to CPS'...
    ,'Tilt-corrected GND-EY to CPS'...
    )
title('Ground-to-CPS Sensor correction')

figure(11)
clf
% hold on
loglog(...
    f2,gndexSc2cpsAsd,'k',...
    f2,gndix2cpsAsd,'m',...
    f2,xdiffAsd,'r',...
    f2,xdiffScAsd,'b',...
    f2Rms,gndexSc2cpsRms,'k--',...
    f2Rms,gndix2cpsRms,'m--',...
    f2Rms,xdiffRms,'r--',...
    f2Rms,xdiffScRms,'b--'...
    )
axis([5e-3 0.5 1e-8 5e-6])
set(gca,'FontSize',18)
xlabel('Frequency [Hz]')
ylabel('Inertial-equivalent velocity [m/s/\surdHz]')
legend(...
    'Tilt-corrected GND-EX to CPS'...
    ,'GND-IX to CPS'...
    ,'X-arm no tilt-correction'...
    ,'X-arm corrected'...
    )
title('Differential Ground-to-CPS Sensor correction')

figure(12)
clf
% hold on
loglog(...
    f2,gndeySc2cpsAsd,'k',...
    f2,gndiy2cpsAsd,'m',...
    f2,ydiffAsd,'r',...
    f2,ydiffScAsd,'b',...
    f2Rms,gndeySc2cpsRms,'k--',...
    f2Rms,gndiy2cpsRms,'m--',...
    f2Rms,ydiffRms,'r--',...
    f2Rms,ydiffScRms,'b--'...
    )
axis([5e-3 0.5 1e-8 5e-6])
set(gca,'FontSize',18)
xlabel('Frequency [Hz]')
ylabel('Inertial-equivalent velocity [m/s/\surdHz]')
legend(...
    'Tilt-corrected GND-EY to CPS'...
    ,'GND-IY to CPS'...
    ,'Y-arm no tilt-correction'...
    ,'Y-arm corrected'...
    )
title('Differential Ground-to-CPS Sensor correction')







