%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TEST matrix/MultiChannelNoise
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HISTORY:     23-04-2009 L Ferraioli
%                 Creation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERSION

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

%% Loading spectra

fs = 10;
f = [logspace(-6,log10(fs/2),2000)]';

% currPath = cd;
% cd MDC1 % assuming to start from ..\ltpda\software\m-toolbox\test
% [TF,CSD] = mdc1_tf_models(plist('f',f,'fs',fs));
% cd(currPath)

% get noise psd shapes
% parameters names
parnames = {'DH','S11','S1D','SD1','SDD','Dh1','Dh2','w1','w2',...
  'TH','Th1','Th2'};

% nominal params values
parvalues = {1,1,0,-1e-4,1,1,1,-1.3e-6,-2.0e-6,0.35,0.25,0.28};

% Noise model
Ns = matrix(plist('built-in','mdc3_ifo2ifo_noise'));

% evalueat model
[rw,cl]=size(Ns.objs);
for ii=1:rw
  for jj=1:cl
    Ns.objs(ii,jj).setParams(parnames,parvalues);
    Ns.objs(ii,jj).setXvals(f);
    CSD(ii,jj)=eval(Ns.objs(ii,jj));
    if ii==jj
      CSD(ii,jj)= abs(CSD(ii,jj));
    end
  end
end

%% Plot

plpl = plist('Linecolors', {'k', 'r', 'b'},...
  'LineStyles', {'-', '--', ':'},...
  'LineWidths', {3, 3, 3},...
  'Legends', {'S_{o_1 o_1}', 'S_{o_1 o_\Delta}', 'S_{o_\Delta o_\Delta}'});

% iplot(CSD(1,1),CSD(1,2),CSD(2,1),CSD(2,2))
iplot(CSD(1,1),CSD(1,2),CSD(2,2),plpl)

%%
%--------------------------------------------------------------------------
% 2 Dimensional Noisegen Filter
%--------------------------------------------------------------------------


%% 2 dim noisegen filter in z domain - symbolic precision All pass

% plist for noise generation
plns = plist(...
  'csd',       CSD, ...
  'targetobj',  'miir', ...
  'fs',        fs, ...
  'MaxIter',   70, ...
  'PoleType',  3, ...
  'MinOrder',  25, ...
  'MaxOrder',  55, ...
  'Weights',   2, ...
  'Plot',      false,...
  'MSEVARTOL', 1e-1,...
  'FITTOL',    1e-4,...
  'UseSym',    'on',...
  'iunits',    'm',...
  'ounits',    'm');

na = matrix(plns);

%% Check response of CSD

na11 = resp(na.objs(1,1).filters,plist('f',f,'bank','parallel'));
na12 = resp(na.objs(1,2).filters,plist('f',f,'bank','parallel'));
na21 = resp(na.objs(2,1).filters,plist('f',f,'bank','parallel'));
na22 = resp(na.objs(2,2).filters,plist('f',f,'bank','parallel'));

mna = matrix([na11 na12;na21 na22]);

ECSD = mna*conj(transpose(mna));

for kk = 1:numel(CSD)
  CSD(kk).setYunits((unit('m')^2)*(unit('Hz')^-1));
  CSD(kk).setXunits(unit('Hz'));
  ECSD.objs(kk).setYunits((unit('m')^2)*(unit('Hz')^-1));
  ECSD.objs(kk).setXunits(unit('Hz'));
end

% calculate cross-coherence
COH = CSD(1,2)./sqrt(CSD(1,1).*CSD(2,2));
COH.simplifyYunits;
RCOH = real(COH);
ICOH = imag(COH);

ECOH = ECSD.objs(1,2)./sqrt(ECSD.objs(1,1).*ECSD.objs(2,2));
ECOH.simplifyYunits;
RECOH = real(ECOH);
IECOH = imag(ECOH);

% set plist for plotting

plpl = plist('Linecolors', {'k', 'r', 'b'},...
  'LineStyles', {'-', '--', ':'},...
  'LineWidths', {3, 3, 3},...
  'Legends', {'Model', 'Fit', 'Residuals'});

iplot(CSD(1,1),ECSD.objs(1,1),abs(CSD(1,1)-ECSD.objs(1,1)),plpl)
iplot(CSD(2,2),ECSD.objs(2,2),abs(CSD(2,2)-ECSD.objs(2,2)),plpl)

