%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TEST ao/fromCSD
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HISTORY:     23-04-2009 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERSION

'$Id: test_ao_fromCSD.m,v 1.3 2009/04/23 16:03:55 luigi Exp $';

%% Loading spectra

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

f = mpsd(:,1);
psd = mpsd(:,2);
fs = 10;

% 1dim model
mod1D = ao(plist('xvals', f, 'yvals', psd, 'fs', fs, 'dtype', 'fsdata','description','MDC1 IFO CH1'));
mod1D.setName;

% 2dim model
csd11 = ao(plist('xvals', f, 'yvals', mpsd(:,2), 'fs', fs, 'dtype', 'fsdata','description','MDC1 IFO CH1 PSD'));
csd11.setName;
csd12 = ao(plist('xvals', f, 'yvals', mpsd(:,3), 'fs', fs, 'dtype', 'fsdata','description','MDC1 IFO CH12 CSD'));
csd12.setName;
csd21 = ao(plist('xvals', f, 'yvals', conj(mpsd(:,3)), 'fs', fs, 'dtype', 'fsdata','description','MDC1 IFO CH21 CSD'));
csd21.setName;
csd22 = ao(plist('xvals', f, 'yvals', mpsd(:,4), 'fs', fs, 'dtype', 'fsdata','description','MDC1 IFO CH2 PSD'));
csd22.setName;

mod2D = [csd11 csd12;csd21 csd22];


%% Generate 1D Noise

% plist for noise generation

plns = plist(...
  'csd',       mod1D, ...
  'Nsecs',     1e4, ...
  'fs',        fs, ...
  'xunits',    's', ...
  'yunits',    '', ...
  'MaxIter',   50, ...
  'PoleType',  3, ...
  'MinOrder',  7, ...
  'MaxOrder',  35, ...
  'Weights',   2, ...
  'Plot',      false,...
  'MSEVARTOL', 1e-2,...
  'FITTOL',    1e-2);

na = ao(plns);

%% Compare with Noisegen1D

aw = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', 1e4));

pl = plist(...
    'model', mod1D, ...
    'MaxIter', 50, ...
    'PoleType', 3, ...
    'MinOrder', 7, ...
    'MaxOrder', 35, ...
    'Weights', 2, ...
    'Plot', false,...
    'Disp', false,...
    'MSEVARTOL', 1e-2,...
    'FITTOL', 1e-2);

nat = noisegen1D(aw, pl);

%% Make psd

naxx = na.lpsd;
natxx = nat.lpsd;


iplot(naxx,natxx,mod1D)

%% 2 dim noise generation

% plist for noise generation

plns = plist(...
  'csd',       mod2D, ...
  'Nsecs',     1e4, ...
  'fs',        fs, ...
  'xunits',    's', ...
  'yunits',    '', ...
  'MaxIter',   70, ...
  'PoleType',  3, ...
  'MinOrder',  20, ...
  'MaxOrder',  40, ...
  'Weights',   2, ...
  'Plot',      false,...
  'MSEVARTOL', 1e-2,...
  'FITTOL',    1e-2);

na = ao(plns);

%% Compare with Noisegen2D

aw1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', 1e4));
aw2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', 1e4));

pl = plist(...
    'csd11', csd11, ...
    'csd12', csd12, ...
    'csd21', csd21, ...
    'csd22', csd22, ...
    'MaxIter', 70, ...
    'PoleType', 3, ...
    'MinOrder', 20, ...
    'MaxOrder', 40, ...
    'Weights', 2, ...
    'Plot', false,...
    'Disp', false,...
    'MSEVARTOL', 1e-2,...
    'FITTOL', 1e-2);

[nat1,nat2] = noisegen2D(aw1,aw2, pl);

%% Make psd

naxx1 = na(1).lpsd;
natxx1 = nat1.lpsd;
naxx2 = na(2).lpsd;
natxx2 = nat2.lpsd;


iplot(naxx1,natxx1,csd11)
iplot(naxx2,natxx2,csd22)


%% 3 Dim noise generation

% sampling frequency
fs = 1; % Hz
% frequency vector
f = logspace(-6,log10(0.5),300);
f = f.';

%%% Set the 3 channel model
h11 = pzmodel(plist('GAIN', [0.01], 'POLES', [pz(9.9999999999999995e-007,NaN) pz(0.01,NaN) pz(0.02,NaN)], 'ZEROS', [pz(0.0001,NaN) pz(0.002,NaN)]));
h12 = pzmodel(plist('GAIN', [0.0001], 'POLES', [pz(0.10000000000000001,NaN) pz(0.0030000000000000001,NaN) pz(0.0050000000000000001,NaN) pz(1.0000000000000001e-005,NaN)], 'ZEROS', [pz(0.00050000000000000001,NaN) pz(0.001,NaN)]));
h13 = pzmodel(plist('GAIN', [0.00001], 'POLES', [pz(5.0000000000000004e-006,NaN) pz(0.02,NaN)], 'ZEROS', pz(0.00040000000000000002,NaN)));

