%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test script for autodfit
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HISTORY:     12-09-2008 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERSION

'$Id: test_autodfit.m,v 1.1 2009/04/23 10:11:26 luigi Exp $';

%% Cleaning

clear all

%% data



% % frequencies
% f = logspace(-6,log10(5),300); % frequency vector in Hz
% f = f.';
% fs = 10;

% % Continuous rational transfer function
% Num = [-2.726e-007; 1.665e-005; 1.303e-007; 8.381e-010]; % Numerator
% Den = [1; 0.2189; 0.01922; 0.0007803; 0]; % Denominator
% y = freqs(Num,Den,w);

% % filt1
% dRes = [0.2+j*0.003;0.2-j*0.003;0.45+j*0.0007;0.45-j*0.0007];
% dPoles = [0.97+j*0.0003;0.97-j*0.0003;0.75+j*0.00005;0.75-j*0.00005];
% dDTerms = 0;

% % filt2
% dRes = [0.2+j*0.003;0.2-j*0.003;0.45+j*0.0007;0.45-j*0.0007;12;0.45+j*0.07;0.45-j*0.07];
% dPoles = [0.97+j*0.0003;0.97-j*0.0003;0.75+j*0.00005;0.75-j*0.00005;0.1;0.998+j*0.00005;0.998-j*0.00005];
% dDTerms = 0;

% % Model aStefano Tf11
% dRes = [2.44554138162509e-011 - 1.79482547894083e-011i;
% 2.44554138162509e-011 + 1.79482547894083e-011i;
% 2.66402334803101e-009 +  1.1025122049153e-009i;
% 2.66402334803101e-009 -  1.1025122049153e-009i;
% -7.3560293387644e-009;
% -1.82811618589835e-009 - 1.21803627800855e-009i;
% -1.82811618589835e-009 + 1.21803627800855e-009i;
% 1.16258677367555e-009;
% 1.65216557639319e-016;                         
% -1.78092396888606e-016;
% -2.80420398962379e-017;
% 9.21305973049041e-013 - 8.24686706827269e-014i;
% 9.21305973049041e-013 + 8.24686706827269e-014i;
% 5.10730060739905e-010 - 3.76571756625722e-011i;
% 5.10730060739905e-010 + 3.76571756625722e-011i;
% 3.45893698149735e-009;
% 3.98139182134446e-014 - 8.25503935419059e-014i;
% 3.98139182134446e-014 + 8.25503935419059e-014i;
% -1.40595719147164e-011];
% 
% dPoles = [0.843464045655194 - 0.0959986292915475i;
% 0.843464045655194 + 0.0959986292915475i;
% 0.953187595424927 - 0.0190043625473383i;
% 0.953187595424927 + 0.0190043625473383i;
% 0.967176277937188;
% 0.995012027005247 - 0.00268322602801729i;
% 0.995012027005247 + 0.00268322602801729i;
% 0.996564761885673;
% 0.999999366165445;
% 0.999981722418555;
% 0.999921882627659;
% 0.999624431675213 - 0.000813407848742761i;
% 0.999624431675213 + 0.000813407848742761i;
% 0.997312006278751 - 0.00265611346834941i;
% 0.997312006278751 + 0.00265611346834941i;
% 0.990516544257531;
% 0.477796923118318 - 0.311064085401834i;
% 0.477796923118318 + 0.311064085401834i;
% 0];
% 
% dDTerms = 0;

% % Model Stefano TF12
% dRes = [1.44258422208796e-017 + 7.07359428613009e-019i;
% 1.44258422208796e-017 - 7.07359428613009e-019i;
% -3.4918408053655e-021 - 1.05662874569329e-021i;
% -3.4918408053655e-021 + 1.05662874569329e-021i;
% -7.61773292876976e-021;
% 4.84357724603939e-020 + 2.38824204294595e-019i;
% 4.84357724603939e-020 - 2.38824204294595e-019i;
% -4.07088520945753e-020 - 2.31474543846105e-019i;
% -4.07088520945753e-020 + 2.31474543846105e-019i;
% 8.73316588658882e-023;
% -5.21840635377469e-020;
% 1.8461911504859e-023;                         
% 5.20105247464461e-020;
% -4.68960092394415e-022;
% -1.44261407664171e-017 +  6.8922564526833e-019i;
% -1.44261407664171e-017 -  6.8922564526833e-019i;
% 3.13688133935426e-022];
% 
% dPoles = [0.477546340377332 -     0.310830571032376i;
%           0.477546340377332 +     0.310830571032376i;
%            0.99790715414307 -    0.0028490561287024i;
%            0.99790715414307 +    0.0028490561287024i;
%           0.998014205354671                         ;
%           0.999585354543332 -  0.000780408757425194i;
%           0.999585354543332 +  0.000780408757425194i;
%            0.99966003029931 -  0.000830944038363768i;
%            0.99966003029931 +  0.000830944038363768i;
%           0.999962770401331                         ;
%           0.999981881865521                         ;
%           0.999999365763457                         ;
%           0.999981706320212                         ;
%            0.99992421574188                         ;
%           0.477898460791003 +     0.311001926610074i;
%           0.477898460791003 -     0.311001926610074i;
%                           0];
% dDTerms = 0;