plpl = plist('Linecolors', {'k', 'r', 'b'},...
  'LineStyles', {'-', '--', ':'},...
  'LineWidths', {3, 3, 3},...
  'Legends', {'Model', 'Fit', 'Residuals'},...
  'Yscales', {'All', 'lin'},...
  'Ylabels', {'All', 'Re(coh)'});

iplot(RCOH,RECOH,abs(RCOH-RECOH),plpl)

plpl = plist('Linecolors', {'k', 'r', 'b'},...
  'LineStyles', {'-', '--', ':'},...
  'LineWidths', {3, 3, 3},...
  'Legends', {'Model', 'Fit', 'Residuals'},...
  'Yscales', {'All', 'lin'},...
  'Ylabels', {'All', 'Im(coh)'});

iplot(ICOH,IECOH,abs(ICOH-IECOH),plpl)

%% Check response of Filters

% na11 = resp(na.objs(1,1).filters,plist('f',f,'bank','parallel'));
% na12 = resp(na.objs(1,2).filters,plist('f',f,'bank','parallel'));
% na21 = resp(na.objs(2,1).filters,plist('f',f,'bank','parallel'));
% na22 = resp(na.objs(2,2).filters,plist('f',f,'bank','parallel'));


% iplot(TF(1,1),na11,abs(TF(1,1) - na11))
% iplot(TF(1,2),na12,abs(TF(1,2) - na12))
% iplot(TF(2,1),na21,abs(TF(2,1) - na21))
% iplot(TF(2,2),na22,abs(TF(2,2) - na22))

iplot(na11,na12,na21,na22)

%%
%--------------------------------------------------------------------------
% 2 Dimensional Noise Generation
%--------------------------------------------------------------------------
nsecs = 3e5;
plng = plist(...
  'model',   na,...
  'Nsecs',   nsecs,...
  'fs',      fs,...
  'yunits',  'm');

no = matrix(plng);

%% Check psd

no1 = no.objs(1);
no2 = no.objs(2);

no1xx = no1.psd;
no2xx = no2.psd;

iplot(no1xx,csd11,no2xx,csd22)

%% Check cross-coherence

no1 = no.objs(1);
no2 = no.objs(2);
nfft = 6e4;

no12xx = cpsd(no1,no2,plist('nfft',nfft));
no1xx = no1.psd(plist('nfft',nfft));
no2xx = no2.psd(plist('nfft',nfft));
%%

ecoh = no12xx./(sqrt(no1xx.*no2xx));
recoh = real(ecoh);
iecoh = imag(ecoh);

iplot(recoh,RECOH,plist('Yscales',{'All','lin'}))
iplot(iecoh,IECOH,plist('Yscales',{'All','lin'}))

%% Get averaged noise spectral curves

N = 500;
nsecs = 3e5;
nfft = 6e4;

Mnxx1 = 0; Mn2xx1 = 0;
Mnxx2 = 0; Mn2xx2 = 0;

Mnxy1 = 0; Mn2xy1 = 0;
Mnxy2 = 0; Mn2xy2 = 0;

tic
for ii = 1:N
  
  fprintf('===== iter %s =====\n',num2str(ii))
  
  % noise generation
  plng = plist(...
    'model',   na,...
    'Nsecs',   nsecs,...
    'fs',      fs,...
    'yunits',  'm');

  no = matrix(plng);
  
  % periodogram calculation
  no1 = no.objs(1);
  no2 = no.objs(2);

  no1xx = no1.psd;
  no2xx = no2.psd;
  f1 = no1xx.x;
  
  % running average and variance for periodogram
  if ii == 1
    Mnxx1 = no1xx.y;
    Mnxx2 = no2xx.y;
  else
%     Qxx1 = no1xx.y - Mnxx1;
    Mnxx1 = Mnxx1 + (no1xx.y - Mnxx1)./ii;
    Mn2xx1 = Mn2xx1 + (no1xx.y - Mnxx1).*(no1xx.y - Mnxx1);
    
