% test for matrix/linfitsvd
% 
% 17-11-2009 L Ferraioli
%       CREATION
% 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% $Id: test_matrix_linfitsvd.m,v 1.3 2010/04/25 21:27:57 mauro Exp $
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Definitions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

pl_retrieve = plist('hostname', 'btlab.science.unitn.it', 'database', 'algo_testing');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Loading data %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Loading parameters cells

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

% define parameters names and values
parnames = { 'S11',  'S1D', 'SD1', 'SDD', ...
  'TH', 'Th2', 'dH', ...
  'dS11', 'dS1D', 'dSD1', 'dSDD', ...
  'dh1', 'dh2', ...
  'dsH', 'dsh1', 'dsh2', ...
  'dx1', 'dx2', ...
  'w0', 'w1', 'w2'};

exp3_nomvalues = {1 0 0 1 ...
  0.35 0.28 0 ...
  0 0 0 0, ...
  0 0, ...
  0 0 0, ...
  0 0, ...
  0.188 0 + 0.00114i 0 + 0.00141i ...
  };

exp3_truevalues = {1 0 0 1 ...
  0.35 0.28 0.0526103537824434 ...
  0 0 -0.0001 0, ...
  0.0753993274155418 0.0569786061730967, ...
  0.0670701072189726 0.00576108053758739 -0.0846457231187905, ...
  -0.0456546316180145 0.0395739758886965, ...
  0.188 0 + 0.00114i 0 + 0.00141i ...
  };

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

%% 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(pl_retrieve.pset('id', 1));
oid = ao(pl_retrieve.pset('id', 2));

%% load Coloring filters

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

cf = matrix(pl_retrieve.pset('id', 26));
cf11 = cf.getObjectAtIndex(1, 1);
cf12 = cf.getObjectAtIndex(1, 2);
cf21 = cf.getObjectAtIndex(2, 1);
cf22 = cf.getObjectAtIndex(2, 2);

%% load Whitening filters

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

WF = matrix(pl_retrieve.pset('id', 27));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Generating signals %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Get signals output of the system

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

mod = copy(H, true);

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

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

s11 = fftfilt(oi1,mod.objs(1,1),plfft);
s12 = fftfilt(oid,mod.objs(1,2),plfft);
s21 = fftfilt(oi1,mod.objs(2,1),plfft);
s22 = fftfilt(oid,mod.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;


%% 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');
pl_noise    = plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', Nsecs);

% starting noise generation exp1.1
a1_exp_3_1 = ao(pl_noise);
a2_exp_3_1 = ao(pl_noise);

% 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(pl_noise);
a2_exp_3_2 = ao(pl_noise);

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% Build input for linfitsvd %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Build input objects

% empty ao
eao = ao();

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

%%% 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]));

%%
%%% Input signals
iS = collection(is1,is2);

%%% Known parameters
vals = [0 0 0];
nms  = {'dS11','dS1D','dSDD'};
errs = [1e-4 1e-3 1e-4];
kwnpar = pest(vals, nms, errs);


%% do fit

plfit = plist(...
  'FitParams',usedparams,...
  'Model',H,...
  'Input',iS,...
  'WhiteningFilter',WF,...
  'Nloops',30,...
  'Ncut',100,...
  'KnownParams',kwnpar);

opars = linfitsvd(os1,os2,plfit);