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

% Define Channels
if fetchdata
Channels = {'H1:ISI-GND_BRS_ETMX_RY_IN1_DQ',...
'H1:ISI-GND_BRS_ETMX_RY_OUT_DQ',...
'H1:ISI-GND_STS_ETMX_X_DQ',...
'H1:ISI-GND_BRS_ETMX_REF_OUT_DQ',...
'H1:ISI-GND_STS_ETMX_Y_DQ',...
'H1:ISI-GND_STS_ETMX_Z_DQ',...
'H1:PEM-EX_WIND_ROOF_WEATHER_MPH',...
'H1:PEM-EX_WIND_ROOF_WEATHER_DEG',...
};

%Starttime = 1097105000; % windy data
%Starttime = 1097560Rot_000;
Starttime = 1143824000; % Windy Data ~25 mph 04/04/2016
duration = 7000;
%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);
rawT240XV8Hz = decimate(rawdata(1,3).data, rawdata(1,3).rate/sampf);
rawBRSref8Hz = decimate(rawdata(1,4).data, rawdata(1,4).rate/sampf);
rawT240YV8Hz = decimate(rawdata(1,5).data, rawdata(1,5).rate/sampf);
rawT240ZV8Hz = decimate(rawdata(1,6).data, rawdata(1,6).rate/sampf);
rawWINDSPD8Hz = decimate(rawdata(1,7).data, rawdata(1,7).rate/sampf);
rawWINDDIRHz = decimate(rawdata(1,8).data, rawdata(1,8).rate/sampf);

mydata = [rawBRSin8Hz rawBRSout8Hz rawT240XV8Hz rawBRSref8Hz rawT240YV8Hz ...
rawT240ZV8Hz rawWINDSPD8Hz rawWINDDIRHz];
save('GPS1143824000_BRS2Data_Windy.mat','mydata')

end

if (not(exist('mydata','var')))
if exist('GPS1143824000_BRS2Data_Windy.mat','file')
myfile = load('GPS1143824000_BRS2Data_Windy.mat');
mydata = myfile.mydata;
rawBRSin8Hz = mydata(:,1);
rawBRSout8Hz = mydata(:,2);
rawT240XV8Hz = mydata(:,3);
rawBRSref8Hz = mydata(:,4);
rawT240YV8Hz = mydata(:,5);
rawT240ZV8Hz = mydata(:,6);
rawWINDSPD8Hz = mydata(:,7);
rawWINDDIRHz = mydata(:,8);
end
end

set(0,'DefaultAxesFontSize',20);

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

newfudge = 0.72;

BRSin = 2.1362e-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);
WindspeedX = -rawWINDSPD8Hz(Sttime:Endtime) .* cos(rawWINDDIRHz(Sttime:Endtime));
WindspeedY = -rawWINDSPD8Hz(Sttime:Endtime) .* sin(rawWINDDIRHz(Sttime:Endtime));
WindSpeed = rawWINDSPD8Hz(Sttime:Endtime);

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

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

%% torque computation

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

%BRS response inversion filter
BRSInvertFilt = zpk(-2*pi*[pairQ(8.79e-3,4000)],-2*pi*[0 0],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
T240cal_vel = lsim(T240InvertFilt,T240XVdata,Rot_time);

T240cal_acc = lsim(DiffFilt,T240cal_vel,Rot_time);

T240cal_disp = lsim(IntFilt,T240cal_vel,Rot_time);

BRScal_out = lsim(BRSInvertFilt,BRSin, Rot_time);

ResidualAcc = T240cal_acc + BRScal_out + Mtotal*localg/Ibar*doffset*T240cal_disp;

%% 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);
[AT240YV, ~] = asd2(T240YVdata, 1/sampf, Navg, 1, @hann);
[AT240XV, ~] = asd2(T240XVdata, 1/sampf, Navg, 1, @hann);
[AT240ZV, ~] = asd2(T240ZVdata, 1/sampf, Navg, 1, @hann);
[AT240cal_acc, ~] = asd2(T240cal_acc, 1/sampf, Navg, 1, @hann);
[AT240cal_disp, frot] = asd2(T240cal_disp, 1/sampf, Navg, 1, @hann);

%% Calculate Coherence
Navg2 = 9;

[CohBRSinT240, ~] = coh2(BRSin,T240XVdata,1/sampf, Navg2, 1, @hann);
[CohBRSinRes, ~] = coh2(BRSin,ResidualAcc,1/sampf, Navg2, 1, @hann);
[CohBRSinRef, ~] = coh2(BRSin,BRSref,1/sampf, Navg2, 1, @hann);
[CohT240XY, ~] = coh2(T240XVdata,T240YVdata,1/sampf, Navg2, 1, @hann);
[CohT240YZ, ~] = coh2(T240YVdata,T240ZVdata,1/sampf, Navg2, 1, @hann);
[CohT240XZ, frot2] = coh2(T240XVdata,T240ZVdata,1/sampf, Navg2, 1, @hann);

%% Plots

figure(1)
ll=loglog(frot, ABRSin, frot, ABRScalout,frot, AT240cal_acc, frot, Adiff);
% 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-10 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 rY in (raw)','BRS rY OUT','STS2 X * w^2/g','Residual',...
'Location','northeast'); %'IPS X,RY noise')
grid on

figure(2)
%subplot(212)
lll = semilogx(frot2, CohBRSinT240,frot2, CohBRSinRes, frot2,CohBRSinRef); %, 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 IN and STS2 X','BRS rY IN and Residual', 'BRS in and BRS ref','Location','southeast');
grid on

%% seismometer plot
figure(3)
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

%% Wind plot
figure(4)
llll = plot(Rot_time,WindSpeed);
set(llll,'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('EX Wind speed',...
'Location','northeast')%'IPS X,RY noise')
grid on

%% printing

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

figure(2)
FillPage('w')
%IDfig('K. Venkateswara')
saveas(gcf,'BRS1_2.pdf')

figure(4)
FillPage('w')
%IDfig('K. Venkateswara')
saveas(gcf,'BRS1_3.pdf')

end