printFigs = false;
fetchdata = false;
sampf = 8; % sampling frequency in Hz

% Define Channels
if fetchdata
    Channels = {'H1:ISI-GND_BRS_ETMY_RX_IN1_DQ',...
            'H1:ISI-GND_BRS_ETMY_RX_OUT_DQ',...
            'H1:ISI-GND_STS_ETMY_Y_DQ',...
            'H1:ISI-GND_BRS_ETMY_REF_OUT_DQ',...
            'H1:ISI-GND_STS_ETMY_X_DQ',...
            'H1:ISI-GND_STS_ETMY_Z_DQ',...
            'H1:PEM-EY_WIND_ROOF_WEATHER_MPH',...
            'H1:PEM-EY_WIND_ROOF_WEATHER_DEG',...
            'H1:HPI-ETMY_STSINF_C_Y_IN1_DQ',...
            'H1:PEM-EY_ADC_0_10_OUT_DQ',...
            'H1:PEM-EY_ADC_0_09_OUT_DQ',...
            'H1:ISI-GND_BRS_ETMY_TEMPR',...
            };

    conn = nds2.connection('nds.ligo-wa.caltech.edu', 31200);
    Starttime = 1194256326; % 

    duration = 10000;      
%Fetch data
    %rawdata = get_data(Channels,'raw',Starttime,duration);
    buffers = conn.fetch(Starttime, Starttime+duration, Channels(1:4))
%end   
    rawBRSin64Hz = decimate(double(buffers(1).getData()), buffers(1).getChannel.getSampleRate/64);
    rawBRSin8Hz = decimate(rawBRSin64Hz, 64/sampf);
    rawBRSout64Hz = decimate(double(buffers(2).getData()), buffers(2).getChannel.getSampleRate/64);
    rawBRSout8Hz = decimate(rawBRSout64Hz, 64/sampf);
    rawT240YV64Hz = decimate(double(buffers(3).getData()), buffers(3).getChannel.getSampleRate/64);
    rawT240YV8Hz = decimate(rawT240YV64Hz, 64/sampf);
    rawBRSref64Hz = decimate(double(buffers(4).getData()), buffers(4).getChannel.getSampleRate/64);
    rawBRSref8Hz = decimate(rawBRSref64Hz, 64/sampf);
    
    conn = nds2.connection('nds.ligo-wa.caltech.edu', 31200);
    buffers = conn.fetch(Starttime, Starttime+duration, Channels(5:8))
    
    rawT240XV64Hz = decimate(double(buffers(1).getData()), buffers(1).getChannel.getSampleRate/64);
    rawT240XV8Hz = decimate(rawT240XV64Hz, 64/sampf);
    rawT240ZV64Hz = decimate(double(buffers(2).getData()), buffers(2).getChannel.getSampleRate/64);
    rawT240ZV8Hz = decimate(rawT240ZV64Hz, 64/sampf);
    rawWINDSPD8Hz = decimate(double(buffers(3).getData()), buffers(3).getChannel.getSampleRate/sampf);
    rawWINDDIR8Hz = decimate(double(buffers(4).getData()), buffers(4).getChannel.getSampleRate/sampf);
    
    conn = nds2.connection('nds.ligo-wa.caltech.edu', 31200);
    buffers = conn.fetch(Starttime, Starttime+duration, Channels(9:12))
    
    rawTiltSub64Hz = decimate(double(buffers(1).getData()), buffers(1).getChannel.getSampleRate/64);
    rawTiltSub8Hz = decimate(rawTiltSub64Hz, 64/sampf);
    rawPEM10V64Hz = decimate(double(buffers(2).getData()), buffers(2).getChannel.getSampleRate/64);
    rawPEM10V8Hz = decimate(rawPEM10V64Hz, 64/sampf);
    rawPEM09V64Hz = decimate(double(buffers(3).getData()), buffers(3).getChannel.getSampleRate/64);
    rawPEM09V8Hz = decimate(rawPEM09V64Hz, 64/sampf);
    rawTEMPR8Hz = decimate(double(buffers(4).getData()), buffers(4).getChannel.getSampleRate/sampf);

    mydata = [rawBRSin8Hz rawBRSout8Hz rawT240YV8Hz rawBRSref8Hz ...
        rawT240XV8Hz rawT240ZV8Hz rawWINDSPD8Hz rawWINDDIR8Hz ...
        rawTiltSub8Hz rawPEM10V8Hz rawPEM09V8Hz rawTEMPR8Hz];
    save('GPS1194256326_BRSYData_Windy2.mat','mydata')
        