%     Qxx2 = no2xx.y - Mnxx2;
    Mnxx2 = Mnxx2 + (no2xx.y - Mnxx2)./ii;
    Mn2xx2 = Mn2xx2 + (no2xx.y - Mnxx2).*(no2xx.y - Mnxx2);
  end
  clear no1xx no2xx
  
  
  % cross coherence calculation
  no12xx = cpsd(no1,no2,plist('nfft',nfft));
  no1xx2 = no1.psd(plist('nfft',nfft));
  no2xx2 = no2.psd(plist('nfft',nfft));
  f2 = no1xx2.x;

  ecoh = (no12xx.y)./(sqrt((no1xx2.y).*(no2xx2.y)));
  recoh = real(ecoh);
  iecoh = imag(ecoh);
  
  clear no12xx no1xx2 no2xx2  
  % running average and variance for cross coherence
  if ii == 1
    Mnxy1 = recoh;
    Mnxy2 = iecoh;
  else
%     Qxy1 = recoh - Mnxy1;
    Mnxy1 = Mnxy1 + (recoh - Mnxy1)./ii;
    Mn2xy1 = Mn2xy1 + (recoh - Mnxy1).*(recoh - Mnxy1);
    
%     Qxy2 = iecoh - Mnxy2;
    Mnxy2 = Mnxy2 + (iecoh - Mnxy2)./ii;
    Mn2xy2 = Mn2xy2 + (iecoh - Mnxy2).*(iecoh - Mnxy2);
  end
  
end
toc

% get mean and variance for psd 1
Sxx1 = Mnxx1; % mean
if N == 1
  Svxx1 = Sxx1;
else
  Svxx1 = (Mn2xx1/(N-1))/N; % variance
end
% get mean and variance for psd 2
Sxx2 = Mnxx2; % mean
if N == 1
  Svxx2 = Sxx2;
else
  Svxx2 = (Mn2xx2/(N-1))/N; % variance
end
% get mean and variance for real part of cross coherence
Sxy1 = Mnxy1; % mean
if N == 1
  Svxy1 = Sxy1;
else
  Svxy1 = (Mn2xy1/(N-1))/N; % variance
end
% get mean and variance for imaginary part of cross coherence
Sxy2 = Mnxy2; % mean
if N == 1
  Svxy2 = Sxy2;
else
  Svxy2 = (Mn2xy2/(N-1))/N; % variance
end

% save results

save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx1.mat','Sxx1')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx1.mat','Svxx1')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx2.mat','Sxx2')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx2.mat','Svxx2')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxy1.mat','Sxy1')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxy1.mat','Svxy1')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxy2.mat','Sxy2')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxy2.mat','Svxy2')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\f1.mat','f1')
save('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\f2.mat','f2')
save(na,'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\na.mat')

%% Load saved data

load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx2.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx2.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxy1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxy1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxy2.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxy2.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\f1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\f2.mat'

%% Plot spectra

figure()

p22 = loglog(f1(1:end-1),Sxx1(1:end-1) + sqrt(Svxx1(1:end-1)),'b','LineWidth',3);
hold on
grid on
p23 = loglog(f1(1:end-1),Sxx1(1:end-1) - sqrt(Svxx1(1:end-1)),'b','LineWidth',3);
p21 = loglog(f1(1:end-1),Sxx1(1:end-1),'r','LineWidth',3);

