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

'$Id: test_autocfit.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;

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

% Fitting params
clear params
params = struct('spolesopt',2, 'Nmaxiter',40, 'minorder',18,...
  'maxorder',35, 'weightparam',2, 'plot',1,...
  'ctp','chivar','lrscond',5,'rmsevar',2,...
  'stabfit',0,'dterm',0,'spy',0);
% Fitting
disp(' Fitting absolute TF value with unstable model ')
[res,poles,dterm,mresp,rdl,rmse] = utils.math.autocfit(y,f,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')