end    

if (not(exist('mydata','var')))
    if exist('GPS1194256326_BRSYData_Windy2.mat','file')
        myfile = load('GPS1194256326_BRSYData_Windy2.mat');
        mydata = myfile.mydata;
        rawBRSin8Hz = mydata(:,1);
        rawBRSout8Hz = mydata(:,2);
        rawT240YV8Hz = mydata(:,3);
        rawBRSref8Hz = mydata(:,4);
        rawT240XV8Hz = mydata(:,5);
        rawT240ZV8Hz = mydata(:,6);
        rawWINDSPD8Hz = mydata(:,7);
        rawWINDDIR8Hz = mydata(:,8);
        rawTiltSub8Hz = mydata(:,9);
        rawPEM10V8Hz = mydata(:,10);
        rawPEM09V8Hz = mydata(:,11);
        rawTEMPR8Hz = mydata(:,12);
    end
end
 
set(0,'DefaultAxesFontSize',20);
Sttime = 001 * sampf;
Totaltime = 9990; %duraiton in sec
Endtime = Sttime + Totaltime * sampf;
localg = 9.8;

newfudge = 0.78;

BRSin = 1e-9  * rawBRSin8Hz(Sttime:Endtime);
BRSout = 1e-9  * rawBRSout8Hz(Sttime:Endtime);
BRSref = 1e-9  * rawBRSref8Hz(Sttime:Endtime);
STSXVdata = 1e-9/localg * rawT240XV8Hz(Sttime:Endtime);
STSYVdata = 1e-9/localg * rawT240YV8Hz(Sttime:Endtime);
STSZVdata = 1e-9/localg * rawT240ZV8Hz(Sttime:Endtime);
WindspeedX = -rawWINDSPD8Hz(Sttime:Endtime) .* cos(rawWINDDIR8Hz(Sttime:Endtime));
WindspeedY = -rawWINDSPD8Hz(Sttime:Endtime) .* sin(rawWINDDIR8Hz(Sttime:Endtime));
WindSpeed = rawWINDSPD8Hz(Sttime:Endtime);
TiltSub = 1e-9/localg * rawTiltSub8Hz(Sttime:Endtime);
PEM10Vdata = 1e-9*5.0e1/localg * rawPEM10V8Hz(Sttime:Endtime);
%PEM09Vdata = 1e-9*4.5e1/localg* rawPEM09V8Hz(Sttime:Endtime);
TEMPR = 1e-2*rawTEMPR8Hz(Sttime:Endtime);

Rot_time = transpose(1/sampf * (0:1:length(BRSin)-1));
Navg = 11;

%% BRS parameters
Mtotal = 4.5; % Total mass in kg
Ibar = 0.61; % Moment of Inertia in kg m^2
localg = 9.8; % local gravitation acceleration
doffset = 1e-6; % CoM offset from pivot in m. d is plus if CoM below pivot
resfreq = 2*pi*7.6e-3;
Qbar = 100;

%% torque computation

%T240 response inversion filter
STSInvertFilt = zpk(-2*pi*[pairQ(8.2e-3,0.7)],-2*pi*[0 0],1);
STSInvertFilt = 1*STSInvertFilt/abs(freqresp(STSInvertFilt,2*pi*100));

T240InvertFilt = zpk(-2*pi*[pairQ(4.0e-3,0.7)],-2*pi*[0 0],1);
T240InvertFilt = 1*T240InvertFilt/abs(freqresp(T240InvertFilt,2*pi*100));

HPInvFilter = zpk(-2*pi*[0.005 0.005], -2*pi*[0.0001 0.0001],1);
HPInvFilter = 1*HPInvFilter/abs(freqresp(HPInvFilter,2*pi*100));