% % Model Stefano Tf21
% dRes = [-1.80035241582968e-016 + 1.99543917791863e-015i;
%      -1.80035241582968e-016 - 1.99543917791863e-015i;
%      -1.85590889333759e-013 - 1.23844418827409e-014i;
%      -1.85590889333759e-013 + 1.23844418827409e-014i;
%       5.03656596876842e-013                         ;
%      -2.62470963499904e-013 + 2.30024232938878e-012i;
%      -2.62470963499904e-013 - 2.30024232938878e-012i;
%      -9.83780507870955e-018                         ;
%       3.40426735130194e-021                         ;
%       9.78322351492755e-018                         ;
%      -1.65010934542937e-020                         ;
%       2.60918565203438e-015 +  1.0546609464659e-015i;
%       2.60918565203438e-015 -  1.0546609464659e-015i;
%       3.18105585405455e-014 + 2.48839990780042e-013i;
%       3.18105585405455e-014 - 2.48839990780042e-013i;
%       3.23021641947666e-013                         ;
%       4.81265000078114e-016 - 3.18269170053848e-017i;
%       4.81265000078114e-016 + 3.18269170053848e-017i;
%       5.16260024128201e-018];
% 
% dPoles = [0.872004077421604 -     0.110344282822693i;
%           0.872004077421604 +     0.110344282822693i;
%           0.956884129232757 -    0.0225532091775074i;
%           0.956884129232757 +    0.0225532091775074i;
%           0.966514825697177                         ;
%           0.995140550419744 -  0.000755127639524413i;
%           0.995140550419744 +  0.000755127639524413i;
%           0.999981802194393                         ;
%            0.99999936576546                         ;
%           0.999981722418555                         ;
%           0.999921882627659                         ;
%           0.999624431675213 +  0.000813407848742761i;
%           0.999624431675213 -  0.000813407848742761i;
%           0.997312006278751 +   0.00265611346834941i;
%           0.997312006278751 -   0.00265611346834941i;
%           0.990516544257531                         ;
%           0.477796923118318 +     0.311064085401834i;
%           0.477796923118318 -     0.311064085401834i;
%                           0];
%                         
% dDTerms = 0;

% % Model Stefano Tf22
% dRes = [1.1284521501259e-014;
%      -3.72133611555879e-014 - 2.08232683444075e-014i;
%      -3.72133611555879e-014 + 2.08232683444075e-014i;
%       9.84930639106637e-014 - 1.46640810672565e-013i;
%       9.84930639106637e-014 + 1.46640810672565e-013i;
%       2.51323684013671e-014                         ;
%      -5.64078525288305e-014                         ;
%      -1.62476406586366e-014                         ;
%      -1.08424815979566e-011 + 8.32328079357669e-012i;
%      -1.08424815979566e-011 - 8.32328079357669e-012i;
%       2.41831559776112e-011];
%     
% dPoles = [0.988511243978897;
%           0.997305870640646 +   0.00211760900132725i;
%           0.997305870640646 -   0.00211760900132725i;
%           0.999626453270255 +    0.0008125673525946i;
%           0.999626453270255 -    0.0008125673525946i;
%           0.999999366366222                         ;
%           0.999981706320212                         ;
%            0.99992421574188                         ;
%           0.477898460791003 -     0.311001926610074i;
%           0.477898460791003 +     0.311001926610074i;
%                           0];
%                         
% dDTerms = 0;

% % filt4
% dRes = [2.44554138162509e-011;
% 2.66402334803101e-009;
% -7.3560293387644e-009;
% -1.82811618589835e-009;
% 1.16258677367555e-009;
% 1.65216557639319e-016;                         
% -1.78092396888606e-016;
% -2.80420398962379e-017;
% 9.21305973049041e-013;
% 5.10730060739905e-010;
% 3.45893698149735e-009;
% 3.98139182134446e-014;
% -1.40595719147164e-011];
% 
% dPoles = [0.843464045655194;
% 0.953187595424927;
% 0.967176277937188;
% 0.995012027005247;
% 0.996564761885673;
% 0.999999366165445;
% 0.999981722418555;
% 0.999921882627659;
% 0.999624431675213;
% 0.997312006278751;
% 0.990516544257531;
% 0.477796923118318;
% 0];
% 
% dDTerms = 0;
% 
% 
% pfparams.type = 'disc';
% pfparams.freq = f;
% pfparams.fs = fs;
% pfparams.res = dRes;
% pfparams.pol = dPoles;
% pfparams.dterm = dDTerms;
% 
% % response of the model filter
% pfr = pfresp(pfparams);
% y = pfr.resp;

