% look at one piece of data from LLO, search for excess translation from
% tilt, BTL, Oct 2024

clear
close all
set(0,'defaultlinelinewidth',1.5)

[stat, me]= system('whoami');
if strncmp(me, 'BTL',3)
    data_dir = '/Users/BTL/Brians_files/SeismicSVN/seismic/Common/Documents/T2400352_ML_ISI_data/data_sets/';
    disp('running on Brians laptop')
else
    data_dir = '/home/controls/SeismicSVN/seismic/Common/Documents/T2400352_ML_ISI_data/data_sets/';
end


%data_file = 'L1_HAM5_excess_motion_1381528818_4000.mat';
data_file = 'L1_HAM5_excess_motion_1411455618_4000.mat'; 


ifo = 'L1';
chamber = 'HAM5';
%time_str = '1381528818';
time_str = '1411455618';
span_str = '4000';
write_ML_data = true;

plot_str = [' ',ifo,':',chamber,' at GPS=',time_str];
ML_file_header = ['MLdata_',ifo,chamber,'_',time_str,'_',span_str,'_'];
ML_struct_file = [ML_file_header,'struct.mat'];
ML_matrix_file = [ML_file_header,'matrix.mat'];


%%

temp = load([data_dir,data_file]);
dat1 = temp.dat;


dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);
[~,freq] = asd2(dat1(1).data, dT);

for ii = 1:length(dat1)
    dat1(ii).asd = asd2(dat1(ii).data, dT);
end

ch = assign_channels;

%% 
target_calibration = tf('s') * zpk([0,0],-2*pi*[0.030*[1+2i, 1-2i]/sqrt(5), 1*[1+1i, 1-1i]/sqrt(2)], 2*pi*1*2*pi*1);
% this is a band-limited velocity calibration around the microseism.
% seems to give good accuracy to the calcs, and it shows the issue I want to highlight
[comp,resp] = make_compensation_filters(target_calibration);
%close all

if or(contains(data_file,'HAM7'), contains(data_file,'HAM8')) 
    CPS_type = 'fine';
else
    CPS_type = 'coarse';
end


noise = calc_noise(freq,CPS_type);

synth.gnd.z.time = [];  % create the structure so you have something to give to the next section
coher.resid.y.gnd.z = [];
%%
dat1 = remove_mean(dat1);

plot_ground(dat1, ch, resp, plot_str)
%%
synth = plot_performace(dat1, synth, ch, 'z', freq, noise, comp, target_calibration, resp, plot_str);

%%
synth = plot_performace(dat1, synth, ch, 'x', freq, noise, comp, target_calibration, resp, plot_str);
coher = plot_coherence( dat1, synth, ch, 'x', freq, coher, plot_str);
coher = hf_coherence(   dat1, synth, ch, 'x', freq, coher, plot_str);

%%
synth = plot_performace(dat1, synth, ch, 'y', freq, noise, comp, target_calibration, resp, plot_str);
coher = plot_coherence( dat1, synth, ch, 'y', freq, coher, plot_str);
coher = hf_coherence(   dat1, synth, ch, 'y', freq, coher, plot_str);

%%
plot_tilt(dat1, synth, ch, 'rx', freq, noise, comp, target_calibration, resp, plot_str);
coher = tilt_coherence( dat1, synth, ch, 'rx', freq, coher, plot_str);

%%
plot3residuals(dat1, synth, plot_str);

%%
synth = make_synth_rotation(dat1, synth, ch, 'rx', freq, comp, plot_str);  % make the rx, ry, rz channels with the same calibrations
synth = make_synth_rotation(dat1, synth, ch, 'ry', freq, comp, plot_str);  % make the rx, ry, rz channels with the same calibrations
synth = make_synth_rotation(dat1, synth, ch, 'rz', freq, comp, plot_str);  % make the rx, ry, rz channels with the same calibrations

%%

plot_start(dat1, synth)

%%
if write_ML_data
    disp('saving data to')
    disp(ML_struct_file)
    disp(ML_matrix_file)

    note1 = ['created by ',mfilename,' on ',char(datetime('today'))];
    note2 = [ifo,':',chamber,'_',time_str,'_',span_str];
    note3 = ['sample rate is ',num2str(1/dT),' samples per sec'];
    
    save ML_struct_file note1 note2 note3 synth dat1

    % make a matrix for the data

    data_matrix(:,1) = synth.gnd.x.time;
    data_matrix(:,2) = synth.gnd.y.time;
    data_matrix(:,3) = synth.gnd.z.time;

    data_matrix(:,3+1) = synth.cps.x.time;
    data_matrix(:,3+2) = synth.cps.y.time;
    data_matrix(:,3+3) = synth.cps.z.time;
    data_matrix(:,3+4) = synth.cps.rx.time;
    data_matrix(:,3+5) = synth.cps.ry.time;
    data_matrix(:,3+6) = synth.cps.rz.time;

    data_matrix(:,9+1) = synth.gs13.x.time;
    data_matrix(:,9+2) = synth.gs13.y.time;
    data_matrix(:,9+3) = synth.gs13.z.time;
    data_matrix(:,9+4) = synth.gs13.rx.time;
    data_matrix(:,9+5) = synth.gs13.ry.time;
    data_matrix(:,9+6) = synth.gs13.rz.time;

    data_matrix(:,15+1) = synth.resid.x.time;
    data_matrix(:,15+2) = synth.resid.y.time;
    data_matrix(:,15+3) = synth.resid.z.time;

    data_matrix(:,18+1) = dat1(ch.drive.x).data;
    data_matrix(:,18+2) = dat1(ch.drive.y).data;
    data_matrix(:,18+3) = dat1(ch.drive.z).data;
    data_matrix(:,18+4) = dat1(ch.drive.rx).data;
    data_matrix(:,18+5) = dat1(ch.drive.ry).data;
    data_matrix(:,18+6) = dat1(ch.drive.rz).data;

    size(data_matrix)

    save ML_matrix_file data_matrix