%BRS response inversion filter
BRSInvertFilt = zpk(-2*pi*[pairQ(resfreq/2/pi,Qbar) 0.001 0.001],-2*pi*[0.0002 0.0002 0.0002 0.0002],1);
BRSInvertFilt = 1*BRSInvertFilt/abs(freqresp(BRSInvertFilt,2*pi*100));

%Filters to differentiate and integrate
DiffFilt = zpk(-2*pi*[0], -2*pi*2,1);
DiffFilt = 1*DiffFilt/abs(freqresp(DiffFilt,2*pi*0.1592));

IntFilt = zpk(-2*pi*[], -2*pi*5e-4,1);
IntFilt = 1*IntFilt/abs(freqresp(IntFilt,2*pi*0.1592));

%Apply filters
STScal_vel = lsim(STSInvertFilt,STSYVdata,Rot_time);
PEM10cal_vel = lsim(T240InvertFilt,PEM10Vdata,Rot_time);
%T240cal_vel = T240YVdata;

STScal_acc = lsim(DiffFilt,STScal_vel,Rot_time);
PEM10cal_acc = lsim(DiffFilt,PEM10cal_vel,Rot_time);

STScal_disp = lsim(IntFilt,STScal_vel,Rot_time);

BRScal_out = lsim(BRSInvertFilt,BRSin, Rot_time);

ResidualAcc = STScal_acc - newfudge*BRScal_out;
ResidualAcc2 = PEM10cal_acc - 1.00*BRScal_out;

TiltSub_cal = lsim(HPInvFilter, TiltSub, Rot_time);

%% Calculate ASD

[ABRSin, ~] = asd2(BRSin,1/sampf, Navg, 1, @hann);
[ABRSout, ~] = asd2(BRSout,1/sampf, Navg, 1, @hann);
[ABRScalout, ~] = asd2(BRScal_out,1/sampf, Navg, 1, @hann);
[ABRSref, ~] = asd2(BRSref,1/sampf, Navg, 1, @hann);
[Adiff, ~] = asd2(ResidualAcc,1/sampf, Navg, 1, @hann);
[Adiff2, ~] = asd2(ResidualAcc2,1/sampf, Navg, 1, @hann);
[ATiltSub, ~] = asd2(TiltSub_cal, 1/sampf, Navg, 1, @hann);
[AT240YV, ~] = asd2(STSYVdata, 1/sampf, Navg, 1, @hann);
[APEM10V, ~] = asd2(PEM10Vdata, 1/sampf, Navg, 1, @hann);
%[APEM09V, ~] = asd2(PEM09Vdata, 1/sampf, Navg, 1, @hann);
[AT240XV, ~] = asd2(STSXVdata, 1/sampf, Navg, 1, @hann);
[AT240ZV, ~] = asd2(STSZVdata, 1/sampf, Navg, 1, @hann);
[AT240cal_acc, ~] = asd2(STScal_acc, 1/sampf, Navg, 1, @hann);
[APEM10cal_acc, ~] = asd2(PEM10cal_acc, 1/sampf, Navg, 1, @hann);
[AT240cal_disp, frot] = asd2(STScal_disp, 1/sampf, Navg, 1, @hann);

%% Calculate Coherence
Navg2 = 15;

[CohBRSinT240Y, ~] = coh2(BRSin,STSYVdata,1/sampf, Navg2, 1, @hann);
[CohBRSinPEM10, ~] = coh2(BRSin,PEM10Vdata,1/sampf, Navg2, 1, @hann);
[CohBRSinT240Z, ~] = coh2(BRSin,STSZVdata,1/sampf, Navg2, 1, @hann);
[CohBRSinRes, ~] = coh2(BRSin,TiltSub_cal,1/sampf, Navg2, 1, @hann);
[CohBRSinRef, ~] = coh2(BRSin,BRSref,1/sampf, Navg2, 1, @hann);
[CohT240XY, ~] = coh2(STSXVdata,STSYVdata,1/sampf, Navg2, 1, @hann);
[CohT240YZ, ~] = coh2(STSYVdata,STSZVdata,1/sampf, Navg2, 1, @hann);
[CohT240XZ, frot2] = coh2(STSXVdata,STSZVdata,1/sampf, Navg2, 1, @hann);

