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',...
            };

    %Starttime = 1097105000; % windy data
    %Starttime = 1097560Rot_000;
    %Starttime = 1097990000;  % Data from 10/22/2014 Wednesday night, quiet 
    %Starttime = 1098505000; % 10/27/2014 Monday night data, low wind
    %Starttime = 1098575000; % Data from 10/28/2014 Tuesday night , quiet
    %Starttime = 1098660000; % Data from 10/29/2014 Wednesday night , quiet
    %Starttime = 1097297900; % Data from 10/29/2014 earthquake
    %Starttime = 1099205000;
    %Starttime = 1100383000;
    Starttime =  1143511481; % Earthquake data 03/30/2016, Mag 6
    %Starttime = 1099170400;  % Data from 11/4/2014 Wind speeds in 20-30 mph
    %Starttime = 1098340000; %10/26/2014 windy period - looks like damper may have been triggered.
    %Starttime = 1098370000; %10/26/2014 night
    %Starttime = 1098160000;   %30k seconds available
    %Starttime = 1098350000;   % sunday night windy
    duration = 10000;      
%Fetch data
    rawdata = get_data(Channels,'raw',Starttime,duration);
%end   
    rawBRSin8Hz = decimate(rawdata(1,1).data, rawdata(1,1).rate/sampf);
    rawBRSout8Hz = decimate(rawdata(1,2).data, rawdata(1,2).rate/sampf);
    rawT240YV8Hz = decimate(rawdata(1,3).data, rawdata(1,3).rate/sampf);
    rawBRSref8Hz = decimate(rawdata(1,4).data, rawdata(1,4).rate/sampf);
    rawT240XV8Hz = decimate(rawdata(1,5).data, rawdata(1,5).rate/sampf);
    rawT240ZV8Hz = decimate(rawdata(1,6).data, rawdata(1,6).rate/sampf);

    mydata = [rawBRSin8Hz rawBRSout8Hz rawT240YV8Hz rawBRSref8Hz rawT240XV8Hz ...
            rawT240ZV8Hz];
    save('GPS1143511481_BRS2Data.mat','mydata')
        
end    

    if (not(exist('mydata','var')))
        if exist('GPS1143511481_BRS2Data.mat','file')
            myfile = load('GPS1143511481_BRS2Data.mat');
            mydata = myfile.mydata;
            rawBRSin8Hz = mydata(:,1);
            rawBRSout8Hz = mydata(:,2);
            rawT240YV8Hz = mydata(:,3);
            rawBRSref8Hz = mydata(:,4);
            rawT240XV8Hz = mydata(:,5);
            rawT240ZV8Hz = mydata(:,6);
        end
    end
 
set(0,'DefaultAxesFontSize',20);

Sttime = 001 * sampf;
Totaltime = 9990; %duraiton in sec
Endtime = Sttime + Totaltime * sampf;
localg = 9.8;

newfudge = 1;

BRSin = 1e-9 * newfudge * rawBRSin8Hz(Sttime:Endtime);
BRSout = 1e-9 * newfudge * rawBRSout8Hz(Sttime:Endtime);
BRSref = 1e-9 * newfudge * rawBRSref8Hz(Sttime:Endtime);
T240XVdata = 1e-9/localg * rawT240XV8Hz(Sttime:Endtime);
T240YVdata = 1e-9/localg * rawT240YV8Hz(Sttime:Endtime);
T240ZVdata = 1e-9/localg * rawT240ZV8Hz(Sttime:Endtime);

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

%% 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.65e-3;
Qbar = 3000;

%% torque computation

[bb,aa] = butter(4,2*1.000/sampf,'low');
[bbh,aah] = butter(4,2*0.0005/sampf,'high');
[bbp,aap] = butter(4,[2*0.005/sampf,2*0.500/sampf]);

T240Yacceldata = sampf * diff(T240YVdata);
T240Yacceldata = vertcat(T240Yacceldata(1,1),T240Yacceldata);

npoints = length(Rot_time);
T240Ydisp = zeros(npoints,1);

for i1 = 2:npoints
    T240Ydisp(i1) = 1/sampf*T240YVdata(i1,1)+T240Ydisp(i1-1);
end
T240Ydisp(1) = T240Ydisp(2);

[AT240Ydisp, frot] = asd2(T240Ydisp, 1/sampf, Navg, 1, @hann);

T240Yaccel_cal = T240Yacceldata + (2*pi*4.3e-3)^2*T240Ydisp - (2*pi*4.3e-3)/1.4*T240YVdata;
T240Yaccel_calfilt = filter(bb,aa,T240Yaccel_cal);