end

%% calc "noise" of the rotation loops

comp_filt = load_comp_filters(freq, resp);

%%
plot_extra_rotation(dat1, ch, 'rx', freq, noise, resp, comp_filt, plot_str); 
plot_extra_rotation(dat1, ch, 'ry', freq, noise, resp, comp_filt, plot_str); 
plot_extra_rotation(dat1, ch, 'rz', freq, noise, resp, comp_filt, plot_str); 

%%
resid_tf = calc_tfs4residuals(synth,  coher, 'x' ,freq, dT, plot_str);
resid_tf = calc_tfs4residuals(synth,  coher, 'y' ,freq, dT, plot_str);

%

%%    -------  helper functions   ---------- 
function dat1 = remove_mean(dat1)
% remove the DC part of the responses;
% changed the get_and_save to keep the DC, but it messes up all the
% resideual plots

for ii = 1: length(dat1)
    dat1(ii).data = dat1(ii).data - mean(dat1(ii).data);
end

end


function plot_ground(dat1, ch, resp,plot_str)
% just plot the ground displacment ASD as a reference
dT = 1/dat1(1).rate;

[gnd_x_vel,freq] = asd2(dat1(ch.gnd.x).data, dT); 
gnd_y_vel        = asd2(dat1(ch.gnd.y).data, dT); 
gnd_z_vel        = asd2(dat1(ch.gnd.z).data, dT); 

T240_cal = 1e-9 ./ abs(squeeze(freqresp(resp.T240, 2*pi*freq)));

figure
loglog(...
    freq, gnd_x_vel .* T240_cal, ...
    freq, gnd_y_vel .* T240_cal, ...
    freq, gnd_z_vel .* T240_cal)
title('Ground Motion')
legend('X','Y','Z')
xlabel('freq (Hz)')
ylabel('motion ASD (m/\surdHz)')
xlim([1e-2, 10])
ylim([1e-9, 5e-6])
add_plotstring(plot_str);
grid on
IDfig
FillPage('w')


end

function str_handle = add_plotstring(plot_str)
ax = axis;
str_handle = text(ax(1), ax(3), plot_str);
set(str_handle,...
    'fontweight','bold',...
    'fontsize',14,...
    'horizontalalignment','left',...
    'verticalalignment','bottom');

end


function [synth] = plot_performace(dat1, synth, ch, dof, freq, noise, comp, target_calibration, resp, plot_str)
dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);

% disp(dof);
% comp = make_compensation_filters;
synth.gnd.(dof).time   = lsim( comp.T240, dat1(ch.gnd.(dof)).data, time);
synth.gs13.(dof).time  = lsim( comp.gs13, dat1(ch.gs13_blend.(dof)).data, time);
synth.cps.(dof).time   = lsim( comp.cps, dat1(ch.cps_blend.(dof) ).data, time);
synth.resid.(dof).time = (synth.gs13.(dof).time - synth.gnd.(dof).time) - synth.cps.(dof).time;
synth.cps_plus_gnd.(dof).time = (synth.cps.(dof).time + synth.gnd.(dof).time);


synth.gnd.(dof).asd   = asd2(synth.gnd.(dof).time, dT);
synth.gs13.(dof).asd  = asd2(synth.gs13.(dof).time, dT);
synth.cps.(dof).asd   = asd2(synth.cps.(dof).time, dT);
synth.resid.(dof).asd = asd2(synth.resid.(dof).time, dT);
synth.cps_plus_gnd.(dof).asd = asd2(synth.cps_plus_gnd.(dof).time, dT);



%%
figure
subplot(211)
plot(time, -synth.gnd.(dof).time, ...
    time, synth.gs13.(dof).time, ...
    time, synth.cps.(dof).time)
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
dco = get(gca,'colororder');
legend('-gnd','gs13','cps')
title(['band limited ',dof,' velocity'])

subplot(212)
pp=plot(time, synth.resid.(dof).time);
set(pp(1),'color',dco(4,:))
legend('residual')
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
add_plotstring(plot_str);
IDfig
FillPage('w')

%%  plot difference between GS-13 and 'calculated inertial'

figure
subplot(211)
plot(time, synth.cps_plus_gnd.(dof).time, ...
    time, synth.gs13.(dof).time)
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
dco = get(gca,'colororder');
legend('cps + gnd','gs13')
title(['band limited ',dof,' velocity'])

subplot(212)
pp=plot(time, synth.resid.(dof).time);
set(pp(1),'color',dco(4,:))
legend('residual')
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
add_plotstring(plot_str);
IDfig
FillPage('w')