[TFBRSinPEM10, ~] = tfe2(BRScal_out,PEM10cal_acc,1/sampf, Navg2, 1, @hann);

%% Plots

figure(1)
ll=loglog(frot, ABRSin, frot, ABRScalout,frot, AT240cal_acc,...
    frot, ATiltSub.*(2*pi*frot), frot, APEM10cal_acc, frot, Adiff2,'--');
% ll=loglog(frot, ABRSdrift, frot, ABRSout,frot, AT240YV.*(2*pi*frot'),...
%     frot,AT240Yaccel_cal.*Mtotal*localg/Ibar*abs(doffset)./(2*pi*frot').^2);
set(ll,'LineWidth',2);
set(gca,'FontSize',18)
xlim([1e-3 2])
ylim([1e-10 1e-5])
%ylim([1e-10 1e-6])
set(gca,'YTick',10.^(-10:-6))
xlabel('frequency (Hz)');
%ylabel('ASD (m/rt(Hz))');
ylabel('ASD (rad/rt(Hz))');
title('BRS-Y and STS-2');
% legend('BRS-2 rX in (raw)','BRS-2 rX OUT','STS Y (raw) * w^2/g','Acceleration coupling with d = 1e-6 m',...
%     'Location','northeast'); %'IPS X,RY noise')
legend('BRS in','BRS rX OUT','STS Y * w^2/g',...
    'Online Residual','PEM signal','PEM residual (calculated)',...
    'Location','northeast'); %'IPS X,RY noise')
grid on

figure(2)
%subplot(212)
lll = semilogx(frot2, CohBRSinT240Y, frot2,CohBRSinRef,frot2, CohBRSinPEM10);
set(lll,'LineWidth',2);
xlim([1e-3 2])
xlabel('frequency (Hz)');
ylabel('coherence');
%legend('BRS and PEM tilt','T240 and PEM tilt','BRSout and T240X','Location','southeast');
legend('BRS rY IN and STS2 Y', 'BRS in and BRS ref',...
    'BRSY and PEM10','Location','southeast');
grid on

figure(3)
%subplot(212)
lll = semilogx(frot2, 1./sqrt(1-CohBRSinT240Y), frot2,1./sqrt(1-CohBRSinPEM10));
set(lll,'LineWidth',2);
xlim([1e-3 2])
xlabel('frequency (Hz)');
ylabel('sub factor');
%legend('BRS and PEM tilt','T240 and PEM tilt','BRSout and T240X','Location','southeast');
legend('BRS rY IN and STS2 Y', ...
    'BRSY and PEM10','Location','southeast');
grid on

figure(4)
%subplot(212)
lll = semilogx(frot2, wrapTo2Pi(phase(TFBRSinPEM10))*180/pi-180);
set(lll,'LineWidth',2);
xlim([1e-3 2])
ylim([-180 180])
xlabel('frequency (Hz)');
ylabel('phase');
%legend('BRS and PEM tilt','T240 and PEM tilt','BRSout and T240X','Location','southeast');
legend('BRS rY IN and STS2 Y', ...
    'Location','southeast');
grid on

%% Wind plot
figure(5)
lllll = plot(Rot_time,WindSpeed);
set(lllll,'LineWidth',2);
set(gca,'FontSize',18)
%xlim([3e-4 1])
%ylim([1e-10 1e-3])
%set(gca,'YTick',10.^(-10:-3))
xlabel('time (s)');
ylabel('speed (mph)');
%title('T240 seismometer');
legend('EY Wind speed',...
    'Location','northeast')%'IPS X,RY noise')
grid on

%% printing

if printFigs
    figure(1)
    FillPage('w')
    %IDfig('K. Venkateswara')
    saveas(gcf,'BRSY_ASD_MoreWindy2.pdf')

    figure(2)
    FillPage('w')
    %IDfig('K. Venkateswara')
    saveas(gcf,'BRSY_Coherence_MoreWindy2.pdf')
    
    figure(5)
    FillPage('w')
    %IDfig('K. Venkateswara')
    saveas(gcf,'BRSY_WindSpeed_MoreWindy2.pdf')  
    
end
