clear all
close all
L = @(x) strrep(x, '_', '\_');  % hack to get the correct channel name in plots 

%% parameters
gpsb = 1100642716;        % starting GPS time
dt = 1800;                % dration in seconds

fs_out = 2048;            % sampling frequency for all computations 
bands = [70, 500];        % frequency band for intensit noise BLRMS computations (you can define multiple bands)

lpfreq = 0.5;             % and the low pass filter frequency

T = 20;                   % how many seconds of data to throw away to avoid filter transients 

%% List of channels, the first is the main one the others are the auxiliary channels
chans={'H1:IMC-IM4_TRANS_SUM_OUT_DQ'...
       'H1:IMC-DOF_1_P_IN1_DQ', 'H1:IMC-DOF_1_Y_IN1_DQ', ...
       'H1:IMC-DOF_2_P_IN1_DQ', 'H1:IMC-DOF_2_Y_IN1_DQ', ...
       'H1:IMC-DOF_3_P_IN1_DQ', 'H1:IMC-DOF_3_Y_IN1_DQ'};

nx = 3; % number of rows and columns in scatter plot
ny = 2;

%% get all data
data=get_data(chans,'raw',gpsb,dt);
df = decimate(data(1).data, data(1).rate/fs_out);
df0 = data(1).data;
aux = [];
for i=2:length(data)
    aux(:,i-1) = decimate(data(i).data, data(i).rate/fs_out); %#ok<SAGROW>
end

%% compute the BRMS

% define low pass filter
Nyquist_f=fs_out/2; 
[bb, aa] = butter(4, lpfreq/Nyquist_f, 'low');
    
pd = [];
for i=1:size(bands,1)
    
    % create band pass filter with a more numerically stable algorithm
    [z,p,k] = butter(4, [bands(i,1)/Nyquist_f, bands(i,2)/Nyquist_f], 'bandpass');
    [sos,g] = zp2sos(z, p, k);
    fl = dfilt.df2sos(sos, g);
    
    % filter main signal in the band (double reverse pass to avoid phase lag)
    x = fl.filter(df);
    x = fl.filter(x(end:-1:1));
    x = x(end:-1:1);
    % square it to get the BLRMS
    x = x.^2;

    % low pass BLRMS
    x = filtfilt(bb,aa, x);
    % normalize to units of amplitude spectral density
    pd(:,i) = sqrt(abs(x/diff(bands(i,:))));
end

% define time for x axis 
t = (1:size(pd,1))/fs_out;

% low pass all aux channels
for i=1:size(aux,2)
    aux(:,i) = filtfilt(bb,aa,aux(:,i));
end


% throw away first and last seconds to avoid filter transients
pd = pd(T*fs_out:end-T*fs_out,:);
t = t(T*fs_out:end-T*fs_out);
aux = aux(T*fs_out:end-T*fs_out,:);
df = df(T*fs_out:end-T*fs_out,:);

%% plot time series of BLRMS
figure(1)
plot(t,pd); 
xlabel('Time [s]')
ylabel('Noise amplitude relative varation')
legend(arrayfun(@(i) sprintf('Band %d - %d Hz', bands(i,1), bands(i,2)), 1:size(bands,1), 'UniformOutput', false))

%% plot the scatter histograms (main signal vs aux signals)
for k=1:size(pd,2)
    figure()
    for i=1:size(aux,2)
        subplot(ny, nx, i)
        % define the histogram ranges
        xe = linspace(min(aux(:,i)), max(aux(:,i)), 100);
        ye = linspace(min(pd(:,k)), max(pd(:,k)), 100);
        % make 2d histogram
        h1 = hist2(aux(:,i), pd(:,k), xe, ye);
        % and plot it
        pcolor(xe, ye, h1'); shading flat
        xlabel(L(chans{i+1}))
    end
end