%%
ideal_fr = abs(squeeze(freqresp(target_calibration, 2*pi*freq)));
mcc_calc = calc_mccs2(dat1, ch, dof,freq);

gs13_cal = 1e-9 ./ abs(squeeze(freqresp(resp.gs13, 2*pi*freq)));
%%

figure
ll=loglog(...
    freq, 1e-9 * synth.gnd.(dof).asd, ...
    freq, 1e-9 * synth.gs13.(dof).asd, ...
    freq, 1e-9 * synth.cps.(dof).asd, ...
    freq, 1e-9 * synth.resid.(dof).asd,...
    freq, ideal_fr .* noise.gs13.(dof),'--', ...
    freq, ideal_fr .* noise.cps.(dof),'--', ...
    freq, ideal_fr .* gs13_cal .* mcc_calc.gs13.(dof).residual);
title(['ASD of bandlimited velocity, ',upper(dof)])
dco = get(gca,'colororder');
set(ll(5),'color',dco(2,:));
set(ll(6),'color',dco(3,:));

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('band-limited vel ASD, (m/sec)/rtHz (0.05 - 1 Hz)')
grid on
legend('ground','GS-13','CPS','residual','GS-13 noise (for BL vel)',...
    'CPS noise (for BL vel)', ['mccs2: GS-13 ',dof,' - cps & gnd ',dof])
add_plotstring(plot_str);
IDfig
FillPage('w')


figure         % plot as ASD for displacement
ll=loglog(...
    freq, 1e-9 * (1./ideal_fr) .* synth.gnd.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.gs13.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.cps.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.resid.(dof).asd,...
    freq, noise.gs13.(dof),'--', ...
    freq, noise.cps.(dof),'--');
title(['ASD of displacement, ',upper(dof)])
dco = get(gca,'colororder');
set(ll(5),'color',dco(2,:));
set(ll(6),'color',dco(3,:));

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('disp ASD, m/rtHz')
grid on
legend('ground','GS-13','CPS','residual','GS-13 noise','CPS noise')
add_plotstring(plot_str);
IDfig
FillPage('w')


figure         % plot real vs. measured motion
ll=loglog(...
    freq, 1e-9 * (1./ideal_fr) .* synth.cps_plus_gnd.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.gs13.(dof).asd);
title(['ASD of displacement, ',upper(dof)])

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('disp ASD, m/rtHz')
grid on
legend('CPS + ground','GS-13')
add_plotstring(plot_str);
IDfig
FillPage('w')

end


function [synth] = make_synth_rotation(dat1, synth, ch, dof, freq, comp, plot_str)

dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);

% disp(dof);
% comp = make_compensation_filters;
synth.gs13.(dof).time  = lsim( comp.gs13, dat1(ch.gs13_blend.(dof)).data, time);
synth.cps.(dof).time   = lsim( comp.cps,  dat1(ch.cps_blend.(dof)).data, time);

synth.gs13.(dof).asd  = asd2(synth.gs13.(dof).time, dT);
synth.cps.(dof).asd   = asd2(synth.cps.(dof).time, dT);



%%
figure
plot(...
    time, synth.gs13.(dof).time, ...
    time, synth.cps.(dof).time)
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
dco = get(gca,'colororder');
legend('gs13','cps')
title(['band limited ',dof,' velocity'])
add_plotstring(plot_str);
IDfig
FillPage('w')

figure
ll=loglog(...
    freq, 1e-9 * synth.gs13.(dof).asd, ...
    freq, 1e-9 * synth.cps.(dof).asd);
title(['ASD of bandlimited velocity, ',upper(dof)])

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('band-limited vel ASD, (m/sec)/rtHz (0.05 - 1 Hz)')
grid on
legend('GS-13','CPS')
add_plotstring(plot_str);
IDfig
FillPage('w')


end


function plot_start(dat1, synth)
dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);
stop_time = 100;
dof = 'x';
figure
pp=plot(...
    time, synth.gnd.(dof).time,...
    time, -synth.cps.(dof).time,...
    time, synth.gs13.(dof).time,...
    time, synth.resid.(dof).time);
xlim([0,stop_time])
legend('ground','-cps','gs-13','residual')
title(['initial BL velocity for ',dof])
xlabel('seconds')
ylabel('nm/sec (Band Limited)')

dof = 'y';
figure
pp=plot(...
    time, synth.gnd.(dof).time,...
    time, -synth.cps.(dof).time,...
    time, synth.gs13.(dof).time,...
    time, synth.resid.(dof).time);
xlim([0,stop_time])
legend('ground','-cps','gs-13','residual')
title(['initial BL velocity for ',dof])
xlabel('seconds')
ylabel('nm/sec (Band Limited)')

dof = 'z';
figure
pp=plot(...
    time, synth.gnd.(dof).time,...
    time, -synth.cps.(dof).time,...
    time, synth.gs13.(dof).time,...
    time, synth.resid.(dof).time);
xlim([0,stop_time])
legend('ground','-cps','gs-13','residual')
title(['initial BL velocity for ',dof])
xlabel('seconds')
ylabel('nm/sec (Band Limited)')

end


function [synth] = plot_tilt(dat1, synth, ch, dof, freq, noise, comp, target_calibration, resp, plot_str)
dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);

