%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test loglikehood_ssm_td
% 
% Test with an harmonic hoscillator and simplex parameters search
% 
% L Ferraioli 11-10-2010
%
% $Id: test_loglikehood_ssm_td.m,v 1.3 2010/11/26 13:52:32 luigi Exp $
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% set parameters

m = 2; % kg
k = 0.1; % kg s^-2
damp = 0.05; % kg s^-1

fAmp = 3;
NAmp = 1;
fs = 1;

cutbefore = 100*fs;
cutafter = 500*fs;

f = logspace(-5+log10(fs),log10(fs/2),300);

%% Load model

sys = ssm(plist('built-in', 'HARMONIC_OSC_1D', 'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

pl = plist('inputs', 'COMMAND.Force', 'HARMONIC_OSC_1D.outputs', 'Position', 'f', f);
h = bode(sys,pl);

iplot(h)

%% input force and readout noise

% input force
fc = ao(plist('waveform','sine wave','A',fAmp,'f',1e-1,'phi',0,'toff',cutbefore/fs,'nsecs',6e2,'fs',fs));
in = zeropad(fc,plist('Position','post','N',cutafter));


rn = ao(plist('tsfcn', 'randn(size(t))', 'fs', fs, 'nsecs', in.nsecs));
rn = rn.*NAmp;



%% Simulate the system


plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force','noise.readout'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in,rn]);
out = simulate(sys,plsym);

iplot(out,rn)




%% Do chisquare parameter search


mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
parnames = {'m', 'k', 'vbeta'};


x0 = [1 1 1];
x0 = [2.5 0.08 0.1];


% loglk =
% loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)

xp2 = fminsearch(@(x)utils.math.chisquare_ssm_td(x,in,out,parnames,mod,inNames,outNames,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));



%%

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
outfit2 = simulate(sys,plsym);

iplot(out,outfit2)
iplot(out-outfit2)
iplot(out-outfit2-rn)

% xp =
% 
%           2.22352087931512         0.588010809495998         0.195689637008003



%% Do parameters search


mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
inNoiseNames = {'noise.readout'};
inNoise = rn;
outNoiseNames = {'HARMONIC_OSC_1D.position'};
parnames = {'M', 'K', 'VBETA'};


% x0 = xp2;
x0 = [2.5 0.08 0.1];

% loglk =
% loglikehood_ssm_td(xp,in,out,parnames,model,inNames,outNames,varargin)



xp = fminsearch(@(x)utils.math.loglikehood_ssm_td(x,in,out,parnames,mod,inNames,outNames,rn,'cutbefore',cutbefore,'cutafter',cutafter),x0,optimset('Display','iter'));



%%

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
outfit = simulate(sys,plsym);

iplot(out,outfit)
iplot(out-outfit)
iplot(out-outfit-rn)


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% test covariace estimation

mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
parnames = {'m', 'k', 'vbeta'};

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
modeout = simulate(sys,plsym);

% get noise
noise = out - modeout;
nn = noise.y;
% get covariance matrix
cmat = utils.math.xCovmat(nn);
cmatseq = cmat(1,:);

% get covariance sequance
[cseq,lags] = xcov(nn,'unbiased');
cseq = ifftshift(cseq);
lags=ifftshift(lags);
cseqr = cseq(1:numel(cmatseq));
lagsr = lags(1:numel(cmatseq));
lagsr = lagsr.';

% compare results
figure
plot(lagsr,cmatseq.','k')
hold on
plot(lagsr,cseqr,'r')

figure
plot(lagsr,cmatseq.'-cseqr,'k')

[R,p] = chol(cmat);
d = eig(cmat);
% md = min(d);
% cmat2 = cmat + abs(md).*eye(size(cmat));
% [R,p] = chol(cmat2);

ccmat = fliplr(flipud(cmat));
[RR,pp] = chol(ccmat);

%% correlation matrix

[corr,sig] = utils.math.cov2corr(cmat);

% test positive definiteness
[R,p] = chol(corr);

% get a valid correlation
newcorr = validcorr(corr);

%% 

% get covariance sequance
[cseq,lags] = xcov(nn,'unbiased');
Num = numel(nn);
for ii=1:Num
  
  ccmat(ii,:) = cseq(Num-ii+1:2*Num-ii);
  
end

[R,p] = chol(ccmat);


%% test the likelihood

mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
parnames = {'m', 'k', 'vbeta'};

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[m k damp]));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
modeout1 = simulate(sys,plsym);

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[1 1 1]));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
modeout2 = simulate(sys,plsym);

% xp = [1.1        0.6         0.01];

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
modeout3 = simulate(sys,plsym);

% xp2 = [2.22352087931512         0.588010809495998         0.195689637008003];

sys = copy(mod,1);
sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',xp2));
sys.keepParameters();
sys.modifyTimeStep(plist('newtimestep',1/fs));

plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
  'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
  'AOS',[in]);
modeout4 = simulate(sys,plsym);

% get noise
noise1 = out - modeout1; % true values
noise2 = out - modeout2; % starting guess
noise3 = out - modeout3; % fit values
noise4 = out - modeout4; % chi2 fit values


lik1 = utils.math.loglikehood_td(noise1,'cutbefore',cutbefore,'cutafter',cutafter)
lik2 = utils.math.loglikehood_td(noise2,'cutbefore',cutbefore,'cutafter',cutafter)
lik3 = utils.math.loglikehood_td(noise3,'cutbefore',cutbefore,'cutafter',cutafter)
lik4 = utils.math.loglikehood_td(noise4,'cutbefore',cutbefore,'cutafter',cutafter)

%% test a uniform grid

mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
parnames = {'m', 'k', 'vbeta'};

mm = [1.8:0.02:2.2];
kc = [0.1:0.02:1];
dd = [0.01:0.02:1];

idx = 1;
pars = struct;

Ntot = numel(mm)*numel(kc)*numel(dd);

lik = zeros(Ntot,1);
%%

for ii=1:numel(mm)
  for jj=1:numel(kc)
    for kk=1:numel(dd)
      
      fprintf('step %s of %s\n',num2str(idx),num2str(Ntot));
      pars(idx).m = mm(ii);
      pars(idx).k = kc(jj);
      pars(idx).damp = dd(kk);
      
      sys = copy(mod,1);
      sys = sys.setParameters(plist('names',{'M', 'K', 'VBETA'},'values',[mm(ii) kc(jj) dd(kk)]));
      sys.keepParameters();
      sys.modifyTimeStep(plist('newtimestep',1/fs));

      plsym = plist('AOS VARIABLE NAMES',{'COMMAND.Force'},...
        'RETURN OUTPUTS','HARMONIC_OSC_1D.position',...
        'AOS',[in]);
      modeout = simulate(sys,plsym);
      
      noise = out - modeout;
      
      lik(idx) = utils.math.loglikehood_td(noise,'cutbefore',cutbefore,'cutafter',cutafter);
      
      idx = idx +1;
    end
  end
end