% % Model Continuous minphase
% r = [1.4871879011802e-005;                         
%       6.10992134443159e-016 - 9.32963644121133e-014i;
%       6.10992134443159e-016 + 9.32963644121133e-014i;
%        6.5700947750201e-017 - 8.67535202691611e-015i;
%        6.5700947750201e-017 + 8.67535202691611e-015i;
%       7.60899607753932e-017 - 2.31523533550992e-015i;
%       7.60899607753932e-017 + 2.31523533550992e-015i;
%       7.81925532239946e-010 - 5.60617358881436e-009i;
%       7.81925532239946e-010 + 5.60617358881436e-009i;
%      -1.36723085989008e-009 + 5.08414219753465e-009i;
%      -1.36723085989008e-009 - 5.08414219753465e-009i;
%      -8.98316119086409e-009 - 2.15634331132174e-007i;
%      -8.98316119086409e-009 + 2.15634331132174e-007i;
%       9.22133132976409e-009 - 3.24455764655643e-008i;
%       9.22133132976409e-009 + 3.24455764655643e-008i;
%       6.27280610849718e-011 - 1.19002054635157e-009i;
%       6.27280610849718e-011 + 1.19002054635157e-009i;
%       3.28962376794087e-010 - 3.35707070704021e-010i;
%       3.28962376794087e-010 + 3.35707070704021e-010i];
%     
% p = [-1878769.47366179;                         
%      -9.61587854136845e-006 + 4.02759537557155e-008i;
%      -9.61587854136845e-006 - 4.02759537557155e-008i;
%      -3.97325785368196e-005 + 7.21480997398869e-007i;
%      -3.97325785368196e-005 - 7.21480997398869e-007i;
%        -0.00016489329503076 + 4.21265809470161e-006i;
%        -0.00016489329503076 - 4.21265809470161e-006i;
%         -0.0488243534364908 +   0.00506677723199764i;
%         -0.0488243534364908 -   0.00506677723199764i;
%          -0.060222623381404 +    0.0123923197320473i;
%          -0.060222623381404 -    0.0123923197320473i;
%          -0.404482556535086 +    0.0153085700658377i;
%          -0.404482556535086 -    0.0153085700658377i;
%          -0.955593480749454 +    0.0869402878111827i;
%          -0.955593480749454 -    0.0869402878111827i;
%           -2.56698459926376 +     0.591616326379104i;
%           -2.56698459926376 -     0.591616326379104i;
%            -4.4135552953192 +     0.652758967843539i;
%            -4.4135552953192 -     0.652758967843539i];
%          
% d = 0;
% 
% f = logspace(-6,log10(5),300); % frequency vector in Hz
% f = f.';
% pfparams.type = 'cont';
% pfparams.freq = f;
% pfparams.res = r;
% pfparams.pol = p;
% pfparams.dterm = d;
% 
% % response of the model filter
% pfr = pfresp(pfparams);
% y = pfr.resp;


%% psd

load ..\m-toolbox\test\mpsd.mat % load mpsd.mat first column is f then psd1, csd and psd2

f = mpsd(:,1);
% psd = mpsd(:,2:4);
psd = mpsd(:,2);
y = abs(sqrt(psd));

%% auto search for the order

fs = 10; % Sampling frequency in Hz
% Fitting params
params = struct('spolesopt',2, 'Nmaxiter',70, 'minorder',24,...
  'maxorder',35, 'weightparam',1, 'plot',0,...
  'ctp','chivar','lrscond',5,'msevar',2,...
  'stabfit',0,'dterm',0,'spy',1);
[res,poles,dterm,mresp,rdl,mse] = utils.math.autodfit(y,f,fs,params);


%% comparison
figure()
subplot(2,1,1);
loglog(f,abs(y),'k')
hold on
loglog(f,abs(mresp),'r')
xlabel('Frequency [Hz]')
ylabel('Amplitude')
legend('Original', 'CTFIT')

subplot(2,1,2);
semilogx(f,angle(y),'k')
hold on
semilogx(f,angle(mresp),'r')
xlabel('Frequency [Hz]')
ylabel('Amplitude')
legend('Original', 'CTFIT')

% plotting mean squared error
figure()
semilogy(mse,'-ok')
% hold on
% grid on
% semilogy(mlr2,'-or')

% plotting squared error variation
figure()
semilogy(abs(diff(mse)./mse(1:end-1)),'-ok')
% hold on
% grid on
% semilogy(abs(diff(mlr2)),'-or')