% disp(dof);
% comp = make_compensation_filters;
%synth.gnd.(dof).time   = lsim( comp.T240, dat1(ch.gnd.(dof)).data, time);
synth.gs13.(dof).time  = lsim( comp.gs13, dat1(ch.gs13_blend.(dof)).data, time);
synth.cps.(dof).time   = lsim( comp.cps, dat1(ch.cps_blend.(dof) ).data, time);
%synth.resid.(dof).time = (synth.gs13.(dof).time - synth.gnd.(dof).time) - synth.cps.(dof).time;
%synth.cps_plus_gnd.(dof).time = (synth.cps.(dof).time + synth.gnd.(dof).time);


%synth.gnd.(dof).asd   = asd2(synth.gnd.(dof).time, dT);
synth.gs13.(dof).asd  = asd2(synth.gs13.(dof).time, dT);
synth.cps.(dof).asd   = asd2(synth.cps.(dof).time, dT);
%synth.resid.(dof).asd = asd2(synth.resid.(dof).time, dT);
%synth.cps_plus_gnd.(dof).asd = asd2(synth.cps_plus_gnd.(dof).time, dT);



%%
figure

plot(...
    time, synth.gs13.(dof).time, ...
    time, synth.cps.(dof).time)
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
legend('gs13','cps')
title(['band limited ',dof,' velocity'])

add_plotstring(plot_str);
IDfig
FillPage('w')

%%  plot difference between GS-13 and 'calculated inertial'

figure
subplot(211)
plot(time, synth.cps.(dof).time, ...
    time, synth.gs13.(dof).time)
xlim([400,600])
ylim([-800 800])
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
legend('cps','gs13')
title(['band limited ',dof,' velocity'])

add_plotstring(plot_str);
IDfig
FillPage('w')

%%
ideal_fr = abs(squeeze(freqresp(target_calibration, 2*pi*freq)));
%mcc = calc_mccs2(dat1, ch, dof);

gs13_cal = 1e-9 ./ abs(squeeze(freqresp(resp.gs13, 2*pi*freq)));
%%

figure
ll=loglog(...
...    freq, 1e-9 * synth.gnd.(dof).asd, ...
    freq, 1e-9 * synth.gs13.(dof).asd, ...
    freq, 1e-9 * synth.cps.(dof).asd, ...
...    freq, 1e-9 * synth.resid.(dof).asd,...
    freq, ideal_fr .* noise.gs13.(dof),'--', ...
    freq, ideal_fr .* noise.cps.(dof),'--');
...    freq, ideal_fr .* gs13_cal .* mcc.gs13.(dof).aligned_sigs);
title(['ASD of bandlimited velocity, ',upper(dof)])
dco = get(gca,'colororder');
set(ll(3),'color',dco(1,:));
set(ll(4),'color',dco(2,:));

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('band-limited vel ASD, (m/sec)/rtHz (0.05 - 1 Hz)')
grid on
legend('GS-13','CPS','GS-13 noise (for BL vel)',...
    'CPS noise (for BL vel)')
add_plotstring(plot_str);
IDfig
FillPage('w')


figure         % plot as ASD for displacement
ll=loglog(...
...    freq, 1e-9 * (1./ideal_fr) .* synth.gnd.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.gs13.(dof).asd, ...
    freq, 1e-9 * (1./ideal_fr) .* synth.cps.(dof).asd, ...
...    freq, 1e-9 * (1./ideal_fr) .* synth.resid.(dof).asd,...
    freq, noise.gs13.(dof),'--', ...
    freq, noise.cps.(dof),'--');
title(['ASD of displacement, ',upper(dof)])
dco = get(gca,'colororder');
set(ll(3),'color',dco(1,:));
set(ll(4),'color',dco(2,:));

xlim([1e-2, 10])
ylim([1e-12 1e-5])
xlabel('freq (Hz)')
ylabel('disp ASD, m/rtHz')
grid on
legend('GS-13','CPS','GS-13 noise','CPS noise')
add_plotstring(plot_str);
IDfig
FillPage('w')


end


function plot3residuals(dat1, synth, plot_str)
dT = 1/dat1(1).rate;
time = dT * (0:length(dat1(1).data)-1);

y_range = [-400, 400];

figure
subplot(311)
pp= plot(time, synth.gs13.z.time);
dco = get(gca,'colororder');
set(pp(1),'Color',dco(2,:))
xlim([400,600])
ylim(y_range)
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on
dco = get(gca,'colororder');
legend('Vertical GS-13')
title('Compare the band limited velocities')

subplot(312)
pp=plot(time, synth.resid.x.time);
set(pp(1),'color',dco(4,:))
legend('residual for X')
xlim([400,600])
ylim(y_range)
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on

subplot(313)
pp=plot(time, synth.resid.y.time);
set(pp(1),'color',dco(4,:))
legend('residual for Y')
xlim([400,600])
ylim(y_range)
xlabel('time (sec)')
ylabel('BL vel, nm/sec')
grid on

add_plotstring(plot_str);
IDfig
FillPage('w')

end


function mcc = calc_mccs2(dat1, ch, dof, freq)

dT = 1/dat1(1).rate;

