%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test loglikehood_ssm_td
% 
% Test with an harmonic hoscillator and simplex parameters search
% 
% L Ferraioli 11-10-2010
%
% $Id: test_ao_mcmc_td.m,v 1.1 2010/11/26 13:52:21 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)


%% test parameters search with simplex only


mod = ssm(plist('built-in', 'HARMONIC_OSC_1D',...
  'SYMBOLIC PARAMS',{'M', 'K', 'VBETA'}));
inNames = {'COMMAND.Force'};
outNames = {'HARMONIC_OSC_1D.position'};
inNoise = rn;
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,inNoise,'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)

%% Optimal covariance (taken from smodel)

smod = smodel('1./(M.*((2.*i.*pi.*f).^2)+VBETA.*(2.*i.*pi.*f)+K)');
smod.setParams({'M','VBETA','K'},{m,k,damp});
%smod.setParams({'M','VBETA','K'},{2.5, 0.08, 0.1});
smod.setXvar('f');
smod.setXvals(f);
smod.setYunits('kg^-1 s^-2');

% get derivatives
dsmod_M = diff(smod,plist('var','M'));
dsmod_K = diff(smod,plist('var','K'));
dsmod_VBETA = diff(smod,plist('var','VBETA'));

% eval first order term
eM = fftfilt(in,dsmod_M);
eK = fftfilt(in,dsmod_K);
eVBETA = fftfilt(in,dsmod_VBETA);

% information matrix
bm = [eM.y eK.y eVBETA.y];
Im = bm'*bm;

% covariance matrix
Cm = inv(Im);

%% test mcmc_td

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

% c = (1/sqrt(9))^2;
c=0.8;

ranges = {[1 3],[0 2],[0 1]};

x0 = [2.5 0.08 0.1];



pl = plist('N',6000,...
  'Input',in,...
  'cov',c*Cm,... 
  'range',ranges,...
  'parnames',parnames,...
  'noise',inNoise,...
  'model',mod,...
  'inNames',inNames,...
  'outNames',outNames,...
  'search',true,...
  'Tc',[2000 4000],...
  'heat',4,...
  'jumps',[2e0 1e2 1e3 1e4],...
  'x0',x0,...
  'simplex',true,...
  'plot',[1 2 3],...
  'debug',true);

%  metropolis(measurement,noise,model,plist)
b = mcmc_td(out,pl);
% save(b,'stoc6_mcmc_all_9p_23061300_ssm.mat');