T240Yacceldiff1 = sampf * diff(T240Yaccel_calfilt);
T240Yacceldiff1 = vertcat(T240Yacceldiff1(1,1),T240Yacceldiff1);

T240Yacceldiff2 = sampf * diff(T240Yacceldiff1);
T240Yacceldiff2 = vertcat(T240Yacceldiff2(1,1),T240Yacceldiff2);

T240Yaccel_calfilt2 = filter(bbh,aah,T240Yaccel_cal);

T240YV_cal = zeros(npoints,1);
for i1 = 2:npoints
    T240YV_cal(i1) = 1/sampf*T240Yaccel_calfilt2(i1,1)+T240YV_cal(i1-1);
end
T240YV_cal(1) = T240YV_cal(2);

T240Ydisp_cal = zeros(npoints,1);
for i1 = 2:npoints
    T240Ydisp_cal(i1) = 1/sampf*T240YV_cal(i1,1)+T240Ydisp_cal(i1-1);
end
T240Ydisp_cal(1) = T240Ydisp_cal(2);
T240Ydisp_calfilt2 = filter(bbh,aah,T240Ydisp_cal);

[AT240Yaccel_cal, frot] = asd2(T240Yaccel_cal,1/sampf, Navg, 1, @hann);


BRSoutfilt = filter(bb,aa,BRSin);
BRSvelocity = sampf * diff(BRSoutfilt);
BRSvelocity = vertcat(BRSvelocity(1,1),BRSvelocity);
BRSoutfilt = filter(bbp,aap,BRSin);

BRSaccel = sampf * diff(BRSvelocity);
BRSaccel = vertcat(BRSaccel(1,1),BRSaccel);

BRSaccel_cal = BRSaccel + (resfreq)^2 * BRSin; % - resfreq/400*BRSvelocity;

[bbp2,aap2] = butter(4,[2*0.005/sampf,2*0.030/sampf]);
%[bbp3,aap3] = butter(4,[2*0.001/sampf,2*0.005/sampf]);
BRSaccel_calfilt = filter(bbp2,aap2,BRSaccel_cal);
%BRSaccel_calfilt2 = filter(bbp3,aap3,BRSaccel_cal);
%BRSreffilt = filter(bbp3,aap3,BRSref);

BRSsubaccel = BRSaccel_cal+Mtotal/Ibar*doffset*localg*T240Yaccel_cal+T240Yacceldiff2;
%% SUBTRACTION

%AngleDiff = T240Xacceldata + BRSout + 0*Mtotal/Ibar*doffset*localg*T240Xdisp_calfilt2;
AngleDiff = T240Yacceldata + BRSout + 0*Mtotal/Ibar*doffset*localg*T240Ydisp;


%% Calculate ASD

[ABRSdrift, ~] = asd2(BRSin,1/sampf, Navg, 1, @hann);
[ABRSout, ~] = asd2(BRSout,1/sampf, Navg, 1, @hann);
[ABRSref, ~] = asd2(BRSref,1/sampf, Navg, 1, @hann);
[Adiff, ~] = asd2(AngleDiff,1/sampf, Navg, 1, @hann);
[AT240YV, ~] = asd2(T240YVdata, 1/sampf, Navg, 1, @hann);
[AT240XV, ~] = asd2(T240XVdata, 1/sampf, Navg, 1, @hann);
[AT240ZV, ~] = asd2(T240ZVdata, 1/sampf, Navg, 1, @hann);

[ABRSaccel_cal, ~] = asd2(BRSaccel_cal, 1/sampf, Navg, 1, @hann);
[AT240Xacceldiff2, ~] = asd2(T240Yacceldiff2, 1/sampf, Navg, 1, @hann);
[ABRSsubaccel,frot] = asd2(BRSsubaccel, 1/sampf, Navg, 1, @hann);