[mcc.gs13.(dof).signal, mcc.gs13.(dof).residual, mcc.orig_freq] = mccs2(dat1(ch.gs13_blend.(dof)).data, ...
    [dat1(ch.cps_blend.(dof)).data, dat1(ch.gnd.(dof)).data], dT);

if length(mcc.orig_freq) > length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    mcc.gs13.(dof).signal   = mcc.gs13.(dof).signal( 1:length(orig_freq));
    mcc.gs13.(dof).residual = mcc.gs13.(dof).residual(1:length(orig_freq));
end

if length(mcc.orig_freq) < length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    zero_pad_points = length(freq) - length(mcc.orig_freq);
    mcc.gs13.(dof).signal   = [mcc.gs13.(dof).signal;   zeros(zero_pad_points,1)];
    mcc.gs13.(dof).residual = [mcc.gs13.(dof).residual; zeros(zero_pad_points,1)];
end

end


function coher = plot_coherence(dat1, synth, ch, dof, freq, coher, plot_str)
% calculate and plot a few of the interesting residuals
dT = 1/dat1(1).rate;

if dof == 'x'
    cross_dof = 'ry';
elseif dof == 'y'
    cross_dof = 'rx';
elseif dof == 'z'
    %cross_dof = 'rz';
    disp('no calcs are done for the z dof ')
    return
end


coher.resid.(dof).gnd.z            = coh2(synth.resid.(dof).time, dat1(ch.gnd.z).data, dT);
coher.resid.(dof).gs13.z           = coh2(synth.resid.(dof).time, dat1(ch.gs13_blend.z).data, dT);
coher.resid.(dof).cps.(cross_dof)  = coh2(synth.resid.(dof).time, dat1(ch.cps_blend.(cross_dof)).data, dT);
coher.resid.(dof).gs13.(cross_dof) = coh2(synth.resid.(dof).time, dat1(ch.gs13_blend.(cross_dof)).data, dT);

if length(coher.resid.(dof).gnd.z) < length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    zero_pad_points = length(freq) - length(coher.resid.(dof).gnd.z);
    coher.resid.(dof).gnd.z            = [coher.resid.(dof).gnd.z;   zeros(zero_pad_points,1)];
    coher.resid.(dof).gs13.z           = [coher.resid.(dof).gs13.z;   zeros(zero_pad_points,1)];
    coher.resid.(dof).cps.(cross_dof)  = [coher.resid.(dof).cps.(cross_dof);   zeros(zero_pad_points,1)];
    coher.resid.(dof).gs13.(cross_dof) = [coher.resid.(dof).gs13.(cross_dof);   zeros(zero_pad_points,1)];
end

figure
semilogx(...
    freq, coher.resid.(dof).gnd.z,...
    freq, coher.resid.(dof).gs13.z,...
    freq, coher.resid.(dof).cps.(cross_dof),...
    freq, coher.resid.(dof).gs13.(cross_dof));
title(['Coherence of the ',upper(dof),' residual'])
legend('with z ground', 'with GS-13 z ', ['with ',cross_dof,' cps'], ['with ',cross_dof,' GS-13'],'location','nw')
grid on
x_min = 0.01; y_min = 0;
xlim([x_min, 10])
ylim([y_min 1])
sh =add_plotstring(plot_str);
set(sh,'BackgroundColor',[1 1 1])
set(sh,'Position',[x_min * 1.05, y_min+0.02, 1])
IDfig
FillPage('w')

% figure
% semilogx(...
%     freq, coher.resid.(dof).gs13.z,...
%     freq, coher.resid.(dof).cps.(cross_dof));
% title(['Coherence of the ',upper(dof),' residual'])
% legend('with GS-13 z ',['with ',cross_dof,' cps'], 'location','nw')
% grid on
% x_min = 0.01; y_min = 0;
% xlim([x_min, 10])
% ylim([y_min 1])
% sh =add_plotstring(plot_str);
% set(sh,'BackgroundColor',[1 1 1])
% set(sh,'Position',[x_min * 1.05, y_min+0.02, 1])
% IDfig
% FillPage('w')

end

function coher = tilt_coherence(dat1, synth, ch, dof, freq, coher, plot_str)
% calculate and plot a few of the interesting residuals
dT = 1/dat1(1).rate;

coher.gs13.(dof).gnd.z      = coh2(dat1(ch.gs13_blend.(dof)).data, dat1(ch.gnd.z).data, dT);
coher.cps.(dof).gnd.z       = coh2(dat1(ch.cps_blend.(dof)).data,  dat1(ch.gnd.z).data, dT);
coher.gs13.(dof).cps.(dof)  = coh2(dat1(ch.gs13_blend.(dof)).data, dat1(ch.cps_blend.(dof)).data, dT);

if length(coher.gs13.(dof).gnd.z) < length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    zero_pad_points = length(freq) - length(coher.gs13.(dof).gnd.z);
    coher.gs13.(dof).gnd.z      = [coher.gs13.(dof).gnd.z;     zeros(zero_pad_points,1)];
    coher.cps.(dof).gnd.z       = [coher.cps.(dof).gnd.z;      zeros(zero_pad_points,1)];
    coher.gs13.(dof).cps.(dof)  = [coher.gs13.(dof).cps.(dof); zeros(zero_pad_points,1)];
end

