%% Import the transfer function data
M1_meas_filename='M1_to_M1_SR3_Y.txt'; %named this locally

%%% Grab the raw data. Raw data is formatted in |freq||real|imag| 
M1_2_M1=struct(); %M1 drive to M1 gap transfer function

%%% Grab the M1 TF [um/drive_cts]
raw_data=load(M1_meas_filename);
M1_2_M1.freq=raw_data(:,1); %freq
M1_2_M1.data=raw_data(:,2)+1i*raw_data(:,3);% real+i*imag

%% Select a freq range for the fit
mask=boolean((M1_2_M1.freq>0.1).*(M1_2_M1.freq<11));
freq_fit=M1_2_M1.freq(mask);
fitme=M1_2_M1.data(mask);

%% Vector fitting (VFIT3 by Bjorn Gustavsen)
%%% Transpose the column vector into a row vector for VF3 format
freq_trans=transpose(1i*freq_fit*2*pi);
fitme_trans=transpose(fitme);

%%% Use a strong inverse weighting scheme for a better fit
weight=1./(abs(fitme_trans));


%%% To choose poles:

%%% Specify min and max frequencies to get a range to evenly space poles
min_freq=min(freq_trans);
max_freq=max(freq_trans);
num_Poles=6;
evenly_spaced=linspace(min_freq,max_freq,num_Poles/2);
poles=[]; % initialize an empty array
for n=1:length(evenly_spaced) % populate the array with evenly spaced poles
    alf=-evenly_spaced(n)/100;
    poles=[poles (alf-1i*evenly_spaced(n)) (alf+1i*evenly_spaced(n)) ];
end

%Specify options
opts.stable=1;

%iterate a specified number of times to produce the final fit
for iter=1:num_Poles
    [SER,poles,rmserr,fit]=vectfit3(fitme_trans,freq_trans,poles,weight,opts);
end

%%% Convert to readable format 
fit_3=zpk(ss(SER.A,SER.B,SER.C,SER.D));

vectfit_3=squeeze(freqresp(fit_3,2*pi*freq_fit));

figure, 
subplot(2,1,1)
loglog(freq_fit,abs(fitme),freq_fit,abs(vectfit_3))
title('SR3 M1 to M1 (vectfit3)')
ylabel('Amplitude [m/m]')
legend('M1 to M1 measurement (2025-06-24)', 'vectfit ISI')
subplot(2,1,2)
semilogx(freq_fit,cleanangle_deg(fitme),freq_fit,cleanangle_deg(vectfit_3))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')

%% Preliminary Spectrumest Fit

%%% Produce a fit by specifying the number of zeros and poles
estimate=zpk(spectrumest(abs(fitme).^2,2*pi*freq_fit,0,6,4)); % 4 zeros and 6 poles
est_f_spec=squeeze(freqresp(estimate,2*pi*freq_fit));

%%% Plot the fit
figure,
subplot(2,1,1)
loglog(freq_fit,abs(fitme),freq_fit,abs(est_f_spec))
title('SR3 M1 to M1 (Spectrumest)')
legend('Data','Spectrumest Fit')
subplot(2,1,2)
semilogx(freq_fit,cleanangle_deg(fitme),freq_fit,cleanangle_deg(est_f_spec))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')

%% Preliminary AAA (rational) Fit

%%% Produce a fit by specifying the maximum number of poles, the
%%% ErrorMetric to weigh zeros more heavily, set causal to true for stable
%%% poles
fit=rational(2*pi*freq_fit,fitme,MaxPoles=6,ErrorMetric="Relative",Causal=true);
AAA_f=squeeze(freqresp(fit,2*pi*freq_fit));

%%% Plot the fit
figure, 
subplot(2,1,1)
loglog(freq_fit,abs(fitme),freq_fit,abs(AAA_f))
title('SR3 M1 to M1 (AAA)')
ylabel('Amplitude [m/m]')
legend('M1 to M1 measurement (2025-06-24)', 'AAA ISI fit')
subplot(2,1,2)
semilogx(freq_fit,cleanangle_deg(fitme),freq_fit,cleanangle_deg(AAA_f))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')
%% Vector fitting (rationalfit)

%%% Produce a fit by specifying the tolerance (tolerance has a floor of
%%% -21.7dB for this specific data)
fit_vect=rationalfit(2*pi*freq_fit,fitme,Tolerance=-40);
vect_f=squeeze(freqresp(fit_vect,2*pi*freq_fit));

%%% Polot the fit
figure, 
subplot(2,1,1)
loglog(freq_fit,abs(fitme),freq_fit,abs(vect_f))
title('SR3 M1 to M1 (vectfit)')
ylabel('Amplitude [m/m]')
legend('M1 to M1 measurement (2025-06-24)', 'vectfit ISI')
subplot(2,1,2)
semilogx(freq_fit,cleanangle_deg(fitme),freq_fit,cleanangle_deg(vect_f))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')

%% Auxiliary functions
function ang=cleanangle_deg(TF)
    bit=0.1;
    ang=angle(TF*exp(1i*pi*bit));
    ang=ang-pi*bit;
    ang=ang*180/pi;
end
function zpkstring=printzpkstring(TF)
    rn=3; %number of significant digits
    zz=round(zero(TF),rn);
    pp=round(pole(TF),rn);
    kk=round(TF.K,rn);
    zpkstring='zpk([';
    %Zeros
    for ii=1:numel(zz)
    zpkstring=[zpkstring,num2str(zz(ii)),','];
    end
    zpkstring=[zpkstring(1:(end-1)),'],['];
    %Poles
    for ii=1:numel(pp)
    zpkstring=[zpkstring,num2str(pp(ii)),','];
    end
    zpkstring=[zpkstring(1:(end-1)),'],',num2str(kk),')'];
end

