%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TEST Fitting procedure in 2dim z-domain VDFIT
% At the end you have 4 stable transfer functions in partial fractions
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HISTORY:     02-10-2008 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERSION

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

%% Clear

clear all

%% Loading spectra

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

f = mpsd(:,1);
psd1 = mpsd(:,2);
csd = mpsd(:,3);
psd2 = mpsd(:,4);
fs = 10;


%% Eigendecomposition

[tf11,tf12,tf21,tf22] = utils.math.eigcsd(psd1,csd,conj(csd),psd2);

%% Constructing vector

f1 = [tf11 tf21];
f2 = [tf12 tf22];

%% VCFIT

N = 26; %Order of approximation 


% Max Iteration
Nmaxiter = 70;

% Fitting params
fitin.stable = 0;
fitin.dterm = 0;
fitin.plot = 0;

weight = utils.math.wfun(f1,2);
% weight = 1./abs(f1);

pparams = struct('spolesopt',2, 'type','CONT', 'pamp', 0.01);
poles1 = utils.math.startpoles(N,f,pparams);
for hh = 1:Nmaxiter
  [res1,poles1,dterm1,mresp1,rdl1,mse1] = utils.math.vcfit(f1,f,poles1,weight,fitin); % Fitting
%   disp(['Iter' num2str(hh)])

  % Stop condition checking
  order = length(poles1);
  mlr1(hh) = mse1(:,1);
  mlr2(hh) = mse1(:,2);
%   [ext1,msg1] = stopfit(f1(:,1),rdl1(:,1),mlr1,order,'mlsrvar',2,0.0001,2);
%   [ext2,msg2] = stopfit(f1(:,2),rdl1(:,2),mlr2,order,'mlsrvar',2,0.0001,2);
% 
%   if ext1 && ext2
%     disp(msg1)
%     break
%   end

end

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

% % plotting RMS error variation
% figure()
% semilogy(abs(diff(sqrt(mlr1))),'-ok')
% hold on
% grid on
% semilogy(abs(diff(sqrt(mlr2))),'-or')

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

%% All Passing

[np1,resp1] = pfallps(res1,poles1,dterm1,mresp1,f);


figure()
subplot(2,1,1);
p1 = loglog(f,abs(mresp1),'k');
hold on
p2 = loglog(f,abs(resp1),'r');
xlabel('Frequency [Hz]')
ylabel('Amplitude')
legend([p1(1) p2(1)],'VDFIT','Stabilized')
hold off

subplot(2,1,2);
p4 = semilogx(f,(180/pi).*unwrap(angle(mresp1)),'k');
hold on
p5 = semilogx(f,(180/pi).*unwrap(angle(resp1)),'r');
xlabel('Frequency [Hz]')
ylabel('Phase [Deg]')
legend([p4(1) p5(1)],'VDFIT', 'Stabilized')
hold off

%% VCFIT

N = 14; %Order of approximation


% Max Iteration
Nmaxiter = 50;

% Fitting params
fitin.stable = 0;
fitin.dterm = 0;
fitin.plot = 1;

weight = wfun(f2,1);

pparams = struct('spolesopt',1, 'type','CONT', 'pamp', 0.01);
poles2 = startpoles(N,f,pparams);

for hh = 1:Nmaxiter
  [res2,poles2,dterm2,mresp2,rdl2] = vcfit(f2,f,poles2,weight,fitin); % Fitting
  disp(['Iter' num2str(hh)])

  % Stop condition checking
  order = length(poles1);
  mlr1(hh) = mean(log10(abs(f2(:,1)))-log10(abs(rdl2(:,1))));
  mlr2(hh) = mean(log10(abs(f2(:,2)))-log10(abs(rdl2(:,2))));
  [ext1,msg1] = stopfit(f2(:,1),rdl2(:,1),mlr1,order,'mlrsvar',2,0.0001,2);
  [ext2,msg2] = stopfit(f2(:,2),rdl2(:,2),mlr2,order,'mlrsvar',2,0.0001,2);

  if ext1 && ext2
    disp(msg1)
    break
  end

end