load('damped_ISI2M1.mat')
load('M1_to_M1.mat')
% The Dofs are set up in the same way as the M1 dtttfs scripts.
% L T V R P Y are 1 2 3 4 5 6, respectively

mask=boolean((freq2>0.3).*(freq2<14));
freq=freq2(mask);
ISI_meas=squeeze(damped_ISItoM1(6,6,mask));

%Add a few hand-chosen zeros or poles
elim=zpk([],[],1); %Don't add anything, I think this works pretty well
elim_f=abs(squeeze(freqresp(elim,2*pi*freq)));

fitme=abs(squeeze(ISI_meas)).^2./elim_f.^2;
estimate=zpk(spectrumest(fitme,2*pi*freq,0,6,6)); % 6 zeros and 6 poles

est_f=squeeze(freqresp(estimate*elim,2*pi*freq));
figure, loglog(freq,abs(ISI_meas),freq,abs(est_f))

%% Clean up the transfer functions
zz=zero(estimate);
pp=pole(estimate);
kk=estimate.K;

% Ensure all zeros are on the correct side of the complex plane
zz2=[zz(1:4);0;0];
zz2=-abs(real(zz2))+1i*imag(zz2);

% Correct the zeros and poles by looking at the plots we do below
fact=18/17.5; % Manual fudge factor to get the zero in the right place
zz2(3)=fact*zz2(3);
zz2(4)=fact*zz2(4);

% This is the pole Q correction, the frequencies look really nice (in my opinion)
pm=[-0.25;+0.25;-0.12;+0.12;-0.28;+0.28]; % Plus minus array for fudging the phases... Hand chosen to fit the M1 to M1 transfer functions
pp=pp.*exp(pi*1i*pm/180);
%(angle(-pp)/pi)*180 % Useful to quickly look at the Qs

% This is the gain correction (should be just a plus or minus 1)
kk=-kk;
est2=zpk(zz2,pp,kk);
est2_f=squeeze(freqresp(est2,2*pi*freq));
%% Plot the ISI to M1 transfer function
figure,
subplot(2,1,1)
loglog(freq,abs(ISI_meas),freq,abs(est2_f))
ylabel('Amplitude [m/m]')
legend('ISI to M1 measurement (2025-04-18)', 'spectrumest ISI fit')

subplot(2,1,2)
semilogx(freq,cleanangle_deg(ISI_meas),freq,cleanangle_deg(est2_f))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')
%% Fit the ISI TF to the M1 to M1 data
est_M1=est2-kk; % remove the high frequency asymptote from the ISI TF

% Manually select the gain for the M1 to M1 data
gain=59/72*18/24; % Fitting the high-frequency response, where we depend on the estimator
est_M1=gain*est_M1;
est_M1_f=squeeze(freqresp(est_M1,2*pi*meas.freq));

%[DEPRECATED] Find the M1 to M1 spectrumest fit (I decided to not use this one)
estimate3=zpk(spectrumest(abs(meas.eul2eul(6,6).f).^2,2*pi*meas.freq,0,6,4));
est_f3=squeeze(freqresp(estimate3*elim,2*pi*freq));


%% Plot the M1 to M1 transfer function
figure,
subplot(2,1,1)
loglog(meas.freq,abs(meas.eul2eul(6,6).f),meas.freq,abs(est_M1_f))
ylabel('Amplitude [m/m]')
legend('M1 to M1 measurement (2025-04-18)', 'spectrumest ISI fit')
xlim([0.3,20])
subplot(2,1,2)
semilogx(meas.freq,cleanangle_deg(meas.eul2eul(6,6).f),meas.freq,cleanangle_deg(est_M1_f))
xlabel('Frequency [Hz]')
ylabel('Phase [deg]')
xlim([0.3,20])

%%
printzpkstring(est2)
printzpkstring(est_M1)

%% 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