% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = psabr32(t,T,f,F,alpha,A,beta,nu,rho)
% Wu expansion of SABR density (2nd order)
tau = (T-t)/T;
if beta == 1    % account for log normal setting
    u = (log(f) - log(F))./(alpha * sqrt(T));
else
    u = (f^(1-beta)-F.^(1-beta))/(alpha*(1-beta)*sqrt(T));
end
v = log(alpha./A)./(nu*sqrt(T));
a11 = -beta * F^(beta-1).*A./nu.*(u-rho*v);
a10 = u^2*v-rho*u*v.^2;

a = 2*rho+beta*F^(beta-1)*A./nu;
b = 2+beta*(1-beta)*F^(2*(beta-1))*A.^2./nu^2;
c = beta*F^(beta-1)*A./nu;

a23 = rho^4*(20-6*b)-12*rho^3*a+rho^2*(3*a.^2-28+12*b) ...
    +12*a*rho+8-3*a.^2-6*b;
a22 = u^2*(3*a.^2-12*a*rho+6*b*(-1+rho^2)+2*rho^2+10)...
    - 2*u*v.*(rho^3*(2+3*b)+rho^2*(-9*a+3*c) ...
    +rho*(10+3*a.^2-3*b)-(3*a+3*c)) + v.^2.*((2+3*(a-2*rho).^2)*rho^2 ...
    + 6*c*rho*(-1+rho^2)-2);
a21 = u^4+v.^4*rho^2+u^3*v.*(8*rho-6*a)+u*v.^3.*(8*rho^3-6*a*rho^2) ...
    +u^2*v.^2.*(-14*rho^2+12*a*rho-4);
a20 = 3*u^4*v.^2-6*u^3*v.^3*rho+3*u^2*v.^4*rho^2;
y = 1./(nu*T*F^beta*A.^2).*(1+nu*sqrt(T)./(2*(-1+rho^2)).*(a11+a10/tau) ...
    +(nu^2*T)./(24*(1-rho^2)^2).*(a23*tau+a22+a21/tau+a20/tau^2))...
    ./(2*pi*tau*sqrt(1-rho^2)).*exp(-(u^2-2*rho*u*v+v.^2) ...
    ./(2*tau*(1-rho^2)));
end


function F=frame(ftype,varargin);
%FRAME  Construct a new frame
%   Usage: F=frame(ftype,...);
%
%   `F=frame(ftype,...)` constructs a new frame object *F* of type
%   *ftype*. Arguments following *ftype* are specific to the type of frame
%   chosen.
%
%   Time-frequency frames
%   ---------------------
%
%   `frame('dgt',g,a,M)` constructs a Gabor frame with window *g*,
%   time-shift *a* and *M* channels. See the help on |dgt| for more
%   information.
%
%   `frame('dgtreal',g,a,M)` constructs a Gabor frame for real-valued
%   signals with window *g*, time-shift *a* and *M* channels. See the help
%   on |dgtreal| for more information.
%
%   `frame('dwilt',g,M)` constructs a Wilson basis with window *g* and *M*
%   channels. See the help on |dwilt| for more information.
%
%   `frame('wmdct',g,M)` constructs a windowed MDCT basis with window *g*
%   and *M* channels. See the help on |wmdct| for more information.
%
%   `frame('filterbank',g,a,M)` constructs a filterbank with filters *g*,
%   time-shifts of *a* and *M* channels. For the ease of implementation, it
%   is necessary to specify *M*, even though it strictly speaking could be
%   deduced from the size of the windows. See the help on |filterbank| for
%   more information on the parameters. Similarly, you can construct a
%   uniform filterbank by selecting `'ufilterbank'`, a positive-frequency
%   filterbank by selecting `'filterbankreal'` or a uniform
%   positive-frequency filterbank by selecting `'ufilterbankreal'`.
%
%   `frame('nsdgt',g,a,M)` constructs a non-stationary Gabor frame with
%   filters *g*, time-shifts of *a* and *M* channels. See the help on
%   |nsdgt| for more information on the parameters. Similarly, you can
%   construct a uniform NSDGT by selecting `'unsdgt'`, an NSDGT for
%   real-valued signals only by selecting `'nsdgtreal'` or a
%   uniform NSDGT for real-valued signals by selecting `'unsdgtreal'`.
%
%   Wavelet frames
%   --------------
%
%   `frame('fwt', w, J)` constructs a wavelet frame with wavelet definition
%   *w* and *J* number of filterbank iterations. Similarly, a redundant time
%   invariant wavelet representation can be constructed by selecting `'ufwt'`.
%   See the help on |fwt| and |ufwt| for more information.
%
%   `frame('wfbt', wt)` constructs a wavelet filterbank tree defined by
%   the wavelet filterbank tree definition *wt*. Similarly, an undecimated
%   wavelet filterbank tree can be constructed by selecting `'uwfbt'`. See the
%   help on |wfbt| and |uwfbt| for more information.
%
%   `frame('wpfbt', wt)` constructs a wavelet packet filterbank tree
%   defined by the wavelet filterbank tree definition *wt*. Similarly, an
%   undecimated wavelet packet filterbank tree can be constructed by selecting
%   `'uwpfbt'`. See the help on |wpfbt| and |uwpfbt| for more information.
%
%   Pure frequency frames
%   ---------------------
%
%   `frame('dft')` constructs a basis where the analysis operator is the
%   |dft|, and the synthesis operator is its inverse, |idft|. Completely
%   similar to this, you can enter the name of any of the cosine or sine
%   transforms |dcti|, |dctii|, |dctiii|, |dctiv|, |dsti|, |dstii|,
%   |dstiii| or |dstiv|.
%
%   `frame('reddft',red)` constructs so called harmonic Parseval tight
%   frame or redundant dft with redundancy `red`. The frame accepts any
%   `red`, but |frana| will only work for signal lengths `Ls` for which
%   the number of coefficients `Ls*red` is an integer.
%
%   `frame('dftreal')` constructs a normalized |fftreal| basis for
%   real-valued signals of even length only. The basis is normalized
%   to ensure that is it orthonormal.
%
%   Special / general frames
%   ------------------------
%
%   `frame('gen',g)` constructs a general frame with a synthesis matrix *g*.
%   The frame atoms must be stored as column vectors in the matrix.
%
%   `frame('identity')` constructs the canonical orthonormal basis, meaning
%   that all operators return their input as output, so it is the dummy
%   operation.
%
%   Container frames
%   ----------------
%
%   `frame('fusion',w,F1,F2,...)` constructs a fusion frame, which is
%   the collection of the frames specified by *F1*, *F2*,... The vector
%   *w* contains a weight for each frame. If *w* is a scalar, this weight
%   will be applied to all the sub-frames.
%
%   `frame('tensor',F1,F2,...)` constructs a tensor product frame, where the
%   frames *F1, *F2*,... are applied along the 1st, 2nd etc. dimensions. If
%   you don't want any action along a specific dimension, use the `identity`
%   frame along that dimension. Any remaining dimensions in the input
%   signal are left alone.
%
%   Wrapper frames
%   --------------
%
%   Frames types in this section are "virtual". They serve as a wrapper for
%   a different type of frame.
%
%   `frame('erbletfb',fs,Ls,...)` constructs an Erb-let filterbank frame for
%   a given samp. frequency *fs* working with signals of length *Ls*. See
%   |erbfilters| for a description of additional parameters as all
%   parameters other than the frame type string 'erbletfb' are passed to it.
%   NOTE: The resulting frame is defined only for a single signal length
%   *Ls*. Shorter signals will be zero-padded, signals longer than *Ls*
%   cannot be processed.
%   The actual frame type is 'filterbank' or 'filterbankreal'.
%
%   `frame('cqtfb',fs,fmin,fmax,bins,Ls,...)` constructs a CQT filterbank
%   frame for a given samp. frequency *fs* working with signals of length
%   *Ls*. See |cqtfilters| for a description of other parameters.
%   NOTE: The resulting frame is defined only for a single signal length
%   *Ls*. Shorter signals will be zero-padded, signals longer than *Ls*
%   cannot be processed.
%   The actual frame type is 'filterbank' or 'filterbankreal'.
%
%   Examples
%   --------
%
%   The following example creates a Modified Discrete Cosine Transform frame,
%   analyses an input signal and plots the frame coefficients:::
%
%      F=frame('wmdct','gauss',40);
%      c=frana(F,greasy);
%      plotframe(F,c,'dynrange',60);
%
%   See also: frana, frsyn, plotframe

complainif_notenoughargs(nargin,1,'FRAME');

if ~ischar(ftype)
  error(['%s: First argument must be a string denoting the type of ' ...
         'frame.'],upper(mfilename));
end;

ftype=lower(ftype);

% True if the frame only works with real-valued input.
F.realinput=0;

% True if the frame only works with a fixed length.
F.fixedlength = 0;

% Handle the windowed transforms
switch(ftype)
 case {'dgt','dwilt','wmdct','filterbank','ufilterbank',...
       'nsdgt','unsdgt','wfbt','uwfbt','wpfbt'}
  F.g=varargin{1};

 case {'dgtreal','filterbankreal','ufilterbankreal',...
      'nsdgtreal','unsdgtreal'}
  F.g=varargin{1};
  F.realinput=1;

 case {'fwt','ufwt'}
  F.g=varargin{1};
  F.J=varargin{2};
  complainif_notposint(F.J,'J','FRAME');
end;

% Input param checking
switch(ftype)
  case 'fusion'
     wtmp = varargin{1};
     % Check w
     if ~isnumeric(varargin{1}) || ...
        ~(isscalar(wtmp) || numel(wtmp) == numel(varargin) -1)
       error('%s: Weights are not in a correct format.',upper(mfilename));
     end

     % Check frame objects
     for ii=2:numel(varargin)
        complainif_notvalidframeobj(varargin{ii},'FRAME');
     end
  case 'tensor'
     % Check frame objects
     for ii=1:numel(varargin)
        complainif_notvalidframeobj(varargin{ii},'FRAME');
     end
end

% For parsing optional parameters to the transforms.
vargs={};
definput=struct();