figure
semilogx(...
    freq, coher.gs13.(dof).gnd.z,...
    freq, coher.cps.(dof).gnd.z,...
    freq, coher.gs13.(dof).cps.(dof));
title(['Coherence of the ',upper(dof),' signals'])
legend('GS-13 w. Z gnd', 'CPS w. Z gnd', 'CPS w. GS-13', 'location','nw')
grid on
x_min = 0.01; y_min = 0;
xlim([x_min, 10])
ylim([y_min 1])
sh =add_plotstring(plot_str);
set(sh,'BackgroundColor',[1 1 1])
set(sh,'Position',[x_min * 1.05, y_min+0.02, 1])
IDfig
FillPage('w')


end

function coher = hf_coherence( dat1, ~, ch, dof, freq, coher, plot_str)
% look for coherence around 10 Hz
if dof == 'x'
    cross_dof = 'ry';
elseif dof == 'y'
    cross_dof = 'rx';
elseif dof == 'z'
    %cross_dof = 'rz';
    disp('no calculations are done for the z dof')
    return
end

dT = 1/dat1(1).rate;

coher.gs13.(dof).gnd.z           = coh2(dat1(ch.gs13_blend.(dof)).data, dat1(ch.gnd.z).data, dT);
coher.gs13.(dof).gnd.(dof)       = coh2(dat1(ch.gs13_blend.(dof)).data, dat1(ch.gnd.(dof)).data, dT);
coher.gs13.(dof).cps.(cross_dof) = coh2(dat1(ch.gs13_blend.(dof)).data, dat1(ch.cps_blend.(cross_dof)).data, dT);

if length(coher.gs13.(dof).gnd.z) < length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    zero_pad_points = length(freq) - length(coher.gs13.(dof).gnd.z);
    coher.gs13.(dof).gnd.z           = [coher.gs13.(dof).gnd.z;           zeros(zero_pad_points,1)];
    coher.gs13.(dof).gnd.(dof)       = [coher.gs13.(dof).gnd.(dof);       zeros(zero_pad_points,1)];
    coher.gs13.(dof).cps.(cross_dof) = [coher.gs13.(dof).cps.(cross_dof); zeros(zero_pad_points,1)];
end

figure
semilogx(...
    freq, coher.resid.(dof).gnd.z,...
    freq, coher.gs13.(dof).cps.(cross_dof),...
    freq, coher.gs13.(dof).gnd.(dof));
title(['Coherence of the ',upper(dof),' GS-13 signal'])
legend('with z ground',['with ',cross_dof,' cps'],['with ',dof,' ground'], 'location','e')
grid on
x_min = 0.03; y_min = 0;
xlim([x_min, 60])
ylim([y_min 1])
sh =add_plotstring(plot_str);
set(sh,'BackgroundColor',[1 1 1])
set(sh,'Position',[x_min * 1.05, y_min+0.02, 1])
IDfig
FillPage('w')

end


function ch = assign_channels

% 1   'ifo:ISI-GND_STS_ITMY_DQ',...
%     'ifo:ISI-GND_STS_ITMY_Y_DQ',...
%     'ifo:ISI-GND_STS_ITMY_Z_DQ',...
% 4   'ifo:ISI-HAM7_BLND_GS13X_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_GS13Y_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_GS13Z_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_GS13RX_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_GS13RY_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_GS13RZ_IN1_DQ',...
% 10  'ifo:ISI-HAM7_SCSUM_CPS_X_IN_DQ',...
%     'ifo:ISI-HAM7_SCSUM_CPS_Y_IN_DQ',...
%     'ifo:ISI-HAM7_SCSUM_CPS_Z_IN_DQ',...
%     'ifo:ISI-HAM7_BLND_CPSRX_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_CPSRY_IN1_DQ',...
%     'ifo:ISI-HAM7_BLND_CPSRZ_IN1_DQ',..
% 16  'ifo:ISI-chamber_DRIVE_X_DQ',...
%     'ifo:ISI-chamber_DRIVE_Y_DQ',...
%     'ifo:ISI-chamber_DRIVE_Z_DQ',...
%     'ifo:ISI-chamber_DRIVE_RX_DQ',...
%     'ifo:ISI-chamber_DRIVE_RY_DQ',...
%     'ifo:ISI-chamber_DRIVE_RZ_DQ'};

ch.gnd.x = 1; ch.gnd.y = 2; ch.gnd.z = 3;
ch.gs13_blend.x  = 4;
ch.gs13_blend.y  = 5;
ch.gs13_blend.z  = 6;
ch.gs13_blend.rx = 7;
ch.gs13_blend.ry = 8;
ch.gs13_blend.rz = 9;

ch.cps_blend.x  = 10;
ch.cps_blend.y  = 11;
ch.cps_blend.z  = 12;
ch.cps_blend.rx = 13;
ch.cps_blend.ry = 14;
ch.cps_blend.rz = 15;

ch.drive.x  = 16;
ch.drive.y  = 17;
ch.drive.z  = 18;
ch.drive.rx = 19;
ch.drive.ry = 20;
ch.drive.rz = 21;

end

function [comp, resp] = make_compensation_filters(ideal_resp)
% make filters which turn the instruements into some common, ideal version

figure; bode(ideal_resp)
title('ideal response, in cnts/nm')