h21 = pzmodel(plist('GAIN', [0.00001], 'POLES', [pz(5.0000000000000004e-006,NaN) pz(0.10000000000000001,NaN) pz(0.050000000000000003,NaN)], 'ZEROS', [pz(0.002,NaN) pz(0.00020000000000000001,NaN)]));
h22 = pzmodel(plist('GAIN', [0.001], 'POLES', [pz(0.01,NaN) pz(9.9999999999999995e-008,NaN) pz(0.002,NaN) pz(0.001,NaN)], 'ZEROS', [pz(1.0000000000000001e-005,NaN) pz(2.0000000000000002e-005,NaN) pz(5.0000000000000002e-005,NaN) pz(0.20000000000000001,NaN)]));
h23 = pzmodel(plist('GAIN', [0.0001], 'POLES', [pz(0.01,NaN) pz(0.029999999999999999,NaN) pz(0.10000000000000001,NaN) pz(5.0000000000000002e-005,NaN)], 'ZEROS', [pz(0.0001,NaN) pz(0.0050000000000000001,NaN)]));

h31 = pzmodel(plist('GAIN', [0.001], 'POLES', [pz(0.01,NaN) pz(0.029999999999999999,NaN) pz(0.10000000000000001,NaN) pz(0.00050000000000000001,NaN)], 'ZEROS', [pz(0.0001,NaN) pz(0.0050000000000000001,NaN)]));
h32 = pzmodel(plist('GAIN', [0.00001], 'POLES', [pz(0.01,NaN) pz(0.029999999999999999,NaN) pz(0.10000000000000001,NaN) pz(0.00050000000000000001,NaN) pz(0.00029999999999999997,NaN)], 'ZEROS', [pz(0.002,NaN) pz(0.059999999999999998,NaN)]));
h33 = pzmodel(plist('GAIN', [0.05], 'POLES', [pz(0.0001,NaN) pz(9.9999999999999995e-008,NaN) pz(0.002,NaN)], 'ZEROS', [pz(3.0000000000000001e-005,NaN) pz(5.0000000000000002e-005,NaN) pz(0.10000000000000001,NaN)]));

%%% TFs resps
% filters response
plresp = plist('f',f);

rh11 = resp(h11,plresp);
rh12 = resp(h12,plresp);
rh13 = resp(h13,plresp);

rh21 = resp(h21,plresp);
rh22 = resp(h22,plresp);
rh23 = resp(h23,plresp);

rh31 = resp(h31,plresp);
rh32 = resp(h32,plresp);
rh33 = resp(h33,plresp);

%%% Spectra calculation with Kay style method
% Note that the definition of cross-spectral matrix follows that of s M
% Kay, Modern Spectral Estimation

% csd11 
G11 = rh11.y.*conj(rh11.y) + rh12.y.*conj(rh12.y) + rh13.y.*conj(rh13.y);
G11 = ao(plist('xvals', f, 'yvals', G11, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G11.setName;

% csd12
G12 = conj(rh11.y).*rh21.y + conj(rh12.y).*rh22.y + conj(rh13.y).*rh23.y;
G12 = ao(plist('xvals', f, 'yvals', G12, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G12.setName;

% csd13
G13 = conj(rh11.y).*rh31.y + conj(rh12.y).*rh32.y + conj(rh13.y).*rh33.y;
G13 = ao(plist('xvals', f, 'yvals', G13, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G13.setName;

% csd21
G21 = conj(G12);
G21.setName;

% csd22
G22 = rh21.y.*conj(rh21.y) + rh22.y.*conj(rh22.y) + rh23.y.*conj(rh23.y);
G22 = ao(plist('xvals', f, 'yvals', G22, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G22.setName;

% csd23
G23 = conj(rh21.y).*rh31.y + conj(rh22.y).*rh32.y + conj(rh23.y).*rh33.y;
G23 = ao(plist('xvals', f, 'yvals', G23, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G23.setName;

% csd31
G31 = conj(G13);
G31.setName;

% csd32
G32 = conj(G23);
G32.setName;

% csd33
G33 = conj(rh31.y).*rh31.y + conj(rh32.y).*rh32.y + conj(rh33.y).*rh33.y;
G33 = ao(plist('xvals', f, 'yvals', G33, 'fs', fs, 'dtype', 'fsdata', 'yunits', unit('m^2').*unit('Hz^-1')));
G33.setName;

%%% Build CSD matrix
mod3D = [G11 G12 G13;G21 G22 G23;G31 G32 G33];

%% noise generation

% plist for noise generation

plns = plist(...
  'csd',       mod3D, ...
  'Nsecs',     1e4, ...
  'fs',        fs, ...
  'xunits',    's', ...
  'yunits',    '', ...
  'MaxIter',   90, ...
  'PoleType',  3, ...
  'MinOrder',  20, ...
  'MaxOrder',  50, ...
  'Weights',   2, ...
  'Plot',      false,...
  'MSEVARTOL', 1e-2,...
  'FITTOL',    1e-2);

na = ao(plns);

%% Make psd

naxx1 = na(1).psd;

naxx2 = na(2).psd;

naxx3 = na(3).psd;



iplot(naxx1,G11)
iplot(naxx2,G22)
iplot(naxx3,G33)