%% ---- Pre-optional parameters
% Common operations to deal with the input parameters.
switch(ftype)
  case {'dgt','dgtreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    vargs=varargin(4:end);
    definput.keyvals.lt=[0 1];
    definput.flags.phase={'freqinv','timeinv'};

  case {'dwilt','wmdct'}
    F.M=varargin{2};
  case {'filterbank','ufilterbank','filterbankreal','ufilterbankreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    [F.a,~]=comp_filterbank_a(F.a,F.M,struct());

  case {'nsdgt','unsdgt','nsdgtreal','unsdgtreal'}
    F.a=varargin{2};
    F.M=varargin{3};

    % Sanitize 'a' and 'M'. Make M a column vector of length N,
    % where N is determined from the length of 'a'
    F.a=F.a(:);
    F.N=numel(F.a);
    F.M=bsxfun(@times,F.M(:),ones(F.N,1));
  case {'ufwt'}
    vargs=varargin(3:end);
    definput.flags.scaling={'sqrt','noscale','scale'};
  case {'uwfbt'}
    vargs=varargin(2:end);
    definput.flags.scaling={'sqrt','noscale','scale'};
  case {'wpfbt'}
    vargs=varargin(2:end);
    definput.flags.interscaling={'intsqrt','intnoscale','intscale'};
  case {'uwpfbt'}
    vargs=varargin(2:end);
    definput.flags.interscaling={'intsqrt','intnoscale','intscale'};
    definput.flags.scaling={'sqrt','noscale','scale'};
end;

[F.flags,F.kv]=ltfatarghelper({},definput,vargs);

F.type=ftype;
F.origargs=varargin;
F.vargs=vargs;

%% ------ Post optional parameters

% Default value, works for all bases
F.red=1;

% Default value, frame works for all lengths
F.length=@(Ls) Ls;

switch(ftype)
  case 'gen'
    F.g=varargin{1};
    F.frana=@(insig) F.g'*insig;
    F.frsyn=@(insig) F.g*insig;
    F.length = @(Ls) size(F.g,1);
    F.red = size(F.g,2)/size(F.g,1);

  case 'identity'
    F.frana=@(insig) insig;
    F.frsyn=@(insig) insig;

  case 'dft'
    F.frana=@(insig) dft(insig,[],1);
    F.frsyn=@(insig) idft(insig,[],1);

  case 'reddft'
    F.red = 1;
    if nargin > 1, F.red = varargin{1}; end
    if ~isscalar(F.red) || F.red < 1
        error('%s: Redundancy must be greater or equal to 1.',upper(mfilename));
    end
    F.frana     = @(insig) dft(insig,F.red*size(insig,1),1);
    F.frsyn     = @(insig) postpad(idft(insig,[],1),size(insig,1)/F.red);
    F.lengthcoef= @(Ncoef) Ncoef/F.red;
    F.clength   = @(L) L*F.red;

  case 'dftreal'
    F.frana=@(insig) fftreal(insig,[],1)/sqrt(size(insig,1));
    F.frsyn=@(insig) ifftreal(insig,(size(insig,1)-1)*2,1)*sqrt((size(insig,1)-1)*2);
    F.length=@(Ls) ceil(Ls/2)*2;
    F.lengthcoef=@(Ncoef) (Ncoef-1)*2;
    F.realinput=1;
    F.clength = @(L) floor(L/2)+1;

  case 'dcti'
    F.frana=@(insig) dcti(insig,[],1);
    F.frsyn=@(insig) dcti(insig,[],1);

  case 'dctii'
    F.frana=@(insig) dctii(insig,[],1);
    F.frsyn=@(insig) dctiii(insig,[],1);

  case 'dctiii'
    F.frana=@(insig) dctiii(insig,[],1);
    F.frsyn=@(insig) dctii(insig,[],1);

  case 'dctiv'
    F.frana=@(insig) dctiv(insig,[],1);
    F.frsyn=@(insig) dctiv(insig,[],1);

  case 'dsti'
    F.frana=@(insig) dsti(insig,[],1);
    F.frsyn=@(insig) dsti(insig,[],1);

  case 'dstii'
    F.frana=@(insig) dstii(insig,[],1);
    F.frsyn=@(insig) dstiii(insig,[],1);

  case 'dstiii'
    F.frana=@(insig) dstiii(insig,[],1);
    F.frsyn=@(insig) dstii(insig,[],1);

  case 'dstiv'
    F.frana=@(insig) dstiv(insig,[],1);
    F.frsyn=@(insig) dstiv(insig,[],1);

  case 'dgt'
    F.coef2native=@(coef,s) reshape(coef,[F.M,s(1)/F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dgt(insig,F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv,0,0));
    F.frsyn=@(insig) comp_idgt(F.coef2native(insig,size(insig)),F.g,F.a,F.kv.lt,F.flags.do_timeinv,0);
    F.length=@(Ls) dgtlength(Ls,F.a,F.M,F.kv.lt);
    F.red=F.M/F.a;

  case 'dgtreal'
    F.coef2native=@(coef,s) reshape(coef,[floor(F.M/2)+1,s(1)/(floor(F.M/ ...
                                                      2)+1),s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dgtreal(insig,F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv));
    F.frsyn=@(insig) comp_idgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M,F.kv.lt,F.flags.do_timeinv);
    F.length=@(Ls) dgtlength(Ls,F.a,F.M,F.kv.lt);
    F.red=F.M/F.a;
    F.lengthcoef=@(Ncoef) Ncoef/(floor(F.M/2)+1)*F.a;
    F.clength = @(L) L/F.a*(floor(F.M/2)+1);

  case 'dwilt'
    F.coef2native=@(coef,s) reshape(coef,[2*F.M,s(1)/F.M/2,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dwilt(insig,F.g,F.M));
    F.frsyn=@(insig) comp_idwilt(F.coef2native(insig,size(insig)),F.g);
    F.length=@(Ls) dwiltlength(Ls,F.M);

  case 'wmdct'
    F.coef2native=@(coef,s) reshape(coef,[F.M,s(1)/F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_dwiltiii(insig,F.g,F.M));
    F.frsyn=@(insig) comp_idwiltiii(F.coef2native(insig,size(insig)),F.g);
    F.length=@(Ls) dwiltlength(Ls,F.M);

  case 'filterbank'
    F.red=sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) Ncoef/F.red;
    F.native2coef=@(coef) cell2mat(coef(:));
    F.coef2native=@(coef,s) vect2cell(coef,round(s(1)/F.red*F.a(:,2)./F.a(:,1)));
    F.frana=@(insig) F.native2coef(comp_filterbank(insig,F.g,F.a));
    F.frsyn=@(insig) comp_ifilterbank(F.coef2native(insig,size(insig)),...
                                      F.g,F.a,round(size(insig,1)/F.red));
    F.destructor=@() clear('comp_filterbank','comp_ifilterbank');

  case 'filterbankreal'
    F.red=2*sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) 2*Ncoef/(F.red);
    F.native2coef=@(coef) cell2mat(coef(:));
    F.coef2native=@(coef,s) vect2cell(coef,round(2*s(1)/F.red*F.a(:,2)./F.a(:,1)));
    F.frana=@(insig) F.native2coef(comp_filterbank(insig,F.g,F.a));
    F.frsyn=@(insig) 2*real(comp_ifilterbank(F.coef2native(insig,size(insig)),F.g,F.a,...
                                             round(2*size(insig,1)/F.red)));
    F.destructor=@() clear('comp_filterbank','comp_ifilterbank');

  case 'ufilterbank'
    F.red=sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) round(Ncoef/F.red);
    F.coef2native=@(coef,s) reshape(coef,[s(1)/F.M,F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(ufilterbank(insig,F.g,F.a));
    F.frsyn=@(insig) ifilterbank(F.coef2native(insig,size(insig)),F.g,F.a);

  case 'ufilterbankreal'
    F.red=2*sum(F.a(:,2)./F.a(:,1));
    F.length=@(Ls) filterbanklength(Ls,F.a);
    F.lengthcoef=@(Ncoef) round(Ncoef/F.red*2);
    F.coef2native=@(coef,s) reshape(coef,[s(1)/F.M,F.M,s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(ufilterbank(insig,F.g,F.a));
    F.frsyn=@(insig) 2*real(ifilterbank(F.coef2native(insig,size(insig)),F.g, ...
                                        F.a));

  case 'nsdgt'
    F.coef2native=@(coef,s) mat2cell(coef,F.M,s(2));
    F.native2coef=@(coef) cell2mat(coef(:));
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.frana=@(insig) F.native2coef(nsdgt(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgt(F.coef2native(insig,size(insig)),F.g,F.a);

  case 'unsdgt'
    F.coef2native=@(coef,s) reshape(coef,[F.M(1),s(1)/F.M(1),s(2)]);
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(unsdgt(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgt(F.coef2native(insig,size(insig)),F.g,F.a);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);

  case 'nsdgtreal'
    F.coef2native=@(coef,s) mat2cell(coef,floor(F.M/2)+1,s(2));
    F.native2coef=@(coef) cell2mat(coef(:));
    F.frana=@(insig) F.native2coef(nsdgtreal(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.clength=@(L) sum(floor(F.M/2)+1);

  case 'unsdgtreal'
    F.coef2native=@(coef,s) reshape(coef,floor(F.M(1)/2)+1,s(1)/ ...
                                    (floor(F.M(1)/2)+1),s(2));
    F.native2coef=@(coef)   reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(unsdgtreal(insig,F.g,F.a,F.M));
    F.frsyn=@(insig) insdgtreal(F.coef2native(insig,size(insig)),F.g,F.a,F.M);
    F.length=@(Ncoef) sum(F.a);
    F.lengthcoef=@(Ncoef) sum(F.a);
    F.red=sum(F.M)/sum(F.a);
    F.clength=@(L) numel(F.M)*(floor(F.M(1)/2)+1);

  case 'fusion'
    F.w=varargin{1};
    F.frames=varargin(2:end);

    if any(cellfun(@(fEl) fEl.realinput,F.frames))
        error(['%s: Real-valued-input-only frames are not currently ',...
               'supported in the fusion frame.'],upper(mfilename));
    end

    F.Nframes=numel(F.frames);
    F.w=bsxfun(@times,F.w(:),ones(F.Nframes,1));
    F.length = @(Ls) comp_framelength_fusion(F,Ls);
    F.red=sum(cellfun(@framered,F.frames));

    % These definitions binds F itself, so they must execute last
    F.frana=@(insig) comp_frana_fusion(F,insig);
    F.frsyn=@(insig) comp_frsyn_fusion(F,insig);

  case 'tensor'
    % This frame type is currently broken. It must be reworked to reshape
    % to the standard layout in order not to break all the assumptions.
    F.frames=varargin;
    F.Nframes=numel(F.frames);
    for ii=1:F.Nframes
        if F.frames{ii}.realinput
            error(['It is not safe to embed a real-valued-input-only frame ' ...
                   'into the tensor frame.']);
        end;
    end;

    F.frana=@(insig) comp_frana_tensor(F,insig);
    F.frsyn=@(insig) comp_frsyn_tensor(F,insig);

    F.length=@(Ls) comp_framelength_tensor(F,Ls);

    F.red=prod(cellfun(@framered,F.frames));

  case {'fwt','dwt'}
    % We have to initialize F.g here already
    [F.g, F.info]=fwtinit({'strict',F.g});
    F.red= 1/(F.g.a(1)^(F.J)) + sum(1./(F.g.a(1).^(0:F.J-1))*sum(1./F.g.a(2:end)));
    F.frana=@(insig) wavcell2pack(comp_fwt(insig,F.g.h,F.g.a,F.J,'per'));
    F.frsyn=@(insig) comp_ifwt(...
                        wavpack2cell(insig,fwtclength(size(insig,1)/F.red,F.g,F.J)),...
                               F.g.g,F.g.a,F.J,size(insig,1)/F.red,'per');
    F.length=@(Ls) fwtlength(Ls,F.g,F.J);
  case {'wfbt'}
    [F.g,F.info]=wfbtinit({'strict',F.g});
    F.red = sum(1./treeSub(F.g));
    % comp_ specific
    [F.wtPath, F.rangeLoc, F.rangeOut] = treeBFranges(F.g);

    F.coef2native = @(coef,s) wavpack2cell(coef,wfbtclength(s(1)/F.red,F.g));
    F.native2coef = @(coef) wavcell2pack(coef);
    F.frana=@(insig) F.native2coef(comp_wfbt(insig,F.g.nodes(F.wtPath),...
                                             F.rangeLoc,F.rangeOut,'per'));
    F.frsyn=@(insig) comp_iwfbt(F.coef2native(insig,size(insig)),...
                                F.g.nodes(F.wtPath(end:-1:1)),...
                                [nodesInLen(F.wtPath(end:-1:1),size(insig,1)/F.red,1,F.g);size(insig,1)/F.red],...
                                F.rangeLoc(end:-1:1),F.rangeOut(end:-1:1),...
                                'per');
    F.length=@(Ls) wfbtlength(Ls,F.g);
  case {'wpfbt'}
    F.g=wfbtinit({'strict',F.g});
    F.red = sum(cellfun(@(aEl) sum(1./aEl),nodesSub(nodeBForder(0,F.g),F.g)));
    % comp_ specific
    F.wtPath = nodeBForder(0,F.g);
    F.rangeLoc = nodesLocOutRange(F.wtPath,F.g);
    [F.pOutIdxs,F.chOutIdxs] = treeWpBFrange(F.g);

    F.coef2native = @(coef,s) wavpack2cell(coef,...
                    s(1)./cell2mat(cellfun(@(aEl) aEl(:),...
                    reshape(nodesSub(nodeBForder(0,F.g),F.g),[],1),...
                    'UniformOutput',0))./F.red);
    F.native2coef = @(coef) wavcell2pack(coef);

    F.frana=@(insig) F.native2coef(...
                        comp_wpfbt(insig,F.g.nodes(F.wtPath),...
                                   F.rangeLoc,'per',F.flags.interscaling));
    F.frsyn=@(insig) comp_iwpfbt(F.coef2native(insig,size(insig)),...
                                 F.g.nodes(F.wtPath(end:-1:1)),...
                                 F.pOutIdxs,F.chOutIdxs,...
                                 size(insig,1)/F.red,...
                                 'per',F.flags.interscaling);
    F.length=@(Ls) wfbtlength(Ls,F.g);
  case {'ufwt'}
    F.g=fwtinit({'strict',F.g});
    F.coef2native = @(coef,s) reshape(coef,[s(1)/(F.J*(numel(F.g.a)-1)+1),F.J*(numel(F.g.a)-1)+1,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(comp_ufwt(insig,F.g.h,F.g.a,F.J,F.flags.scaling));
    F.frsyn=@(insig) comp_iufwt(F.coef2native(insig,size(insig)),F.g.g,F.g.a,F.J,F.flags.scaling);
    F.length=@(Ls) Ls;
    F.red=(F.J*(numel(F.g.a)-1)+1);
  case {'uwfbt'}
    F.g=wfbtinit({'strict',F.g});

    % comp_ specific
    [F.wtPath, F.rangeLoc, F.rangeOut] = treeBFranges(F.g);
    F.nodesUps = nodesFiltUps(F.wtPath,F.g);
    F.red = sum(cellfun(@numel,F.rangeOut));

    F.coef2native = @(coef,s) reshape(coef,[s(1)/F.red,F.red,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(...
                        comp_uwfbt(insig,F.g.nodes(F.wtPath),F.nodesUps,...
                                   F.rangeLoc,F.rangeOut,F.flags.scaling));
    F.frsyn=@(insig) comp_iuwfbt(F.coef2native(insig,size(insig)),...
                                 F.g.nodes(F.wtPath(end:-1:1)),...
                                 F.nodesUps(end:-1:1),F.rangeLoc(end:-1:1),...
                                 F.rangeOut(end:-1:1),F.flags.scaling);
    F.length=@(Ls) Ls;
  case {'uwpfbt'}
    F.g= wfbtinit({'strict',varargin{1}});
    F.red = sum(cellfun(@(fEl) numel(fEl.g),F.g.nodes));
    % comp_ specific
    F.wtPath = nodeBForder(0,F.g);
    F.nodesUps = nodesFiltUps(F.wtPath,F.g);
    F.rangeLoc = nodesLocOutRange(F.wtPath,F.g);
    [F.pOutIdxs,F.chOutIdxs] = treeWpBFrange(F.g);

    F.coef2native = @(coef,s) reshape(coef,[s(1)/F.red,F.red,s(2)]);
    F.native2coef = @(coef) reshape(coef,[size(coef,1)*size(coef,2),size(coef,3)]);
    F.frana=@(insig) F.native2coef(...
                        comp_uwpfbt(insig,F.g.nodes(F.wtPath),F.rangeLoc,...
                                    F.nodesUps,F.flags.scaling,...
                                    F.flags.interscaling));
    F.frsyn=@(insig) comp_iuwpfbt(F.coef2native(insig,size(insig)),...
                                  F.g.nodes(F.wtPath(end:-1:1)),...
                                  F.nodesUps(end:-1:1),F.pOutIdxs,F.chOutIdxs,...
                                  F.flags.scaling,F.flags.interscaling);
    F.length=@(Ls) Ls;

  %%%%%%%%%%%%%%%%%%%%
  %% WRAPPER FRAMES %%
  %%%%%%%%%%%%%%%%%%%%
  case {'erbletfb','cqtfb'}
    switch(ftype)
        case 'erbletfb'
            [g,a,~,L] = erbfilters(varargin{:});
        case 'cqtfb'
            [g,a,~,L] = cqtfilters(varargin{:});
    end
    % Search for the 'complex' flag
    do_complex = ~isempty(varargin(strcmp('complex',varargin)));
    if do_complex
       F = frameaccel(frame('filterbank',g,a,numel(g)),L);
    else
       F = frameaccel(frame('filterbankreal',g,a,numel(g)),L);
    end
    F.fixedlength = 1;

  otherwise
    error('%s: Unknown frame type: %s',upper(mfilename),ftype);

end;

% This one is placed at the end, to allow for F.red to be defined
% first.
if ~isfield(F,'lengthcoef')
    F.lengthcoef=@(Ncoef) Ncoef/framered(F);
end;


function [v1,v2,v3,v4,v5,v6,v7]=lsqnonlinSym(funhandle,varargin)
% LSQNONLINSYM   A wrap of lsqnonlin to use symbolic toolbox to obtain Jacobian.
% Limitation: The cost function and nonlinear constraint function cannot
% have conditional operation, such as if, max, min etc.
% Its usage is the same as the original lsqnonlin.
%
% Example: see lsqnonlinSym_example
%
% By Yi Cao, Cranfield University, 25/09/2007
%
x0=varargin{1};
n=numel(x0);

x=[];
for k=1:n
    eval(['syms x' num2str(k)]);
    eval(['x=[x;x' num2str(k) '];']);
end

nv=numel(varargin);
if nv>4
    F=feval(funhandle,x,varargin(5:end));
else
    F=feval(funhandle,x);
end
F=simplify(F);
Fx=simplify(jacobian(F,x));

if nv>=4
    opt = varargin{4};
    opt = optimset(opt,'jacobian','on');
else
    opt = optimset('jacobian','on');
end

switch numel(varargin)
    case 1
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,[],[],opt,F,Fx);
    case 2
        LB=varargin{2};
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,LB,[],opt,F,Fx);
    otherwise
        LB=varargin{2};
        UB=varargin{3};
        [v1,v2,v3,v4,v5,v6,v7]=lsqnonlin(@symFun,x0,LB,UB,opt,F,Fx);
end

function [E,J]=symFun(x0,F,Fx)
n=numel(x0);
for k=1:n
    eval(['x' num2str(k) '=x0(k);']);
end
E = eval(F);
if nargout>1
    J  = eval(Fx);
end


function unit = seconds2unit(seconds,seconds0,seconds1)
% Convert seconds past midnight to unit times
%
% USAGE:
%   [UNIT] = seconds2unit(SECONDS,SECONDS0,SECONDS1)
%
% INPUTS:
%   SECONDS - m by 1 column vector of times expressed as seconds past midnight
%               (e.g. 1:00:00 is 3600, 12:00:15 is 43215) where m>=2
%   SECONDS0   - Base time, maps to 0 un the unit interval
%   SECONDS1   - End time, maps to 1 un the unit interval
%
% OUTPUTS:
%   UNIT    - m by 1 column vector of times measured as fraction of time
%               between min(SECONDS) and max(SECONDS)
%
% COMMENTS:
%   This is a helper function for REALIZED_KERNEL
%
%  See also WALL2UNIT, WALL2SECONDS, SECONDS2WALL, REALIZED_KERNEL, REALIZED_VARIANCE

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 5/1/2008

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin~=3
    error('Three inputs required.')
end
if any(seconds<0)
    error('SECONDS must be non-negative');
end
if seconds0<0
    error('SECONDS0 must be non-negative');
end
if seconds1<0
    error('SECONDS1 must be non-negative');
end

if (seconds1-seconds0)<eps
    error('SECONDS1 must be larger than SECONDS0.');
end

if size(seconds,2)>size(seconds,1)
    seconds=seconds';
end

seconds = double(seconds);
seconds0 = double(seconds0);
seconds1 = double(seconds1);

if size(seconds,2)>1
    error('SECONDS must be an m by 1 column vector');
end
if length(seconds)<2
    error('SECONDS must contain at least 2 elements');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
unit=(seconds-seconds0)/(seconds1 - seconds0);


function [ ishow, list_num, list, nregion ] = voxels_region_3d ( maxlist, ...
  nx, ny, nz, ishow )

%*****************************************************************************80
%
%% VOXELS_REGION_3D arranges a set of voxels into contiguous regions in 3D.
%
%  Discussion:
%
%    On input, the ISHOW array contains zero and nonzero values.  The nonzero
%    values are taken to be active voxels.  On output, the zero voxels remain
%    zero, and all the active voxels have been assigned a value which now
%    indicates membership in a region, or group of contiguous voxels.
%
%    On output, the array LIST contains information about the regions.
%    The last used element of LIST is LIST_NUM.
%
%    The number of elements in region NREGION is NELEM = LIST(LIST_NUM).
%    The (I,J,K) indices of the last element in this region are in
%    LIST(LIST_NUM-3) through LIST(LIST_NUM-1), and the first element is
%    listed in LIST(LIST_NUM-3*NELEM), LIST(LIST_NUM-3*NELEM+1),
%    LIST(LIST_NUM-3*NELEM+2).
%
%    The number of elements in NREGION-1 is listed in LIST(LIST_NUM-3*NELEM-1),
%    and the (I,J,K) indices of the these elements are listed there.
%
%  Picture:
%
%    Input:
%
%      0  2  0  0 17  0  3
%      0  0  3  0  1  0  4
%      1  0  4  8  8  0  7
%      3  0  6 45  0  0  0
%      3 17  0  5  9  2  5
%
%    Output:
%
%      0  1  0  0  2  0  3
%      0  0  2  0  2  0  3
%      4  0  2  2  2  0  3
%      4  0  2  2  0  0  0
%      4  4  0  2  2  2  2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer MAXLIST, the maximum length of the array used to
%    list the elements of the regions.
%
%    Input, integer NX, NY, NZ, the number of voxels in the X, Y and
%    Z directions.
%
%    Input, integer ISHOW(NX,NY,NZ), the only significance to
%    the entries is whether they are zero or nonzero.
%
%    Output, integer ISHOW(NX,NY,NZ), the nonzero entries have now been revalued
%    so that contiguous entries have the same value, indicating a grouping into
%    a region.
%
%    Output, integer LIST_NUM, the number of entries of LIST that were used.
%    However, if MAXLIST < LIST_NUM, then there was not enough space in
%    LIST to store the data properly, and LIST should not be used,
%    although the data in ISHOW should be correct.
%
%    Output, integer LIST(MAXLIST), contains, in stack form, a list
%    of the indices of the elements in each region.
%
%    Output, integer NREGION, the number of regions discovered.
%

%
%  Reset all nonzero entries of ISHOW to -1.
%
  for i = 1 : nx
    for j = 1 : ny
      for k = 1 : nz
        if ( ishow(i,j,k) ~= 0 )
          ishow(i,j,k) = -1;
        end
      end
    end
  end
%
%  Start the number of items in the region list at 0.
%
  list_num = 0;
%
%  Start the number of regions at 0.
%
  nregion = 0;
%
%  The stack begins empty.
%
  nstack = 0;
%
%  Search for an unused "ON" voxel from which we can "grow" a new region.
%
  for i = 1 : nx
    for j = 1 : ny
      for k = 1 : nz
%
%  We found a voxel that is "ON", and does not belong to any region.
%
        if ( ishow(i,j,k) == -1 )
%
%  Increase the number of regions.
%
          nregion = nregion + 1;
%
%  Add this voxel to the region.
%
          ishow(i,j,k) = nregion;
%
%  Add this voxel to the stack.
%
          stack(nstack+1) = i;
          stack(nstack+2) = j;
          stack(nstack+3) = k;

          stack(nstack+4) = 1;

          nstack = nstack + 4;
%
%  Add this voxel to the description of the region.
%
          nelements = 1;

          if ( list_num + 3 <= maxlist )
            list(list_num+1) = i;
            list(list_num+2) = j;
            list(list_num+3) = k;
          end

          list_num = list_num + 3;

          while ( 1 )
%
%  Find all neighbors of BASE that are "ON" but unused.
%  Mark them as belonging to this region, and stack their indices.
%
            ibase = stack(nstack-3);
            jbase = stack(nstack-2);
            kbase = stack(nstack-1);

            ilo = max ( ibase-1, 1 );
            ihi = min ( ibase+1, nx );
            jlo = max ( jbase-1, 1 );
            jhi = min ( jbase+1, ny );
            klo = max ( kbase-1, 1 );
            khi = min ( kbase+1, nz );

            nabes = 0;

            for i2 = ilo : ihi
              for j2 = jlo : jhi
                for k2 = klo : khi
%
%  We found a neighbor to our current search point, which is "ON" and unused.
%
                  if ( ishow(i2,j2,k2) == -1 )
%
%  Increase the number of neighbors.
%
                    nabes = nabes + 1;
%
%  Mark the neighbor as belonging to the region.
%
                    ishow(i2,j2,k2) = nregion;
%
%  Add the neighbor to the stack.
%
                    stack(nstack+1) = i2;
                    stack(nstack+2) = j2;
                    stack(nstack+3) = k2;

                    nstack = nstack + 3;
%
%  Add the neighbor to the description of the region.
%
                    nelements = nelements + 1;

                    if ( list_num + 3 <= maxlist )
                      list(list_num+1) = i2;
                      list(list_num+2) = j2;
                      list(list_num+3) = k2;
                    end

                    list_num = list_num + 3;

                  end
                end
              end
            end
%
%  If any new neighbors were found, take the last one as the basis
%  for a deeper search.
%
            if ( 0 < nabes )
              stack(nstack+1) = nabes;
              nstack = nstack + 1;
              continue
            end
%
%  If the current search point had no new neighbors, drop it from the stack.
%
            ncan = stack(nstack) - 1;
            nstack = nstack - 3;
            stack(nstack) = ncan;
%
%  If there are still any unused candidates at this level, take the
%  last one as the basis for a deeper search.
%
            if ( 0 < stack(nstack) )
              continue
            end
%
%  If there are no more unused candidates at this level, then we need
%  to back up a level in the stack.  If there are any candidates at
%  that earlier level, then we can still do more searching.
%
            nstack = nstack - 1;

            if ( nstack <= 0 )
              break
            end

          end
%
%  If we have exhausted the stack, we have completed this region.
%  Tag the number of elements to the end of the region description list.
%
          list_num = list_num + 1;
          if ( list_num <= maxlist )
            list(list_num) = nelements;
          end

        end

      end
    end
  end
%
%  Print some warnings.
%
  if ( maxlist < list_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'VOXELS_REGION - Warning!\n' );
    fprintf ( 1, '  MAXLIST was too small to list the regions.\n' );
    fprintf ( 1, '  Do not try to use the LIST array!\n' );
    fprintf ( 1, '  The ISHOW data is OK, however.\n' );
  end

  return
end


function [ X ] = special_add_left( X, S )
% add on Lie group-Lie Algebra space
    s_theta=S(1:3);
    s_p=S(4:6);
    sizeS=size(S,1);
    NumberOfLandmarks=(sizeS-6)/3;
    X.position=X.position+X.orientation*jaco_r(-s_theta)*s_p;
    if NumberOfLandmarks>=1
        s_landmarksMatrix=reshape(S(7:end),3,NumberOfLandmarks);
        X.landmarks(1:3,:)=X.landmarks(1:3,:)+X.orientation*jaco_r(-s_theta)*s_landmarksMatrix;
    end
    X.orientation=X.orientation*so3_exp(s_theta);
end


function [dLdp,g,x] = spm_mci_adjoint (Pr,M,U,Y)
% Gradient of log joint from adjoint method
% FORMAT [dLdp,g,x] = spm_mci_adjoint (Pr,M,U,Y)
%
% Pr        Parameters (vectorised and in M.V subspace)
% M         Model structure
% U         Inputs  [Nin x N]
% Y         Data
%
% dLdp      Gradient    [Np x 1]
% g         Outputs     [N x Nout]
% x         States      [N x Nstates]
%
% If M.adjlike=1 this function returns gradient of log likelihood
%
% This function uses integrators from MATLAB's ODE Suite
%
% B. Sengupta, K. Friston and W. Penny (2014) Efficient Gradient
% Computation for Dynamical Models. Neuroimage,98, 521-527.
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: spm_mci_adjoint.m 6697 2016-01-27 14:57:28Z spm $

try, adjlike=M.adjlike; catch, adjlike=0; end

% Parameters in original space
P = M.V*Pr+M.vpE;

if isempty(U)
    U=zeros(1,M.N);
end

[g,x] = spm_mci_fwd(P,M,U);

% Gradient at computed times
% When computing output sensitivities, assume dydx=L, ie not a
% function of x. Generalise later
[tmp,L]=feval(M.g,M.x0,U(:,1),P,M);
e=Y-g;
djdx=e*M.iCe*L;

% integrate adjoint equation
lambda = spm_mci_adjoint_int(U,P,M,x,djdx);

% If observation function becomes dependent on parameters
% djdp will need updating
djdp = zeros(1,M.Np);

dLdp=zeros(1,M.Np);
for n=1:M.N,
    % Evaluate parameter Jacobian
    if isfield(M,'dfdp')
        Fp = feval(M.dfdp,x(n,:)',U(:,n),P,M);
    else
        Fp = spm_diff(M.f,x(n,:)',U(:,n),P,M,3);
    end
    dLdp=dLdp+djdp-lambda(n,:)*Fp;
end

if ~adjlike
    dlogpriordp = spm_mci_gprior_deriv (Pr,M);
    dLdp=dLdp+dlogpriordp;
end


function Z = spinv(A, B)
% SPINV    Evaluate the sparse inverse matrix
%
% z = sinv(A)  returns the elements of inv(A)_ij, for which A_ij
%      is different from zero.
%
% z = sinv(LD, 1)  returns the elements of inv(A)_ij, for which A_ij
%     is different from zero, and where LD is the LDL cholesky
%     decomposition of A. LD has to be in the form returned by ldlchol
%     in SuiteSparse by Tim Davis.
%
%   Note! If z = sinv(LD, 1) is used LD must not be modified in Matlab
%   after ldlchol. Matlab destroys the symbolic sparsity structure in
%   the Cholesky decomposition, which is needed in the spinv
%   algorithm. If LD is modified the worst scenario is memory corruption.
%
%
%     For details, see:
%     Jarno Vanhatalo and Aki Vehtari (2008). Modelling local and
%     global phenomena with sparse Gaussian processes. Proceedings of
%     the 24th Conference on Uncertainty in Artificial Intelligence
%

% Copyright (c) 2008-2010      Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 2 or later); please refer to the file
% License.txt, included with the software, for details.

warning('Using M-file. Compile MEX-file!');

    n = size(A,1);

    if nargin == 1
        [LD, p, q] = ldlchol(A);
    else
        LD = A;
    end

    [I,J,ld] = find(LD);
    temp = [I(:) J(:) ; J(:) I(:)];
    temp = sortrows(unique(temp,'rows'),2);
    Iz = temp(:,1); Jz = temp(:,2);

    % Find the column starting points
    a1=zeros(n,1);
    a2 = cumsum(histc(J,1:n));
    a1(1) = 1; a1(2:end) = a2(1:end-1) + 1;
    az1=zeros(n,1);
    az2 = cumsum(histc(Jz,1:n));
    az1(1) = 1; az1(2:end) = az2(1:end-1) + 1;

    for j=1:n
        indaz{j} = az1(j):az2(j);
        indIz{j} = Iz(indaz{j})';
    end

    % Evaluate the sparse inverse
    z = zeros(size(Iz));
    z(end) = 1./ld(end);
    % Allocate memory
    cindit=zeros(n,1);
    for jj = n-1:-1:1
        fil = ld(a1(jj)+1:a1(jj+1)-1);
        fi = I(a1(jj)+1:a1(jj+1)-1);
        lfi = length(fi);
        Zt = zeros(lfi,lfi);
        indz = cumsum(histc(indIz{jj},[0 ; fi]));
        indz = az1(jj) + indz(1:end-1);

        i4=0;
        for i1 = 1:lfi
            cind1=indaz{fi(i1)};
            Icind1=indIz{fi(i1)};
            indfi = lfi;
            i2=length(Icind1);
            go = true;
            while go
                if Icind1(i2)==jj  % Find the indeces for the jj'th rows in fi columns
                    i4=i4+1;
                    cindit(i4)=cind1(i2);
                    go = false;
                end
                if indfi >= 1 && fi(indfi) == Icind1(i2) % Find the indeces for the fi'th rows in i2'nd columns
                    Zt(indfi,i1) = z(cind1(i2));
                    indfi = indfi-1;
                end
                i2 = i2-1;
            end
        end
        % remove extras
        cindi=cindit(1:i4);

        zij = -fil'*Zt;
        z(cindi) = zij;
        z(indz) = zij;
        zij = 1./ld(a1(jj)) - fil'*z(indz);
        z(az1(jj)-1+find(indIz{jj}==jj,1)) = zij;
    end

    Z = sparse(Iz,Jz,z);

    if nargin == 1
        r(q) = 1:n;
        Z = Z(r,r);
    end
end


function F = diff(F, k, dim)
%DIFF   Componentwise partial derivative of a CHEBFUN3V object.
%   DIFF(F) is the derivative of components of F along the first variable.
%
%   DIFF(F, K) is the Kth derivative of each component of F along the first
%   variabel.
%
%   DIFF(F, K, DIM) is the Kth derivative of F along the dimension DIM.
%   DIM = 1 (default) is the derivative in the 1st input variable.
%   DIM = 2 is the derivative in the 2nd variable.
%   DIM = 3 is the derivative in the 3rd varialbe.
%
%   DIFF(F, [K1 K2], [DIM1 DIM2]) means K1-th derivative of F in dimension
%   DIM1 and K2-th derivative in dimension DIM2. DIM1 and DIM2 can be 1, 2
%   or 3 in any order.
%
%   DIFF(F, [K1 K2 K3]) is the K1-th partial derivative of F in the first
%   variable, K2-th partial derivative of F in the second variable and
%   K3-th partial derivative of F in the third variable.
%   For example, DIFF(F, [1 2 3]) is d^6F/(dx d^2y d^3z).

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check:
if ( isempty(F) )
    return
end

% Defaults:
if ( ( nargin == 1 ) || isempty(k) )
    k = 1;
end
if ( nargin < 3 )
    dim = 1;
end

% Diff each component.
for j = 1:F.nComponents
    F.components{j} = diff(F.components{j}, k, dim);
end

end


function [Jul1,Jul2]=GPS2TCG(Jul1,Jul2)
%GPS2TT  Convert from the timescale used by the Global Positioning System
%           (GPS)  given as a two-part Julian date to geocentric coordinate
%           time (TCG), represented as a two-part Julian date.
%
%INPUTS:    Jul1, Jul2  Two parts of a Julian date given in GPS time. The
%                       units of the date are days. The full date is the
%                       sum of both terms. The date is broken into two
%                       parts to provide more bits of precision. It does
%                       not matter how the date is split.
%
%OUTPUTS:   Jul1, Jul2  The time as a Julian date in TCG.
%
%This function just calls GPS2TAU and TAI2TCG.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=GPS2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TCG(Jul1,Jul2);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [] = gsp_plot_filter(G, filters, param)
%GSP_PLOT_FILTER  Plot a system of filters
%   Usage:  [filter_data,test_sum]=gsp_plot_filter(G,filters);
%           [filter_data,test_sum]=gsp_plot_filter(G,filters,param);
%
%   Input parameters:
%       G       : Graph object (Or lmax)
%       filters : Cell array of filters (or single filter)
%       param   : Optional variable containing additional parameters
%   Output parameters:
%       none
%
%
%   'gsp_plot_filters(G, filter, param)' plots a system of graph spectral
%   filters.
%
%   Example:::
%
%         Nf = 4;
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_mexican_hat(G, Nf);
%         gsp_plot_filter(G, g);
%
%
%   Additional parameters
%   ---------------------
%
%   * *param.line_width* : Width of the filter plots (default 4).
%   * *param.npoints* : Number of point where the filters are evaluated
%     (default 1000).
%   * *param.x_tic* : Distance between x-tick labels.
%   * *param.y_tic* : Distance between y-tick labels (default 0.25).
%   * *param.minor_tick* : To show minor tick marks (default 1).
%   * *param.plot_eigenvalues* : To plot black X marks at all eigenvalues
%     of the graph (You need to compute the Fourier basis to use this
%     option). By default the eigenvalues are plot if they are contained in
%     the Graph.
%   * *param.lambda_highlights* : To plot red X marks at highlight
%     eigenvalues (default 0).
%   * *param.x_width* : Width of X marks for the eigenvalues (default 3).
%   * *param.x_size* : Size of X marks for the eigenvalues (default 8).
%   * *param.show_sum* : To plot an extra line showing the sum of the
%     squared magnitudes of the filters (default 1 if there is multiple
%     filters).
%   * *param.colors_rgb* : To specify the line colors.
%   * *param.cla* : Clear axis (default 1).
%   * *param.yrange* : To specify a range for the y axis.
%   * *param.verbose* : Verbosity level (1 display the warning - 0 no log)
%     (default 1).
%
%   Demos: gsp_demo
%

% Author : David I Shuman, Nathanael Perraudin
% Testing: test_filter

% Read input parameters
if nargin < 3
   param = struct;
end

if ~isstruct(G)
   G.lmax = G;
   param.plot_eigenvalues = 0;
end

if ~isfield(G,'lmax')
    G = gsp_estimate_lmax(G);
    warning(['GSP_FILTER_ANALYSIS: The variable lmax is not ',...
        'available. The function will compute it for you. ',...
        'However, if you apply many time this function, you ',...
        'should precompute it using the function: ',...
        'gsp_estimate_lmax']);
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'line_width'), param.line_width = 4; end
if ~isfield(param,'x_width'), param.x_width = 3; end
if ~isfield(param,'x_size'), param.x_size = 8; end
if ~isfield(param,'show_sum'), param.show_sum = length(filters)>1; end
if ~isfield(param,'y_tic'), param.y_tic = 0.25; end
if ~isfield(param,'minor_tick'), param.minor_tick = 1; end
if ~isfield(param,'npoints'), param.npoints = 1000; end
if ~isfield(param,'cla'), param.cla = 1; end
if ~isfield(param,'x_tic')
    param.x_tic=max(1,ceil(G.lmax/10));
end
if ~isfield(param,'plot_eigenvalues')
    param.plot_eigenvalues = isfield(G,'e');
end

if ~isfield(G,'lmax')
    if param.verbose
        fprintf('GSP_KERNEL_MEXICAN_HAT has to compute lmax \n')
    end
    G = gsp_estimate_lmax(G);
end

lambdas = linspace(0,G.lmax,param.npoints);

if param.cla
    cla;
end
hold on;
% apply the filter
fd = gsp_filter_evaluate(filters,lambdas);

% plot the filter
plot(lambdas,fd,'LineWidth',param.line_width);

% plot the eigenvalues
if param.plot_eigenvalues
    if isfield(G,'e')
        plot(G.e,zeros(G.N,1),'xk','LineWidth',...
            param.x_width,'MarkerSize',param.x_size);
    else
        if param.verbose
            warning('GSP_PLOT_FILTER: No eigenvalues found in the graph');
        end
    end
end

% plot hightlights eigenvalues
if isfield(param,'lambda_highlights')
    plot(param.lambda_highlights, ...
        zeros(length(param.lambda_highlights),1),...
        'xr','LineWidth',param.x_width,'MarkerSize',param.x_size);
end

% plot the sum
if param.show_sum
    test_sum=sum(fd.^2,2);
    plot(lambdas,test_sum,'k','LineWidth',param.line_width);
end

box on;
% X axis
xlim(full([0 G.lmax]));
% Y axis
if isfield(param,'yrange')
    yrange=param.yrange;
    ylim(yrange);
    set(gca,'YTick',yrange(1):y_tic:yrange(2));
end

% Add marks
if param.minor_tick
    set(gca,'XMinorTick','on','YMinorTick','on');

end

% Change the color
if isfield(param,'colors_rgb');
    set(gca, 'ColorOrder', param.colors_rgb);
end
set(gca,'XTick',0:param.x_tic:G.lmax);

hold off;
end


% OP_UGRADU_JAC_V: assemble the second term of the Jacobian of the
% convective term (see solve_navier_stokes for usage)
%
%   mat = op_ugradu_jac_v (spu, spv, msh, coeff);
%   [rows, cols, values] = op_ugradu_jac (spu, spv, msh, uder);
%
% INPUT:
%
%  spu:   structure representing the space of trial functions (see sp_scalar/sp_evaluate_col)
%  spv:   structure representing the space of test functions  (see sp_scalar/sp_evaluate_col)
%  msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%  uder:  derivative of the velocity, evaluated in all the degrees of freedom
%
% OUTPUT:
%
%  mat:    assembled jacobian matrix
%  rows:   row indices of the nonzero entries
%  cols:   column indices of the nonzero entries
%  values: values of the nonzero entries
%
% Copyright (C) 2018 Luca Coradello, Luca Pegolotti
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_ugradu_jac_v (spu, spv, msh, uder)

  gradu_shpu = sum(bsxfun( @times, reshape(spu.shape_functions, ...
                   [1 size(spu.shape_functions)]),uder),2);

  shpu = reshape (gradu_shpu, spu.ncomp, msh.nqn, spu.nsh_max, msh.nel);
  shpv = reshape (spv.shape_functions, spv.ncomp, msh.nqn, spv.nsh_max, msh.nel);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:,iel)))
      shpu_iel = reshape (shpu(:, :, 1:spu.nsh(iel), iel), spu.ncomp, msh.nqn, 1, spu.nsh(iel));
      shpv_iel = reshape (shpv(:, :, 1:spv.nsh(iel), iel), spv.ncomp, msh.nqn, spv.nsh(iel), 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1,msh.nqn,1,1]);

      jacdet_shpu = bsxfun (@times, jacdet_iel, shpu_iel);
      tmp1 = sum (bsxfun (@times, jacdet_shpu, shpv_iel), 1);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = reshape (sum (tmp1, 2), spv.nsh(iel), spu.nsh(iel));

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc;
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc;
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);
    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_u_v: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_ugradu_jac_v: wrong number of output arguments')
  end

end


function [map, aps, pr, prs] = compute_map (ranks, gnd, kappas)
% COMPUTE_MAP  This function computes the mAP for a given set of returned results.
%
% Usage:
%   map = compute_map (ranks, gnd)
%         computes mean average precsion (map) only
%
%   [map, aps, pr, prs] = compute_map (ranks, gnd, kappas)
%         computes mean average precision (map), average precision (aps) for each query
%         computes mean precision at kappas (pr), precision at kappas (prs) for each query
%
% Notes:
% 1) ranks starts from 1, size(ranks) = db_size X #queries
% 2) The junk results (e.g., the query itself) should be declared in the gnd stuct array
% 3) If there are no positive images for some query, that query is excluded from the evaluation

  if ~exist('kappas'), kappas = 0; end

  nq = numel (gnd);   % number of queries
  % init map and pr
  map = 0;
  aps = zeros (nq, 1);
  pr = zeros(1, numel(kappas));
  prs = zeros (nq, numel(kappas));
  nempty = 0;

  for i = 1:nq
    qgnd = gnd(i).ok;

    if isempty(qgnd) % no positive at all, skip from the average
      aps (i) = nan;
      prs (i, :) = nan;
      nempty = nempty + 1;
      continue;
    end

    if isfield (gnd(i), 'junk')
      qgndj = gnd(i).junk;
    else
      qgndj = [];
    end

    % positions of positive and junk images
    [~, pos] = intersect (ranks (:,i), qgnd);
    [~, junk] = intersect (ranks (:,i), qgndj);

    pos = sort(pos);
    junk = sort(junk);

    k = 0;
    ij = 1;

    if length (junk)
        % decrease positions of positives based on the number of junk images appearing before them
        ip = 1;
        while ip <= numel (pos)

            while ( ij <= length (junk) & pos (ip) > junk (ij) )
                k = k + 1;
                ij = ij + 1;
            end

            pos (ip) = pos (ip) - k;
            ip = ip + 1;
        end
    end

    % compute ap
    ap = score_ap_from_ranks1 (pos, length (qgnd));
    map = map + ap;
    aps (i) = ap;

    % compute precision@k
    for j = 1:numel(kappas)
      kq = min(max(pos), kappas(j));
      prs(i, j) = numel(find(pos <= kq)) ./ kq;
    end
    pr = pr + prs(i, :);

  end

  map = map / (nq-nempty);
  pr = pr / (nq-nempty);

end

% This function computes the AP for a query
function ap = score_ap_from_ranks1 (ranks, nres)

% number of images ranked by the system
nimgranks = length (ranks);
ranks = ranks - 1;

% accumulate trapezoids in PR-plot
ap = 0;

recall_step = 1 / nres;

for j = 1:nimgranks
  rank = ranks(j);

  if rank == 0
    precision_0 = 1.0;
  else
    precision_0 = (j - 1) / rank;
  end

  precision_1 = j / (rank + 1);
  ap = ap + (precision_0 + precision_1) * recall_step / 2;
end

end


function [horiList, vertList, horiPos, vertPos] = createDeBruijnSeq(prjW, prjH)
%% Create the De Bruijn sequence we used in the paper.
% 1 = Red
% 2 = Yellow
% 3 = Lime
% 4 = Green
% 5 = Cyan
% 6 = Blue
% 7 = Purple
% 8 = Magenta
% See also: ImgProc.genStructuredLight

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%%

HStripeT = 2;
VStripeT = 2;
TopBotbuffer = 10;
LeftRightbuffer = 10;
Debrujin_k = 4; % of color
Debrujin_n = 3; %size of window (seq length)
Debrujin_size = power(Debrujin_k, Debrujin_n)+(Debrujin_n-1); %length of Debruijn sequence, +n-1 for wrap

Hsr = TopBotbuffer; % start position of Horizontal strips
Vsr = LeftRightbuffer; % start position of Vertical strips
Hspace = floor((prjH - TopBotbuffer * 2 - HStripeT) / (Debrujin_size-1));  % has to match CreateDeBrujin
Vspace = floor((prjW - LeftRightbuffer * 2 - VStripeT) / (Debrujin_size-1));

% Create Debruijn sequence
sequence = zeros(Debrujin_size,1);

HL = 4;

digOne = HL - 1;
digTwo = HL;
i = 0;
for curHL = 1 : HL
    for curdigOne = 1:digOne
        for curdigTwo = 1:digTwo
            i = i+1;
            sequence(i) = (HL - (curdigOne - 1));
            i = i+1;
            sequence(i) = (HL - (curdigTwo - 1));
            i = i+1;
            sequence(i) = (curHL);
        end
    end

    if (curHL < HL)
        i = i+1;
        sequence(i) = (curHL);
    end

    digOne = digOne - 1;
    digTwo = digTwo - 1;

end

% Three additions of HL to the end
i = i+1;
sequence(i) = (HL);
i = i+1;
sequence(i) = (HL);
i = i+1;
sequence(i) = (HL);

horiList = zeros(Debrujin_size,1);
vertList = zeros(Debrujin_size,1);
horiPos = zeros(Debrujin_size,1);
vertPos = zeros(Debrujin_size,1);

% Create De Bruijn look up table(position and list)
for i = 1:Debrujin_size
    horiList(i) = 2 * sequence(i) - 1;
    horiPos(i)  = Hsr+2; % for 1280 and topbottom buffer = 20, space is 19.375 without HstripT
    Hsr = Hsr + Hspace;

    %Vertical
    vertList(i)  = 2 * sequence(i);
    vertPos(i)  = Vsr+2; % for 1280 and topbottom buffer = 20, space is 19.375 without HstripT
    Vsr = Vsr + Vspace;
end

horiList = horiList';
vertList = vertList';
horiPos = horiPos';
vertPos = vertPos';

end

%     horiList = [7,7,1,7,5,1,7,3,1,7,1,1,5,7,1,5,5,1,5,3,1,5,1,1,3,7,1,3,5,1,...
%         3,3,1,3,1,1,1,7,7,3,7,5,3,7,3,3,5,7,3,5,5,3,5,3,3,3,7,7,5,7,5,5,5,7];
%
%     vertList = [8,8,2,8,6,2,8,4,2,8,2,2,6,8,2,6,6,2,6,4,2,6,2,2,4,8,2,4,6,2,...
%         4,4,2,4,2,2,2,8,8,4,8,6,4,8,4,4,6,8,4,6,6,4,6,4,4,4,8,8,6,8,6,6,6,8];
%
%     horiPos = [10,21,32,43,54,65,76,87,98,109,120,131,142,153,164,175,186,197,...
%         208,219,230,241,252,263,274,285,296,307,318,329,340,351,362,373,384,395,...
%         406,417,428,439,450,461,472,483,494,505,516,527,538,549,560,571,582,593,604,...
%         615,626,637,648,659,670,681,692,703];
%
%     vertPos = [10,29,48,67,86,105,124,143,162,181,200,219,238,257,276,295,314,333,...
%         352,371,390,409,428,447,466,485,504,523,542,561,580,599,618,637,656,675,...
%         694,713,732,751,770,789,808,827,846,865,884,903,922,941,960,979,998,1017,...
%         1036,1055,1074,1093,1112,1131,1150,1169,1188,1207];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KR_1000_1300_TITAN(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR 1000 1300 TITAN robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KR_1000_1300_TITAN returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR_1000_1300_TITAN');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%   Authors:   Javier Martinez Gonzalez
%              Jose Francisco Munoz Sempere
%              Silvia Carretero Monasor
%              Marcos Gomez Parres
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

function q = inversekinematic_KR_1000_1300_TITAN(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

L6=d(6); %Distancia de la mu�eca al efector final.

A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
%Tomamos el vector "a"
W = T(1:3,3); %ax, ay, az

% Pm: wrist position (Posici�n de la mu�eca)
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
%Obtenemos "theta 1" mediante m�todos geom�tricos.
q1=atan2(-Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%At this point, we want to asure, that, at least, the function returns
%4 real solutions q(1:4). If any of the solutions q(1:4) is complex, only
%the real part will be returned. If any of the solutions q(5:8) is complex,
%it will not be considered and removed
q = arrange_solutions(q);

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% % solve for the last three joints
% % for any of the possible combinations (theta1, theta2, theta3)
% %Please note the special orientation of the wrist of this robot. In this
% %case, we employ an 'ad hoc' function to solve for the final orientaion
for i=1:2:size(q,2),
    qtemp = solve_for_last_three_joints(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_for_last_three_joints(robot, q(:,i), T, -1); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
%Par�metros con los que calculamos "theta 2" y "theta 3", tambi�n mediante
%m�todos geom�tricos.
%Tenemos en cuenta el desfase de 65mm entre los centros de los sistemas
%de referencia de los eslabones 2 y 3.
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3)); %ESTE ELEMENTO ES EL DESFASE CITADO
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

L4 = sqrt(A2^2 + L3^2); %LONGITUD DEL ESLAB�N TENIENDO EN CUENTA EL DESFASE
%(DISTANCIA REAL ENTRE MU�ECA Y ESLAB�N 2)
%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2); %r ES LA DISTANCIA DEL SIST. 1 A LA MU�ECA

beta = atan2(-p1(2), p1(1)); %BETA=ARCTG(-Y/X)
gamma = (acos((L2^2+r^2-L4^2)/(2*r*L2))); %TEOREMA DEL COSENO

if ~isreal(gamma)
    disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta
q2(1) = pi/2-gamma-beta; %elbow up (CODO ARRIBA)
q2(2) = pi/2+gamma-beta; %elbow down (CODO ABAJO)
%DEBEMOS A�ADIR UN �NGULO DE PI/2 DEBIDO A DESFASES EN NUESTROS SISTEMAS DE
%REFERENCIA

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

L4 = sqrt(A2^2 + L3^2); %VOLVEMOS A TENER EN CUENTA EL DESFASE

phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); %TEOMERA DEL COSENO
%PHI, ES EL �NGULO QUE FORMAN LAS L�NEA QUE DEFINEN EL DESFASE ENTRE 2 Y 3,
%Y EL PROPIO ESLAB�N 4
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2); %DE NUEVO, DISTANCIA ENTRE SIST.1 Y MU�ECA

eta = (acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); %TEOREMA DEL COSENO
%ETA, ES EL �NGULO REAL ENTRE LOS ESLABONES 3 Y 4

if ~isreal(eta)
   disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta; %CODO ARRIBA
q3(2) = pi - phi + eta; %CODO ABAJO
%SE A�ADEN LOS �NGULOS "PI" EN AMBAS SOLUCIONES DEBIDO A DESFASES ENTRE
%NUESTROS SISTEMAS DE REFERENCIA.

% Solve for the last three joints asuming an spherical wrist
function q = solve_for_last_three_joints(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz6=T(1:3,3);
vz5=T(1:3,3); % The vector a T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz6);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(-vy3,vz4);
    sinq4=wrist*dot(vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(vx4,vz5);
sinq5=dot(vy4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


function [dlnZ_dmu, dlnZ_dvs] = cmpndNoiseGradVals(noise, mu, varsigma, y)

% CMPNDNOISEGRADVALS Gradient of CMPND noise log Z with respect to input mean and variance.
% FORMAT
% DESC computes the gradient of the compound
% noise with respect to the input mean and the input variance.
% ARG noise : noise structure for which gradients are being
% computed.
% ARG mu : mean input locations with respect to which gradients are
% being computed.
% ARG varSigma : variance input locations with respect to which
% gradients are being computed.
% ARG y : noise model output observed values associated with the given points.
% RETURN dlnZ_dmu : the gradient of log Z with respect to the input mean.
% RETURN dlnZ_dvs : the gradient of log Z with respect to the input variance.
%
% SEEALSO cmpndNoiseParamInit, cmpndNoiseGradientParam, noiseGradVals,
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

startVal = 1;
endVal = 0;
dlnZ_dmu = zeros(size(mu));
dlnZ_dvs = zeros(size(varsigma));
for i = 1:length(noise.comp)
  fhandle = str2func([noise.comp{i}.type 'NoiseGradVals']);
  [dlnZ_dmu(:, i), dlnZ_dvs(:, i)]  = fhandle(noise.comp{i}, ...
                                              mu(:, i), ...
                                              varsigma(:, i), ...
                                              y(:, i));
end


function [z0,deltaZ]=addAstroRefrac(algorithm,plhObs,zTrue,Rh,P,T,wl)
%%ADDASTROREFRAC Add the effects of refraction to the true zenith angle of
%                an object outside the atmosphere using low-precision
%                atmospheric models for an observer near the surface of
%                the Earth viewing an object using a narrowband sensor.
%
%INPUTS: algorithm This specified the algorithm used. The possible values
%                  are:
%                  0: Use a numerical integration method from Chapter 7.2
%                     of [1] and from [2]. This algorithm uses the inputs
%                     plhObs,z0, Rh, P, T, and wl and makes use of the
%                     Sinclair atmospheric model. This algorithm is the
%                     most precise of all of the methods. If a point is too
%                     far below the horizon, z0 and deltaZ will be empty
%                     matrices.
%                  1: Use the simple formula of [3] for the refraction
%                     experienced by an observer at sea level viewing light
%                     with a wavelength of 0.574 micrometers (yellow). This
%                     algorithm uses the inputs z0, Rh, P, T. This
%                     algorithm is only valid for refraction-corrupted
%                     zenith distances below 70 degrees. Values that are
%                     too high will lead to z0 and deltaZ being empty
%                     matrices.
%                  2: Use the algorithm from the International Astronomical
%                     Union's (IAU) standards of fundamental astronomy
%                     library. This algorithm uses the inputs z0,Rh,P,T,wl
%                     for an observer at sea level. This algorithm will
%                     produce results for all positive zenith distances,
%                     though the results might not be very good for large
%                     values.
%           plhObs The WGS-84 ellipsoidal latitude, longitude and height
%                  of the observer in meters. Only algorithm 0 uses this
%                  parameter, and only the latitude and height above the
%                  reference ellipsoid are used (the latitude does not
%                  matter). The height is treated as an approximate height
%                  above mean sea level. Given the precision of the
%                  algorithm, the fact that geoid undulations are ignored
%                  probably does not matter.For algorithms 1 and 2, an
%                  empty matrix can be passed.
%            zTrue A vector or matrix of true (refraction-free) positive
%                  zenith "distances" (in radians) of the celestial object
%                  being observed. A zenith distance is the observed angle
%                  of an object down from the gravitational vertical (the
%                  zenith) at the observer's location. This value is
%                  inaccurate as one approaches pi/2 radians (the horizon).
%                  zTrue must be greater than zero. Given the precision of
%                  the algorithms that are available, an angle with respect
%                  to the vertical defined by the WGS-84 reference
%                  ellipsoid could probably be substituted for an angle
%                  with respect to the true gravitational vertical.
%               Rh The relative humidity at the observer (between 0 and 1).
%                  If this parameter is omitted or an empty matrix is
%                  passed, then Constants.standardRelHumid is used.
%                P The atmospheric pressure at the observer in Pascals
%                  (N/m^2). If this parameter is omitted or an empty matrix
%                  is passed, then Constants.standardAtmosphericPressure is
%                  used.
%                T The air temperature at the observer in degrees Kelvin.
%                  If this parameter is omitted or an empty matrix is
%                  passed, then Constants.standardTemp is used.
%               wl The wavelength at which the observation is made in units
%                  of meters. If this parameter is omitted or an empty
%                  matrix is passed, then a wavelength of 0.574 micrometers
%                  is used, which is in the visible spectrum (a rather
%                  yellow color).
%
%OUTPUTS: z0  An NX1 vector of the zTrue values in radians with atmospheric
%             refraction added.
%      deltaZ The refraction value that was applied. z0=zTrue-deltaZ.
%
%The function removeAstroRefrac computes deltaZ for the problem where z0 is
%known and zTrue is unknown. This function iterates the solution a few
%times to try to solve the inverse problem. A fixed 20 iterations are used,
%which is generally sufficient to ensure convergence to within working
%precision limits for all of the algorithms.
%
%REFERENCES:
%[1] S. E. Urban and K. P.Seidelmann, Eds.,Explanatory Supplement to the
%    Astronomical Almanac, 3rd ed. Mill Valley, CA: University Science
%    Books, 2013.
%[2] C. Y. Hohenkerk and A. T. Sinclair, "The computation of angular
%    atmospheric refraction at large zenith angles," United Kingdom
%    Hydrographic Office, HM Nautical Almanac, Tech. Rep. 63, Apr. 1985.
%    http://astro.ukho.gov.uk/data/tn/naotn63.pdf
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<7||isempty(wl))
       wl=0.574e-6;
    end

    if(nargin<6||isempty(T))
       T=Constants.standardTemp;
    end

    if(nargin<5||isempty(P))
        P=Constants.standardAtmosphericPressure;
    end

    if(nargin<4||isempty(Rh))
        Rh=Constants.standardRelHumid;
    end

    %The initial estimate of deltaZ is given by solving the inverse problem
    %at zTrue
    [~,deltaZ]=removeAstroRefrac(algorithm,plhObs,zTrue,Rh,P,T,wl);

    if(~isempty(deltaZ))
        numIter=20;
        for curIter=1:numIter
            [~,deltaZ]=removeAstroRefrac(algorithm,plhObs,zTrue-deltaZ,Rh,P,T,wl);
            if(isempty(deltaZ))
                %This can arise if the observation ends up too far
                %underground.
                break
            end
        end
    end
    if(isempty(deltaZ))
        z0=[];
    else
        z0=zTrue-deltaZ;
    end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


if eps == 0
    DERIV_NOISE = 0;
end

%% Compute Derivative
Nt = size(x,1);
if any(strcmp(InputSignalType,{'sine2', 'chirp','prbs', 'sphs','mixed', 'noise','unforced'})==1) && DERIV_NOISE == 0%eps==0 % eps~=0
    % compute derivative using fourth order central difference
    % use TVRegDiff if more error
    dx = zeros(Nt-5,Nvar+1,Nic);
    for iIC = 1:Nic
    dx_tmp = zeros(Nt-5,3);
    for i=3:Nt-3
        for k=1:size(x,2)
            dx_tmp(i-2,k) = (1/(12*dt))*(-x(i+2,k,iIC)+8*x(i+1,k,iIC)-8*x(i-1,k,iIC)+x(i-2,k,iIC));
        end
    end
    dx(:,1:5,iIC) = dx_tmp;
    end

    % concatenate
    xnew = [x(3:end-3,:,:)];
    unew = [u(3:end-3,:,:)];

    disp(['DONE: computed derivative'])
else %excitation using Gaussian white noise

end

%% Reshape
% uensemble = [u;uv];
% uensemble = u;
X =[];
DX = [];
uensemble = [];
for iIC = 1:Nic
    X = [X; xnew(:,:,iIC)];
    DX = [DX; dx(:,:,iIC)];
    uensemble = [uensemble; unew(:,:,iIC)];
end
% xaug = [X repmat(uensemble(3:end-3,:),[Nic 1])];
xaug = [X uensemble];
DX(:,Nvar+1:size(xaug,2)) = repmat(0*DX(:,Nvar),[1 size(u,2)]);

M = size(xaug,1);

n = size(DX,2)-1;
%% Sparse regression
clear Theta Xi
Theta = poolData(xaug,n,polyorder,usesine);
Theta_norm = zeros(size(Theta,2),1);
for i = 1:size(Theta,2)
   Theta_norm(i) = norm(Theta(:,i));
   Theta(:,i) = Theta(:,i)./Theta_norm(i);
end
m = size(Theta,2);

if exist('lambda_vec') == 1
    Xi = sparsifyDynamicsIndependent(Theta,DX,lambda_vec,n);
else
    Xi = sparsifyDynamics(Theta,DX,lambda,n);
end

if n == 3
    str_vars = {'x','y','u'};
elseif n == 4
    str_vars = {'x','y','z','u'};
elseif n == 5
    str_vars = {'x1','x2','x3','x4','x5'};
elseif n == 6
    str_vars = {'x1','x2','x3','x4','x5','u'};
end

for i = 1:size(Theta,2)
   Xi(i,:) = Xi(i,:)./Theta_norm(i);
end

yout = poolDataLIST(str_vars,Xi,n,polyorder,usesine);


function [ a, rcond, info ] = cpoco ( a, lda, n )

%*****************************************************************************80
%
%% CPOCO factors a complex hermitian positive definite matrix.
%
%  Discussion:
%
%    The routine also estimates the condition of the matrix.
%
%    If RCOND is not needed, CPOFA is slightly faster.
%
%    To solve A*X = B, follow CPOCO by CPOSL.
%
%    To compute inverse(A)*C, follow CPOCO by CPOSL.
%
%    To compute determinant(A), follow CPOCO by CPODI.
%
%    To compute inverse(A), follow CPOCO by CPODI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the hermitian matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); an upper triangular matrix R so that
%      A = hermitian(R)*R
%    where hermitian(R) is the conjugate transpose.  The strict lower
%    triangle is unaltered.  If INFO ~= 0, the factorization is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
%  Local Parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  for j = 1 : n

    z(j) = scasum ( j, a(1:j,j), 1 );

    for i = 1 : j - 1
      z(i) = real ( z(i) ) + cabs1 ( a(i,j) );
    end

  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ a, info ] = cpofa ( a, lda, n );

  if ( info ~= 0 )
    return;
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where hermitian(R)*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve hermitian(R)*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, -z(k) );
    end

    if ( real ( a(k,k) ) < cabs1 ( ek - z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( ek - z(k) );
      z(1:n) = z(1:n) * s;
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = cabs1 ( wk );
    sm = cabs1 ( wkm );
    wk = wk / a(k,k);
    wkm = wkm / a(k,k);
    kp1 = k + 1;

    if ( kp1 <= n )

      for j = kp1 : n
        sm = sm + cabs1 ( z(j) + wkm * conj ( a(k,j) ) );
        z(j) = z(j) + wk * conj ( a(k,j) );
        s = s + cabs1 ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        z(kp1:n) = z(kp1:n) + t * conj ( a(k,kp1:n) );
      end

    end

    z(k) = wk;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
    end

    z(k) = z(k) / a(k,k);
    t = -z(k);
    z(1:k-1) = z(1:k-1) + t * transpose ( a(1:k-1,k) );

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve hermitian(R) * V = Y.
%
  for k = 1 : n

    z(k) = z(k) - z(1:k-1) * conj ( a(1:k-1,k) );

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / a(k,k);

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve R * Z = V.
%
  for k = n: -1 : 1

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / a(k,k);
    t = -z(k);
    z(1:k-1) = z(1:k-1) + t * transpose ( a(1:k-1,k) );

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function [y] = spm_fx_fmri_linear(x,u,P,M)
% state equation for a dynamic model of fMRI (linear version)
% responses
% FORMAT [y] = spm_fx_fmri_linear(x,u,P,M)
% x      - state vector
%   x(:,1) - excitatory neuronal activity             ue
%   x(:,2) - vascular signal                          s
%   x(:,3) - rCBF                                  ln(f)
%   x(:,4) - venous volume                         ln(v)
%   x(:,5) - deoyxHb                               ln(q)
%  [x(:,6) - inhibitory neuronal activity             ui]
%
% y      - dx/dt
%
%___________________________________________________________________________
%
% References for hemodynamic & neuronal state equations:
% 1. Buxton RB, Wong EC & Frank LR. Dynamics of blood flow and oxygenation
%    changes during brain activation: The Balloon model. MRM 39:855-864,
%    1998.
% 2. Friston KJ, Mechelli A, Turner R, Price CJ. Nonlinear responses in
%    fMRI: the Balloon model, Volterra kernels, and other hemodynamics.
%    Neuroimage 12:466-477, 2000.
% 3. Stephan KE, Kasper L, Harrison LM, Daunizeau J, den Ouden HE,
%    Breakspear M, Friston KJ. Nonlinear dynamic causal models for fMRI.
%    Neuroimage 42:649-662, 2008.
% 4. Marreiros AC, Kiebel SJ, Friston KJ. Dynamic causal modelling for
%    fMRI: a two-state model.
%    Neuroimage. 2008 Jan 1;39(1):269-78.
%__________________________________________________________________________

% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston & Klaas Enno Stephan
% $Id: spm_fx_fmri_linear.m 4052 2010-08-27 19:22:44Z karl $

% Neuronal motion
%==========================================================================
P.B   = full(P.B);                       % bi-linear parameters
P.C   = P.C/16;                          % exogenous parameters
P.D   = full(P.D);                       % nonlinear parameters

% excitatory connections
%--------------------------------------------------------------------------
for i = 1:size(P.B,3)
    P.A = P.A + u(i)*P.B(:,:,i);
end

% and nonlinear (state) terms
%--------------------------------------------------------------------------
for i = 1:size(P.D,3)
    P.A = P.A + x(i,1)*P.D(:,:,i);
end

% implement differential state equation y = dx/dt (neuronal)
%--------------------------------------------------------------------------
y    = x;
if size(x,2) == 5

    % one neuronal state per region
    %----------------------------------------------------------------------
    y(:,1) = P.A*x(:,1) + P.C*u(:);

else

    % extrinsic (two neuronal states)
    %----------------------------------------------------------------------
    A      = exp(P.A)/8;             % enforce positivity
    IE     = diag(diag(A));          % inhibitory to excitatory
    EE     = A - IE;                 % excitatory to excitatory
    EI     = 1;                      % excitatory to inhibitory
    SE     = 1;                      % self-inhibition (excitatory)
    SI     = 2;                      % self-inhibition (inhibitory)

    % motion - excitatory and inhibitory: y = dx/dt
    %----------------------------------------------------------------------
    y(:,1) = EE*x(:,1) - SE*x(:,1) - IE*x(:,6) + P.C*u(:);
    y(:,6) = EI*x(:,1) - SI*x(:,6);

end

% Hemodynamic motion
%==========================================================================

% hemodynamic parameters
%--------------------------------------------------------------------------
%   H(1) - signal decay                                   d(ds/dt)/ds)
%   H(2) - autoregulation                                 d(ds/dt)/df)
%   H(3) - transit time                                   (t0)
%   H(4) - exponent for Fout(v)                           (alpha)
%   H(5) - resting oxygen extraction                      (E0)
%   H(6) - ratio of intra- to extra-vascular components   (epsilon)
%          of the gradient echo signal
%--------------------------------------------------------------------------
H      = [0.65 0.41 2.00 0.32 0.34];
H      = [0.64 0.32 2.00 0.32 0.32];

% signal decay
%--------------------------------------------------------------------------
sd     = H(1)*exp(P.decay);

% transit time
%--------------------------------------------------------------------------
tt     = H(3)*exp(P.transit);

% Fout = f(v) - outflow
%--------------------------------------------------------------------------
fv     = 1 + x(:,4)/H(4);

% e = f(f) - oxygen extraction x rCBF
%--------------------------------------------------------------------------
ff     = (1 - log(H(5))).*x(:,3);

% implement differential state equation y = dx/dt (hemodynamic)
%--------------------------------------------------------------------------
y(:,2) = x(:,1) - sd.*x(:,2) - H(2)*x(:,3);
y(:,3) = x(:,2);
y(:,4) = (1  + x(:,3) - fv)./tt;
y(:,5) = (ff - x(:,5) + x(:,4))./tt;
y      = y(:);


%% Recording algorithm behavior with MatlabBGL
% In this example, we will write a simple visitor that outputs an
% algorithm's behavior.  The algorithm we will examine is dijkstra_sp.  To
% examine the runtime behavior we will use a visitor which outputs a string
% every time a function is called.

%% Setup
% To begin, we load a graph.

load ../graphs/clr-25-2.mat

%%
% Next, let's check the documentation to see which functions to implement
% for the visitor

help dijkstra_sp

%%
% The help states that dijkstra_sp allows visitors functions for
% initialize_vertex, discover_vertex, examine_vertex, examine_edge,
% edge_relaxed, edge_not_relaxed, and finish_vertex.
%
% Rather than implementing 7 functions ourselves, we define two helper
% functions.  These helper functions return functions themselves.  There is
% one helper that returns a vertex visitor function and one helper than
% returns an edge visitor function.

vertex_vis_print_func = @(str) @(u) ...
    fprintf('%s called on %s\n', str, char(labels{u}));
edge_vis_print_func = @(str) @(ei,u,v) ...
    fprintf('%s called on (%s,%s)\n', str, char(labels{u}), char(labels{v}));

%%
% These anonymous functions return functions themselves.

ev_func = vertex_vis_print_func('examine_vertex');
ev_func(1)

%%
% I hope you see how these functions are useful in saving quite a bit of
% typing.

%% Calling dijkstra_sp
% We are almost done.  Now, we just have to setup the visitor structure to
% pass to the dijkstra_sp call.

vis = struct();
vis.initialize_vertex = vertex_vis_print_func('initialize_vertex');
vis.discover_vertex = vertex_vis_print_func('discover_vertex');
vis.examine_vertex = vertex_vis_print_func('examine_vertex');
vis.finish_vertex = vertex_vis_print_func('finish_vertex');
vis.examine_edge = edge_vis_print_func('examine_edge');
vis.edge_relaxed = edge_vis_print_func('edge_relaxed');
vis.edge_not_relaxed = edge_vis_print_func('edge_not_relaxed');

%%
% With the visitor setup, there is hardly any work left.

dijkstra_sp(A,1,struct('visitor', vis));

%% Understanding the output
% To understand the output, we find it helpful to have a copy of
% Introduction to Algorithms by Cormen, Leiserson, and Rivest.  The source
% for the graph is Figure 25-2 in that book and the authors use the graph
% to illustrate how Dijkstra's algorithm runs.  In particular, Figure 25-5
% shows a sample run of Dijkstra's algorithm.
%
% Perhaps the first thing to notice is that the initialize vertex visitor
% is never called.  This results from an error in the MatlabBGL and Boost
% documentation.  Once it is resolved, we will update the MatlabBGL
% documentation to match the Boost graph library.
%
% The results: discover_vertex is called before examine_vertex.  For the
% edges, examine_edge is always called before either edge_relaxed
% or edge_not_relaxed.  The edges that are relaxed are the shaded edges in
% Figure 25-5.
%
% Finally, finish vertex is called on a vertex after all of its edges have
% been examined and possibly relaxed.