resp.gs13 = tf('s') * zpk([0,0],-2*pi*[1*[1+1i, 1-1i]/sqrt(2), 500,], 2*pi*500);
figure; bode(resp.gs13)
title('GS-13 response, in cnts/nm')

resp.cps = tf(1);

resp.T240 = tf('s') * zpk([0,0],-2*pi*[0.008*[1+1i, 1-1i]/sqrt(2),60], 2*pi*60);
figure; bode(resp.T240)
title('T240 response, in cnts/nm')


comp.gs13 = ideal_resp / resp.gs13;
comp.cps  = ideal_resp / resp.cps;
comp.T240 = ideal_resp / resp.T240;

figure
bode(comp.gs13, comp.cps, comp.T240)
legend('GS-13 compensation','CPS compensation','T240 compensation')

end

function noise = calc_noise(freq,CPS_type)
% make a bunch of noise calculations for common use
proj = SEI_sensor_noise('projections',freq);
noise.gs13.single = SEI_sensor_noise('gs13calc', freq);
if strcmp(CPS_type,'fine')
    disp('using the fine CPS noise')
    noise.cps.single = SEI_sensor_noise('ADE_p25mm', freq);
elseif strcmp(CPS_type,'coarse')
    disp('using the coarse CPS noise')
    noise.cps.single  = SEI_sensor_noise('ADE_1mm', freq);
else
    error('CPS type must be ''fine'' or ''coarse''')
end

noise.gs13.x  = proj.ham.stg1.gs13.x  * noise.gs13.single;
noise.gs13.y  = proj.ham.stg1.gs13.y  * noise.gs13.single;
noise.gs13.z  = proj.ham.stg1.gs13.z  * noise.gs13.single;
noise.gs13.rx = proj.ham.stg1.gs13.rx * noise.gs13.single;
noise.gs13.ry = proj.ham.stg1.gs13.ry * noise.gs13.single;
noise.gs13.rz = proj.ham.stg1.gs13.rz * noise.gs13.single;

noise.cps.x  = proj.ham.stg1.cps.x  * noise.cps.single;
noise.cps.y  = proj.ham.stg1.cps.y  * noise.cps.single;
noise.cps.z  = proj.ham.stg1.cps.z  * noise.cps.single;
noise.cps.rx = proj.ham.stg1.cps.rx * noise.cps.single;
noise.cps.ry = proj.ham.stg1.cps.ry * noise.cps.single;
noise.cps.rz = proj.ham.stg1.cps.rz * noise.cps.single;


end

function [dfilt_resp,dfilt_obj,cfilt_obj, sos_coef_matrix] = calc_resp(filter_obj, freq)
    filt_name       = filter_obj.name;
    rate            = filter_obj.fs;
    Ts              = 1/rate;
    sos_coef_matrix = filter_obj.soscoef;
    dfilt_obj        = dfilt.df2sos(sos_coef_matrix);
    % from readfiltzpk
    [zd,pd,kd] = sos2zp(sos_coef_matrix);
    
    cfilt_obj = d2c(zpk(zd,pd,kd,Ts),'tustin');
    
%    freq      = logspace(log10(low_freq_end), log10((rate-1)/2), points_to_plot); 
             % dont plot the last point, it screws up the plotting range...
    if length(freq) <2
        disp('adding more points so freqz behaves...')
        freq = [freq(1); 2*freq(1)];
        [dfilt_resp] = freqz(dfilt_obj, freq, rate);
        dfilt_resp = dfilt_resp(1);
        make_plot = false;
    else
        [dfilt_resp] = freqz(dfilt_obj, freq, rate);
        make_plot = true;

    end
    
    if make_plot
        figure
        subplot(211)
        ll= loglog(freq, abs(dfilt_resp),...
            freq, abs(squeeze(freqresp(cfilt_obj,2*pi*freq))));
        set(ll,'LineWidth',1)
        title(filt_name,'interpreter','none')
        legend('freqz resp','resp of cont time approx')
        axis tight
        grid on
        subplot(212)
        ll = semilogx(...
            freq,180/pi*angle(dfilt_resp),...
            freq,180/pi*angle(squeeze(freqresp(cfilt_obj,2*pi*freq))));
        set(ll,'LineWidth',1)
        axis tight
        grid on
    end
 
end    


function comp_filt = load_comp_filters(freq, resp)
SeisSVN = '/Users/BTL/Brians_files/SeismicSVN/seismic/';
filt_path         = [SeisSVN,'Common/Documents/T2400352_ML_ISI_data/'];
HAM7_filter_file  = [filt_path,'L1ISIHAM5_oct2024.txt'];
L5_filters        = readFilterFile(HAM7_filter_file);


[CPS_blend.RX.installed_FR, CPS_blend.RX.installed_dfilt, CPS_blend.RX.installed_cfilt, ~] = ...
    calc_resp(L5_filters.HAM5_BLND_RX_SUPERSENS1_DISP(1), freq);

[GS13_blend.RX.installed_FR, GS13_blend.RX.installed_dfilt, GS13_blend.RX.installed_cfilt, ~] = ...
    calc_resp(L5_filters.HAM5_BLND_RX_SUPERSENS1_INERT_HI(1), freq);

