% test for ao/linlsq
% 
% 17-11-2009 L Ferraioli
%       CREATION
% 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $Id: mdc3_exp3_loop_v3.m,v 1.2 2009/09/24 09:48:12 luigi Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% Loading data

fprintf('===== loading data... =====\n')

% laod parnames and values
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\parnames_10perc.mat
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_nomvalues.mat
load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\exp3_2_10perc_truevalues.mat
% load C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Models\usedparams_10perc.mat


% set ordered used parameters
usedparams = {'dH','dsH','dS11','dS1D','dSD1','dSDD',...
  'dh2','dsh2','dx1','dx2'};

[nonfitparnms,ia,ib] = setxor(parnames,usedparams);
nonfitparvals = exp3_nomvalues(ia);

%% get non-linear response model
% the model is non-linear in the parameter dependence
fprintf('===== Get TF Model... =====\n')

H = matrix(plist('built-in','mdc3_ifo2ifo_v2'));

%% load input signal
% those are the same of exp 1

fprintf('===== Loading input signals... =====\n')

oi1 = ao('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\signals_noise\exp1_1_oi1.mat');
oi1.setName;
oi1.setYunits('m');
oid = ao('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\signals_noise\exp1_2_oid.mat');
oid.setName;
oid.setYunits('m');

%% get signals - true values

fprintf('===== get true values... =====\n')

% get response with true params
for ii = 1:numel(H.objs)
  H.objs(ii).setParams(parnames,exp3_truevalues);
end

plfft = plist('Npad',[]);

s11 = fftfilt(oi1,H.objs(1,1),plfft);
s12 = fftfilt(oid,H.objs(1,2),plfft);
s21 = fftfilt(oi1,H.objs(2,1),plfft);
s22 = fftfilt(oid,H.objs(2,2),plfft);

% get signals for exp 3.1
s1_exp_3_1 = s11;
s1_exp_3_1.setName;
sd_exp_3_1 = s21;
sd_exp_3_1.setName;

% get signals for exp 3.2
s1_exp_3_2 = s12;
s1_exp_3_2.setName;
sd_exp_3_2 = s22;
sd_exp_3_2.setName;

%% load Coloring filters

fprintf('===== loading coloring filters... =====\n')

cf11 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF11.mat');
cf11.setName;
cf12 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF12.mat');
cf12.setName;
cf21 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF21.mat');
cf21.setName;
cf22 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_CF22.mat');
cf22.setName;

%% load Whitening filters

fprintf('===== loading whitening filters... =====\n')

wf11 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF11.mat');
wf11.setName;
wf12 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF12.mat');
wf12.setName;
wf21 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF21.mat');
wf21.setName;
wf22 = miir('C:\Users\Luigi\ltp_data_analysis\MDCs\MDC3\lf_analysis\Filters\exp1_WF22.mat');
wf22.setName;

%% Adding noise to signals

fprintf('===== adding noise to signals... =====\n')

% get params
Nsecs = s1_exp_3_1.nsecs;
fs    = s1_exp_3_1.fs;
plcf = plist('bank','parallel');

% starting noise generation exp1.1
a1_exp_3_1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));
a2_exp_3_1 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));

% coloring noise exp 1.1
na1_exp_3_1 = filter(a1_exp_3_1,cf11,plcf) + filter(a2_exp_3_1,cf12,plcf);
na2_exp_3_1 = filter(a1_exp_3_1,cf21,plcf) + filter(a2_exp_3_1,cf22,plcf);

% starting noise generation exp 1.2
a1_exp_3_2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));
a2_exp_3_2 = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs));

% coloring noise exp 1.2
na1_exp_3_2 = filter(a1_exp_3_2,cf11,plcf) + filter(a2_exp_3_2,cf12,plcf);
na2_exp_3_2 = filter(a1_exp_3_2,cf21,plcf) + filter(a2_exp_3_2,cf22,plcf);

% adding noise to signals
o1_exp_3_1 = s1_exp_3_1 + na1_exp_3_1;
od_exp_3_1 = sd_exp_3_1 + na2_exp_3_1;

o1_exp_3_2 = s1_exp_3_2 + na1_exp_3_2;
od_exp_3_2 = sd_exp_3_2 + na2_exp_3_2;

% %%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%%%% Test on one dimensional data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% %% Build input collection
% 
% H11 = H.objs(1,1);
% H11.setParams(parnames,exp3_nomvalues);
% % subs for non fit parameters
% H11.subs(plist('Params', nonfitparnms, 'Values',nonfitparvals));
% % linearize
% dH11 = linearize(H11);
% 
% H22 = H.objs(2,2);
% H22.setParams(parnames,exp3_nomvalues);
% % subs for non fit parameters
% H22.subs(plist('Params', nonfitparnms, 'Values',nonfitparvals));
% % linearize
% dH22 = linearize(H22);
% 
% %%
% iC1 = collection(o1_exp_3_1,s1_exp_3_1,H11,dH11,wf11);
% iC2 = collection(od_exp_3_1,sd_exp_3_1,H22,dH22,wf22);
% 
% oC = linlsq(iC1,iC2,plist('Nloops',3));

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Test on two dimensional data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Build input collection

% empty ao
eao = ao();

% subs for non fit parameters
for ii=1:numel(H.objs)
  H.objs(ii).setParams(parnames,exp3_nomvalues);
  % subs for non fit parameters
  H.objs(ii).subs(plist('Params', nonfitparnms, 'Values',nonfitparvals));
end
% linearize
dH = linearize(H);

% whitening filter
wf11b = filterbank(plist('filters',wf11,'type','parallel'));
wf11b.setName('wf11');
wf12b = filterbank(plist('filters',wf12,'type','parallel'));
wf12b.setName('wf12');
wf21b = filterbank(plist('filters',wf21,'type','parallel'));
wf21b.setName('wf21');
wf22b = filterbank(plist('filters',wf22,'type','parallel'));
wf22b.setName('wf22');

WF = matrix(wf11b,wf21b,wf12b,wf22b,plist('shape',[2 2]));

% exp_3_1
os1 = matrix(o1_exp_3_1,od_exp_3_1,plist('shape',[2 1]));
is1 = matrix(oi1,eao,plist('shape',[2 1]));

% exp_3_2
os2 = matrix(o1_exp_3_2,od_exp_3_2,plist('shape',[2 1]));
is2 = matrix(eao,oid,plist('shape',[2 1]));

iC1 = collection(os1,is1,H,dH,WF);
iC2 = collection(os2,is2,H,dH,WF);

%% add on ground experiments

dS11 = ao(cdata(0));
dS11.setName;
dS11.setDy(1e-4);

dS1D = ao(cdata(0));
dS1D.setName;
dS1D.setDy(1e-3);

dSDD = ao(cdata(0));
dSDD.setName;
dSDD.setDy(1e-3);

%% do fit

oC = linlsq(iC1,iC2,dS11,dS1D,dSDD,plist('Nloops',1,'Npad',[]));