ABRStilt = ABRSdrift.*sqrt( ( (2*pi*frot').^2 - resfreq^2 ).^2 + resfreq^4/Qbar^2 )./abs( ( 2*pi*frot').^2 - Mtotal*doffset*localg/Ibar);

%% Calculate Coherence
Navg2 = 9;

[CohBRSdriftT240, frot2] = coh2(BRSin,T240XVdata,1/sampf, Navg2, 1, @hann);
[CohBRSoutT240, frot2] = coh2(BRSout,T240XVdata,1/sampf, Navg2, 1, @hann);
[CohBRSoutRef, frot2] = coh2(BRSout,BRSref,1/sampf, Navg2, 1, @hann);
[CohT240XY, frot2] = coh2(T240XVdata,T240YVdata,1/sampf, Navg2, 1, @hann);
[CohT240YZ, frot2] = coh2(T240YVdata,T240ZVdata,1/sampf, Navg2, 1, @hann);
[CohT240XZ, frot2] = coh2(T240XVdata,T240ZVdata,1/sampf, Navg2, 1, @hann);

%% Plots

figure(1)
%ll=loglog(frot, ABRSdrift, frot, ABRSout,frot, AT240YV.*(2*pi*frot'), frot, ABRSref);
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([3e-3 4])
ylim([1e-11 1e-6])
%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-2 and STS');
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 (raw)','BRS OUT','T240 X (raw) * w^2/g','BRS ref mirror',...
%     'Location','northeast'); %'IPS X,RY noise')
grid on

figure(2)
%subplot(212)
lll = semilogx(frot2, CohBRSoutT240,frot2,CohBRSoutRef); %, frot2, CohBRSoutT240);
set(lll,'LineWidth',2);
xlim([3e-4 1])
xlabel('frequency (Hz)');
ylabel('coherence');
%legend('BRS and PEM tilt','T240 and PEM tilt','BRSout and T240X','Location','southeast');
legend('BRS RY out and T240X','BRS RY out and BRS ref','Location','southeast');
grid on

%% angular acceleration plot
figure(3)
llll = loglog(frot, Ibar*ABRSaccel_cal,frot, Ibar*AT240Xacceldiff2,...
    frot,Ibar*AT240Yaccel_cal.*Mtotal*localg/Ibar*abs(doffset)); %,...
    %frot, abs(Ibar*AT240Xaccel_cal.*Mtotal*localg/Ibar*abs(doffset)-Ibar*AT240Xacceldiff2)); %frot,Ibar*ABRSsubaccel);
    %frot,Ibar*ABRSsubaccel);
set(llll,'LineWidth',2);
set(gca,'FontSize',18)
xlim([3e-4 1])
ylim([3e-13 3e-7])
set(gca,'YTick',10.^(-12:-6))
xlabel('frequency (Hz)');
ylabel('Torque ASD (N-m/rt(Hz))');
title('Torque applied on BRS');
legend('Torque on BRS',...
    'Inertial component','Horizontal acceleration coupling component',...
    'Location','northwest')%'IPS X,RY noise')
grid on

%% seismometer plot
figure(4)
lllll = loglog(frot, AT240XV./(2*pi*frot'),frot, AT240YV./(2*pi*frot'),...
    frot, AT240ZV./(2*pi*frot')); %frot,Ibar*ABRSsubaccel);
    %frot,Ibar*ABRSsubaccel);
set(lllll,'LineWidth',2);
set(gca,'FontSize',18)
xlim([3e-4 1])
ylim([1e-10 1e-3])
set(gca,'YTick',10.^(-10:-3))
xlabel('frequency (Hz)');
ylabel('velocity ASD (m/s/rt(Hz))');
title('T240 seismometer');
legend('X','Y','Z',...
    'Location','northeast')%'IPS X,RY noise')
grid on

figure(5)
%subplot(212)
llllll = semilogx(frot2, CohT240XY,frot2,CohT240YZ,frot2,CohT240XZ); %, frot2, CohBRSoutT240);
set(llllll,'LineWidth',2);
xlim([3e-4 1])
xlabel('frequency (Hz)');
ylabel('coherence');
%legend('BRS and PEM tilt','T240 and PEM tilt','BRSout and T240X','Location','southeast');
legend('T240 X and Y','T240 Y and Z','T240 X and Z','Location','southeast');
grid on
%% printing

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

%     figure(2)
%     FillPage('w')
%     IDfig('K. Venkateswara')
%     saveas(gcf,'NoiseStudy2_Coherence.pdf')
%     
%     figure(3)
%     FillPage('w')
%     IDfig('K. Venkateswara')
%     saveas(gcf,'NoiseStudy2_Torque.pdf')
%     
%     figure(4)
%     FillPage('w')
%     IDfig('K. Venkateswara')
%     saveas(gcf,'NoiseStudy2_T240_ASD.pdf')
%     
%     figure(5)
%     FillPage('w')
%     IDfig('K. Venkateswara')
%     saveas(gcf,'NoiseStudy2_T240_coherence.pdf')   
    
end