p11 = loglog(ECSD.objs(1,1).x,ECSD.objs(1,1).y,'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('amplitude [m^2 Hz^{-1}]','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')

figure()

p22 = loglog(f1(1:end-1),Sxx2(1:end-1) + sqrt(Svxx2(1:end-1)),'b','LineWidth',3);
hold on
grid on
p23 = loglog(f1(1:end-1),Sxx2(1:end-1) - sqrt(Svxx2(1:end-1)),'b','LineWidth',3);
p21 = loglog(f1(1:end-1),Sxx2(1:end-1),'r','LineWidth',3);

p11 = loglog(ECSD.objs(2,2).x,ECSD.objs(2,2).y,'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('amplitude [m^2 Hz^{-1}]','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')


%% Plot coherence

figure()

p22 = semilogx(f2,Sxy1 + sqrt(Svxy1),'b','LineWidth',3);
hold on
grid on
p23 = semilogx(f2,Sxy1 - sqrt(Svxy1),'b','LineWidth',3);
p21 = semilogx(f2,Sxy1,'r','LineWidth',3);

p11 = semilogx(RECOH.x,RECOH.y,'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('Re(coh) []','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')

figure()

p22 = semilogx(f2,Sxy2 + sqrt(Svxy2),'b','LineWidth',3);
hold on
grid on
p23 = semilogx(f2,Sxy2 - sqrt(Svxy2),'b','LineWidth',3);
p21 = semilogx(f2,Sxy2,'r','LineWidth',3);

p11 = semilogx(IECOH.x,IECOH.y,'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('Im(coh) []','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')

%% Check response of CSD

currPath = cd;
cd MDC1 % assuming to start from ..\ltpda\software\m-toolbox\test
[TF,CSD] = mdc1_tf_models(plist('f',f1,'fs',fs));
cd(currPath)

na11 = resp(na.objs(1,1).filters,plist('f',f1,'bank','parallel'));
na12 = resp(na.objs(1,2).filters,plist('f',f1,'bank','parallel'));
na21 = resp(na.objs(2,1).filters,plist('f',f1,'bank','parallel'));
na22 = resp(na.objs(2,2).filters,plist('f',f1,'bank','parallel'));

mna = matrix([na11 na12;na21 na22]);

ECSD = mna*conj(transpose(mna));

for kk = 1:numel(CSD)
  CSD(kk).setYunits((unit('m')^2)*(unit('Hz')^-1));
  CSD(kk).setXunits(unit('Hz'));
  ECSD.objs(kk).setYunits((unit('m')^2)*(unit('Hz')^-1));
  ECSD.objs(kk).setXunits(unit('Hz'));
end

plpl = plist('Linecolors', {'k', 'r', 'b'},...
  'LineStyles', {'-', '--', ':'},...
  'LineWidths', {3, 3, 3},...
  'Legends', {'Model', 'Fit', 'Residuals'});

iplot(CSD(1,1),ECSD.objs(1,1),abs(CSD(1,1)-ECSD.objs(1,1)),plpl)
iplot(CSD(2,2),ECSD.objs(2,2),abs(CSD(2,2)-ECSD.objs(2,2)),plpl)

%% get expected response + window

% we should considere that the window has a spectral response that can add
% some systematics inside our spectral estimations. Since the window is a
% product in time domain with the time series, that it corresponds to a
% convolution in frequency domain. We can ifft the frequency response of
% the filters, multiply in time domain for the window and then come back to
% freq domain by fft.
na = matrix('C:\Users\Luigi\Lavori\ltp_noisegen\matlab\na.mat');
%%
% filter responses
fs = 10;
nsecs = 3e5;
nfft = nsecs.*fs;
ff1 = (0:floor(nfft/2)).*fs./nfft;

rsp11 = resp(na.objs(1,1).filters,plist('f',ff1,'bank','parallel'));
rsp11 = rsp11.y;
% rsp11 = [rsp11(1:end-1);flipud(conj(rsp11(2:end)))];
rsp12 = resp(na.objs(1,2).filters,plist('f',ff1,'bank','parallel'));
rsp12 = rsp12.y;
% rsp12 = [rsp12(1:end-1);flipud(conj(rsp12(2:end)))];
rsp21 = resp(na.objs(2,1).filters,plist('f',ff1,'bank','parallel'));
rsp21 = rsp21.y;
% rsp21 = [rsp21(1:end-1);flipud(conj(rsp21(2:end)))];
rsp22 = resp(na.objs(2,2).filters,plist('f',ff1,'bank','parallel'));
rsp22 = rsp22.y;
% rsp22 = [rsp22(1:end-1);flipud(conj(rsp22(2:end)))];
clear na

% psd calc (npsd should contain nfft samples)
npsd11 = rsp11.*conj(rsp11) + rsp12.*conj(rsp12);
% npsd11 = [npsd11;flipud(npsd11(2:end-1))];
npsd22 = rsp21.*conj(rsp21) + rsp22.*conj(rsp22);
% npsd22 = [npsd22;flipud(npsd22(2:end-1))];

%%

% get window frequency response

% BH92 in frequency domain
W = BH92(2.*pi.*ff1./fs,nfft);
W = W./max(abs(W));
W = W.*conj(W);

%% ifft dello spettro

% npsd11 = [npsd11;flipud(npsd11(2:end-1))]./2;
% npsd22 = [npsd22;flipud(npsd22(2:end-1))]./2;

% g11 = ifft(npsd11);
% g22 = ifft(npsd22);

W = blackmanharris(nfft);
if size(W,2)>1
  W = W.';
end
W = W.^2;
W = ifftshift(W);

WS11 = fft(W.*g11);
WS22 = fft(W.*g22);

WS11 = 2.*WS11(1:numel(ff1));
WS22 = 2.*WS22(1:numel(ff1));

%% get convolution

% WS11 = conv(W,npsd11);
% WS11 = WS11(1:numel(ff1));


%% Plot spectra

load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx1.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Sxx2.mat'
load 'C:\Users\Luigi\Lavori\ltp_noisegen\matlab\Svxx2.mat'
%%
figure()

p22 = loglog(ff1(1:end-1),Sxx1(1:end-1) + sqrt(Svxx1(1:end-1)),'b','LineWidth',3);
hold on
grid on
p23 = loglog(ff1(1:end-1),Sxx1(1:end-1) - sqrt(Svxx1(1:end-1)),'b','LineWidth',3);
p21 = loglog(ff1(1:end-1),Sxx1(1:end-1),'r','LineWidth',3);

p11 = loglog(ff1(1:end),WS11(1:end),'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('amplitude [m^2 Hz^{-1}]','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')

%%
figure()

p22 = loglog(ff1(1:end-1),Sxx2(1:end-1) + sqrt(Svxx2(1:end-1)),'b','LineWidth',3);
hold on
grid on
p23 = loglog(ff1(1:end-1),Sxx2(1:end-1) - sqrt(Svxx2(1:end-1)),'b','LineWidth',3);
p21 = loglog(ff1(1:end-1),Sxx2(1:end-1),'r','LineWidth',3);

p11 = loglog(ff1(1:end-1),WS22(1:end-1),'k','LineWidth',3);


xlabel('Frequency [Hz]','fontsize',18)
ylabel('amplitude [m^2 Hz^{-1}]','fontsize',18)
legend([p11(1) p21(1) p22(1)],'Fit Model', 'Data', 'Error \pm \sigma')

%% get statistics

Sn11 = (npsd11(14:end-1)-Sxx1(14:end-1))./npsd11(14:end-1);
Sn11 = ao(cdata(Sn11));
Sn22 = (npsd22(14:end-1)-Sxx2(14:end-1))./npsd22(14:end-1);
Sn22 = ao(cdata(Sn22));

% get equivalent normal distribution and histogram
hpl1 = plist('N', 50);
hpl2 = plist('N', 200);

dS11h = hist(Sn11,hpl1);
dS11h = dS11h./max(dS11h);
dS11n = normdist(Sn11,hpl2);
dS11n = dS11n./max(dS11n);

dS22h = hist(Sn22,hpl1);
dS22h = dS22h./max(dS22h);
dS22n = normdist(Sn22,hpl2);
dS22n = dS22n./max(dS22n);

%% check

iplot(dS11h,dS11n)
iplot(dS22h,dS22n)

%% Plot results of the statistics analysis

figure()

p1 = bar(dS11h.x,dS11h.y,'b');
hold on
grid on
p2 = plot(dS11n.x,dS11n.y,'r','LineWidth',3);
 
xlabel('Normalized Deviation [\Deltax / x]','fontsize',18)
ylabel('Normalized Counts','fontsize',18)
legend([p1(1) p2(1)],'Histogram', 'Equivalent Norm. Distr.')

figure()

p1 = bar(dS22h.x,dS22h.y,'b');
hold on
grid on
p2 = plot(dS22n.x,dS22n.y,'r','LineWidth',3);
 
xlabel('Normalized Deviation [\Deltax / x]','fontsize',18)
ylabel('Normalized Counts','fontsize',18)
legend([p1(1) p2(1)],'Histogram', 'Equivalent Norm. Distr.')



%%
m1 = mean(Sn11.y);
m2 = mean(Sn22.y);
s1 = std(Sn11.y);
s2 = std(Sn22.y);





%%

nfft = 1e2;
fs = 10;
frq = linspace(0,pi,10000);
% D=DirichletKernel(frq,nfft);
% D=D./max(D);
% figure;
% plot(frq,20.*log10(D))
% grid on
W=BH92(frq,nfft);
% W=W./max(W);
figure;
plot(frq./pi,20.*log10(W))
grid on