[CPS_blend.RZ.installed_FR, CPS_blend.RZ.installed_dfilt, CPS_blend.RZ.installed_cfilt, ~] = ...
    calc_resp(L5_filters.HAM5_BLND_RZ_SUPERSENS1_DISP(1), freq);

[GS13_blend.RZ.installed_FR, GS13_blend.RZ.installed_dfilt, GS13_blend.RZ.installed_cfilt, ~] = ...
    calc_resp(L5_filters.HAM5_BLND_RZ_SUPERSENS1_INERT_HI(1), freq);

comp_filt.rx.cps  = CPS_blend.RX.installed_FR;
comp_filt.rx.gs13 = GS13_blend.RX.installed_FR .* squeeze(freqresp(resp.gs13, 2*pi*freq));
comp_filt.rz.cps  = CPS_blend.RZ.installed_FR;
comp_filt.rz.gs13 = GS13_blend.RZ.installed_FR .* squeeze(freqresp(resp.gs13, 2*pi*freq));
%%
figure
subplot(211)
loglog(...
    freq, abs(comp_filt.rx.cps),...
    freq, abs(comp_filt.rx.gs13))
title('RX complementary filters')

subplot(212)
semilogx(...
    freq, 180/pi*angle(comp_filt.rx.cps),...
    freq, 180/pi*angle(comp_filt.rx.gs13));
    

figure
subplot(211)
loglog(...
    freq, abs(comp_filt.rz.cps),...
    freq, abs(comp_filt.rz.gs13))
title('RZ complementary filters')

subplot(212)
semilogx(...
    freq, 180/pi*angle(comp_filt.rz.cps),...
    freq, 180/pi*angle(comp_filt.rz.gs13));

comp_filt.ry = comp_filt.rx;
end


function plot_extra_rotation(dat1, ch, dof, freq, noise, resp, comp_filt, plot_str)
gs13_resp_fr = abs(squeeze(freqresp(resp.gs13, 2*pi*freq)));
gs13_cal = 1e-9 ./gs13_resp_fr;
%dof = 'rx';

figure
loglog(...
    freq, gs13_cal .* dat1(ch.gs13_blend.(dof)).asd, ...
    freq, 1e-9 * dat1(ch.cps_blend.(dof)).asd);
title([upper(dof),': GS-13 and CPS signal'])
xlim([0.01 70 ])
ylim([1e-15 1e-5])
xlabel('freq (Hz)')
ylabel('rad/\surdHz')
grid on
add_plotstring(plot_str);
IDfig
FillPage('w')


figure
loglog(...
    freq, gs13_cal .* dat1(ch.gs13_blend.(dof)).asd, ...
    freq, noise.gs13.(dof) .* abs(comp_filt.(dof).cps),...
    freq, noise.cps.(dof) .* abs(comp_filt.(dof).cps))
title([upper(dof),': GS-13 signal vs. sensor noise'])
legend('GS-13 signal','GS-13 noise * comp LP','CPS noise * comp LP')
xlim([0.01 70 ])
ylim([1e-15 1e-5])
xlabel('freq (Hz)')
ylabel('rad/\surdHz')
grid on
add_plotstring(plot_str);
IDfig
FillPage('w')


figure
loglog(...
    freq, 1e-9 .* dat1(ch.cps_blend.(dof)).asd, ...
    freq, noise.gs13.(dof) .* abs(comp_filt.(dof).gs13),...
    freq, noise.cps.(dof) .* abs(comp_filt.(dof).gs13))
title([upper(dof),': CPS signal vs. sensor noise'])
legend('CPS signal','GS-13 noise * comp HP','CPS noise * comp HP')
xlim([0.01 70 ])
ylim([1e-15 1e-5])
xlabel('freq (Hz)')
ylabel('rad/\surdHz')
grid on
add_plotstring(plot_str);
IDfig
FillPage('w')

end

function resid_tf = calc_tfs4residuals(synth, coher, dof,freq, dT, plot_str)
if strcmp(dof,'x')
    cross_dof = 'ry';
elseif strcmp(dof,'y')
    cross_dof = 'rx';
else
    error('this only works for X and Y')
end

[resid_tf.(dof).gs13.(cross_dof),tfefreq] = tfe2(synth.gs13.(cross_dof).time, synth.resid.(dof).time, dT);

if length(tfefreq) > length(freq)  %sloppy, but adequate for here. The freq vectors are different for a reason - BTL
    resid_tf.(dof).gs13.(cross_dof)   = resid_tf.(dof).gs13.(cross_dof)( 1:length(freq));
elseif length(tfefreq) < length(freq)  
    zero_pad_points = length(freq) - length(tfefreq);
    resid_tf.(dof).gs13.(cross_dof)   = [resid_tf.(dof).gs13.(cross_dof);   zeros(zero_pad_points,1)];
end

figure
subplot(211)
loglog(...
    freq, abs(resid_tf.(dof).gs13.(cross_dof)))
title(['TF of residual ',upper(dof),' / GS-13 ',upper(cross_dof)])
xlim([0.01 1])
ylim([0.1 100])
grid on

subplot(212)
semilogx(freq, coher.resid.(dof).gs13.(cross_dof))
xlim([0.01 1])
grid on
title('coherence')
add_plotstring(plot_str);
IDfig
FillPage('w')


end




