% Data loading

temp=load('../Data/crs_ry_tf_undamped.txt');
temp2=load('../Data/crs_ry_coh_undamped.txt');

%%
%Extract data
freq = temp(:,1);
cps.tf = temp(:,2)+i*temp(:,3);
crs.tf = temp(:,4)+i*temp(:,5);
Cxy = temp2(:,3);

% Only look at frequencies between 3mHz and 5Hz (this is where the
% measurement has good coherence)

mask = Cxy >= 0.8; % to use for the undamped case

crs.tf = crs.tf(mask);
cps.tf = cps.tf(mask);
freq = freq(mask);
f = logspace(-3,2,1e4);
% The ratio between the crs and the cps gives the crs response (the cps's
% response is flat at low-f
crs.resp.meas = -crs.tf./cps.tf*1e3;

% save('crs_resp_damped.mat','crs','freq');
%%
% Manual fit

Q_phase = 89.6

crs.resp.manualfit1.zpk  = zpk(-2*pi*[0 0],-2*pi*[pair(25.4e-3,Q_phase)],1); %phase = 89.6 for the undamped system
crs.resp.manualfit2.zpk  = zpk([0.0027 + 0.0749i  0.0027 - 0.0749i],[0.0013 + 0.1597i 0.0013 - 0.1597i],1.0214); %phase = 89.6 for the undamped system

% crs  = -zpk(-2*pi*[0 0],-2*pi*[pair(1.5e-2,89)],1e-3);
crs.resp.manualfit1.freq  = squeeze(freqresp(crs.resp.manualfit1.zpk,2*pi*f));
crs.resp.manualfit2.freq  = squeeze(freqresp(crs.resp.manualfit2.zpk,2*pi*f));

% Auto fit
sys                   = tfest(frd(crs.resp.meas,2*pi*freq),crs.resp.manualfit1.zpk);
crs.resp.autofit.freq = squeeze(freqresp(sys,2*pi*f));

% fit CRS
figure(1)
subplot(211)
loglog(freq,abs(crs.resp.meas),'+',...
    f,abs(crs.resp.autofit.freq),...
    f,abs(crs.resp.manualfit1.freq),...
    'linewidth',2);
grid on
title(['CRS Instrument Response'])
xlabel('Mag [rad/rad]')
ylabel('Freq [Hz]')
xlim([3e-3 1e0])
legend('Measured crs response','Fit','Ideal response')
% ylim([1e-0 1e4])
subplot(212)
semilogx(freq,180/pi*angle(crs.resp.meas),'+',...
    f,180/pi*angle(crs.resp.autofit.freq),...
    f,180/pi*angle(crs.resp.manualfit1.freq),...
    'linewidth',2);
grid on
xlim([3e-3 1e0])
ylabel('Phase')
xlabel('Freq [Hz]')

%%
% 
figure(1)
% Set the figure size on paper (Width, Height)
set(gcf, 'Units', 'Inches', 'Position', [0, 0, 15, 10]);

% Set the paper size to match the figure size to avoid huge margins
set(gcf, 'PaperPositionMode', 'auto');

% Save the file
print(gcf, ['../Figures/crs_response_07072026.pdf'], '-dpdf', '-fillpage');