function r8mat_exp_story ( test )

%*****************************************************************************80
%
%% R8MAT_EXP_STORY prints explanatory text for each problem.
%
%  Discussion:
%
%     1) Diagonal example
%     2) Symmetric example
%     3) Laub
%     4) Moler and Van Loan
%     5) Moler and Van Loan
%     6) Moler and Van Loan
%     7) Moler and Van Loan
%     8) Wikipedia example
%     9) NAG F01ECF
%    10) Ward #1
%    11) Ward #2
%    12) Ward #3
%    13) Ward #4
%    14) Moler example
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Alan Laub,
%    Review of "Linear System Theory" by Joao Hespanha,
%    SIAM Review,
%    Volume 52, Number 4, December 2010, page 779-781.
%
%    Cleve Moler, Charles VanLoan,
%    Nineteen Dubious Ways to Compute the Exponential of a Matrix,
%    Twenty-Five Years Later,
%    SIAM Review,
%    Volume 45, Number 1, March 2003, pages 3-49.
%
%    Cleve Moler,
%    Cleve's Corner: A Balancing Act for the Matrix Exponential,
%    July 23rd, 2012.
%
%    Robert Ward,
%    Numerical computation of the matrix exponential with accuracy estimate,
%    SIAM Journal on Numerical Analysis,
%    Volume 14, Number 4, September 1977, pages 600-610.
%
%  Parameters:
%
%    Input, integer TEST, the index of the test case.
%
  if ( test == 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This matrix is diagonal.\n' );
    fprintf ( 1, '  The calculation of the matrix exponential is simple.\n' );
  elseif ( test == 2 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This matrix is symmetric.\n' );
    fprintf ( 1, '  The calculation of the matrix exponential is straightforward.\n' );
  elseif ( test == 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to Laub.\n' );
    fprintf ( 1, '  This matrix is ill-suited for the Taylor series approach.\n' );
    fprintf ( 1, '  As powers of A are computed, the entries blow up too quickly.\n' );
  elseif ( test == 4 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to Moler and Van Loan.\n' );
    fprintf ( 1, '  The example will cause problems for the series summation approach,\n' );
    fprintf ( 1, '  as well as for diagonal Pade approximations.\n' );
  elseif ( test == 5 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to Moler and Van Loan.\n' );
    fprintf ( 1, '  This matrix is strictly upper triangular\n' );
    fprintf ( 1, '  All powers of A are zero beyond some (low) limit.\n' );
    fprintf ( 1, '  This example will cause problems for Pade approximations.\n' );
  elseif ( test == 6 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to Moler and Van Loan.\n' );
    fprintf ( 1, '  This matrix does not have a complete set of eigenvectors.\n' );
    fprintf ( 1, '  That means the eigenvector approach will fail.\n' );
  elseif ( test == 7 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to Moler and Van Loan.\n' );
    fprintf ( 1, '  This matrix is very close to example 5.\n' );
    fprintf ( 1, '  Mathematically, it has a complete set of eigenvectors.\n' );
    fprintf ( 1, '  Numerically, however, the calculation will be suspect.\n' );
  elseif ( test == 8 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This matrix was an example in Wikipedia.\n' );
  elseif ( test == 9 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This example is due to the NAG Library.\n' );
    fprintf ( 1, '  It is an example for function F01ECF.\n' );
  elseif ( test == 10 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This is Ward''s example #1.\n' );
    fprintf ( 1, '  It is defective and nonderogatory.\n' );
    fprintf ( 1, '  The eigenvalues are 3, 3 and 6.\n' );
  elseif ( test == 11 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This is Ward''s example #2.\n' );
    fprintf ( 1, '  It is a symmetric matrix.\n' );
    fprintf ( 1, '  The eigenvalues are 20, 30, 40.\n' );
  elseif ( test == 12 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This is Ward''s example #3.\n' );
    fprintf ( 1, '  Ward''s algorithm has difficulty estimating the accuracy\n' );
    fprintf ( 1, '  of its results.  The eigenvalues are -1, -2, -20.\n' );
  elseif ( test == 13 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This is Ward''s example #4.\n' );
    fprintf ( 1, '  This is a version of the Forsythe matrix.\n' );
    fprintf ( 1, '  The eigenvector problem is badly conditioned.\n' );
    fprintf ( 1, '  Ward''s algorithm has difficulty estimating the accuracy\n' );
    fprintf ( 1, '  of its results for this problem.\n' );
  elseif ( test == 14 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  This is Moler''s example.\n' );
    fprintf ( 1, '  This badly scaled matrix caused problems for MATLAB''s expm().\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_EXP_STORY - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of TEST = %d\n', test );
    error ( 'R8MAT_EXP_STORY - Fatal error!' );
  end

  return
end


function [minconpos, mincon]=API(fun1,LB,UB,NumAnts,Nmoves,LocalMoves,nonlinfun,rpmax,RES,varargin)
%
% APA, 2003

warning off
%  API Finds a unconstrained or constrained minimum of a function of several variables.
%     API solves problems of the form:
%         min F(X)  subject to:  g(x) <= 0,  h(x) = Beq (inequality & equality constraints)
%          X                     LB <= X <= UB (boundary constraints)
%
%     X=API(FUN,LB,UB,NumAnts,Nmoves,LocalMoves,NONLNCON,rpmax) minimizes FUN
%     subject to inequality and equality constraints in NONLINCON. (Set NONLINCON=[]
%     and rpmax=[] if no inequality and equality constraints exist).
%     NumAnts = Number of API Ants to be used.
%     Nmoves = Number of Nest Moves.
%     LocalMoves = Number of Local Moves for API Ants.
%     rpmax = Augmented Lagrangian parameter.
%
%     X=API(FUN,LB,UB,NumAnts,Nmoves,LocalMoves,NONLNCON,rpmax,P1,P2,...) passes the
%     problem-dependent parameters P1,P2,... directly to the functions FUN
%     and NONLCON: feval(FUN,X,P1,P2,...) and feval(NONLCON,X,P1,P2,...).
%
% Example:
% FUN='testbanana';LB=[-5 -5];UB=[3 3];NumAnts=20;Nmoves=15;LocalMoves=25;
% NONLNCON='nonlinbanana';rpmax=5;
% X=API(FUN,LB,UB,NumAnts,Nmoves,LocalMoves,NONLNCON,rpmax)
%
% Listing of Objective and Constraint for the above exxample
%
% function f=testbanana(x)
% f=100*(x(1)^2-x(2))^2 + (1-x(1))^2;
%
% function [g,h]=nonlinbanana(x)
% x1=x(1); x2=x(2);
% %%% Equality Constraints %%%%%%%%
% h=0;
% %%% Inequality Constraints %%%%%%
% g(1,:)=-(x1.^2+x2.^2-1.2);
% g(2,:)=-(1.5-(x1-.2)^2-x2.^2);

if isempty(nonlinfun)

    xb=[LB(:) UB(:)];

    % Specify the starting point
    Ninit = (LB+UB)/2;

    n=length(Ninit);

    %%% **************--- Unconstrained Loop ---****************

    N=Ninit;% Initial Nest Location
    min1=inf;
    disp('Nest Move    Objective')
    for ii=1:Nmoves   % Number of Nest Moves

        Smem=[];
        for num=1:NumAnts
            if(num==1)
                Asite=0.01;
            else
                Asite=0.01*(1/0.01)^(num/NumAnts);
            end
            for i=1:2
                for j=1:n
                    r=rand;
                    S(j)=N(j)+((-0.5+r)*Asite*(xb(j,2)-xb(j,1)));
                    while(~((xb(j,1)<=S(j))&(xb(j,2)>=S(j))))
                        r=rand;
                        S(j)=N(j)+((-0.5+r)*Asite*(xb(j,2)-xb(j,1)));
                    end
                end
                Smem=[Smem;S];
            end
        end
        % Fun1='fun';
        %%%%%%%%%%%% Tandem Running %%%%%%%%%%%%%%%
        a1=1+floor(NumAnts*rand);
        a2=1+floor(NumAnts*rand);
        f1=feval(fun1,Smem(2*a1-1,:),varargin{:});
        f2=feval(fun1,Smem(2*a1,:),varargin{:});
        f3=feval(fun1,Smem(2*a2-1,:),varargin{:});
        f4=feval(fun1,Smem(2*a2,:),varargin{:});

        if(f1<f2)
            fpos1=2*a1-1;
            fmin1=f1;
        else
            fpos1=2*a1;
            fmin1=f2;
        end
        if(f3<f4)
            fpos2=2*a2-1;
            fmin2=f3;
        else
            fpos2=2*a2;
            fmin2=f4;
        end
        if(fmin1<fmin2)
            Smem(fpos2,:)=Smem(fpos1,:);
        else
            Smem(fpos1,:)=Smem(fpos2,:);
        end
        %%%%%%%%%%%%%%tandem running loop ends%%%%%%%%%%%%%%%%%%

        for num=1:2:(2*NumAnts-1)

            if(num==1)
                Alocal=0.1*0.01;
            else
                Alocal=(1/10)*0.01*(1/0.01)^(((num+1)/2)/NumAnts);
            end
            pos1=num;
            pos2=num+1;

            for i=1:LocalMoves   % Number of Local Sites % Local Search Loop Starts here

                Spos=Smem(num,:);
                for j=1:n
                    r=rand;
                    Sloc(j)=Spos(j)+(-0.5+r)*Alocal*(xb(j,2)-xb(j,1));
                    while(~((xb(j,1)<=Sloc(j))&(xb(j,2)>=Sloc(j))))
                        r=rand;
                        Sloc(j)=Spos(j)+(-0.5+r)*Alocal*(xb(j,2)-xb(j,1));
                    end
                end

                V1=feval(fun1,Spos,varargin{:});
                V2=feval(fun1,Sloc,varargin{:});
                if(V1>V2)
                    Smem(pos1,:)=Sloc;
                    if(min1>V2)
                        minpos=Sloc;
                        min1=V2;
                    end
                else
                    temppos=Smem(pos1,:);
                    Smem(pos1,:)=Smem(pos2,:);
                    Smem(pos2,:)=temppos;
                    if(min1>V1)
                        minpos=Spos;
                        min1=V1;
                    end
                end

            end

        end
        %! Put-in the residue
        if min1<RES
            return
        end
        N=minpos;
        % disp('[Nest Move    Objective]')
        disp(['    ',num2str(ii),'        ',num2str(min1)])

    end

    %%%***********--- Unconstrained Loop Ends ---***************
    minconpos=minpos;
    mincon=min1;

else

    %API1Gen.m
    ftemp=fun1;nonlinfuntemp=nonlinfun;
    global rp mu1 si1 fun1 nonlinfun
    fun1=ftemp;nonlinfun=nonlinfuntemp;
    xb=[LB(:) UB(:)];

    % Specify the starting point
    Ninit = (LB+UB)/2;

    n=length(Ninit);
    rp=1;
    [g1, h1]=feval('nonlinAPI',Ninit,varargin{:});
    mu1=zeros(length(g1),1);
    si1=zeros(length(h1),1);

    mincon=inf;
    NITER=0;
    while(rp<rpmax)

        NITER=NITER+1;

        %%% **************--- Unconstrained Loop ---****************

        N=Ninit;% Initial Nest Location
        min1=inf;

        for ii=1:Nmoves   % Number of Nest Moves

            Smem=[];
            for num=1:NumAnts
                if(num==1)
                    Asite=0.01;
                else
                    Asite=0.01*(1/0.01)^(num/NumAnts);
                end
                for i=1:2
                    for j=1:n
                        r=rand;
                        S(j)=N(j)+((-0.5+r)*Asite*(xb(j,2)-xb(j,1)));
                        while(~((xb(j,1)<=S(j))&(xb(j,2)>=S(j))))
                            r=rand;
                            S(j)=N(j)+((-0.5+r)*Asite*(xb(j,2)-xb(j,1)));
                        end
                    end
                    Smem=[Smem;S];
                end
            end
            %%%%%%%%%%%% Tandem Running %%%%%%%%%%%%%%%
            a1=1+floor(NumAnts*rand);
            a2=1+floor(NumAnts*rand);
            f1=feval(@funAPI,Smem(2*a1-1,:),varargin{:});
            f2=feval(@funAPI,Smem(2*a1,:),varargin{:});
            f3=feval(@funAPI,Smem(2*a2-1,:),varargin{:});
            f4=feval(@funAPI,Smem(2*a2,:),varargin{:});

            if(f1<f2)
                fpos1=2*a1-1;
                fmin1=f1;
            else
                fpos1=2*a1;
                fmin1=f2;
            end
            if(f3<f4)
                fpos2=2*a2-1;
                fmin2=f3;
            else
                fpos2=2*a2;
                fmin2=f4;
            end
            if(fmin1<fmin2)
                Smem(fpos2,:)=Smem(fpos1,:);
            else
                Smem(fpos1,:)=Smem(fpos2,:);
            end
            %%%%%%%%%%%%%%tandem running loop ends%%%%%%%%%%%%%%%%%%

            for num=1:2:(2*NumAnts-1)

                if(num==1)
                    Alocal=0.1*0.01;
                else
                    Alocal=(1/10)*0.01*(1/0.01)^(((num+1)/2)/NumAnts);
                end
                pos1=num;
                pos2=num+1;

                for i=1:LocalMoves   % Number of Local Sites % Local Search Loop Starts here

                    Spos=Smem(num,:);
                    for j=1:n
                        r=rand;
                        Sloc(j)=Spos(j)+(-0.5+r)*Alocal*(xb(j,2)-xb(j,1));
                        while(~((xb(j,1)<=Sloc(j))&(xb(j,2)>=Sloc(j))))
                            r=rand;
                            Sloc(j)=Spos(j)+(-0.5+r)*Alocal*(xb(j,2)-xb(j,1));
                        end
                    end

                    V1=feval(@funAPI,Spos,varargin{:});
                    V2=feval(@funAPI,Sloc,varargin{:});
                    if(V1>V2)
                        Smem(pos1,:)=Sloc;
                        if(min1>V2)
                            minpos=Sloc;
                            min1=V2;
                        end
                    else
                        temppos=Smem(pos1,:);
                        Smem(pos1,:)=Smem(pos2,:);
                        Smem(pos2,:)=temppos;
                        if(min1>V1)
                            minpos=Spos;
                            min1=V1;
                        end
                    end

                end

            end

            N=minpos;

        end

        %%%***********--- Unconstrained Loop Ends ---***************

        %%% --- Augmented Lagrangian Method ---
        if(rp>1)
            mincon=feval(@funAPI,minconpos,varargin{:});
        end
        if(min1<mincon)
            minconpos=minpos;
            mincon=min1;
        end

        [temp1, temp2]=feval(nonlinfun, minconpos,varargin{:});
        mu1 = mu1 + rp.*temp1(:);
        si1 = si1 + rp.*temp2(:);
        %%% ------ End of Augmented Lagrangian -----

        rp=rp*1.15;
        if NITER==1
            disp('Iteration     Objective    Maximum Constraint')
            disp(['     ',num2str(NITER),'         ',num2str(mincon),'         ',num2str(max(temp1))])
        else
            disp(['     ',num2str(NITER),'         ',num2str(mincon),'         ',num2str(max(temp1))])
        end

    end %%% End of Constrained Loop (i.e. rp)

    %%%% function API1gen(fun1,nonlinAPI) ends

end

function Faug=funAPI(x,varargin)

global mu1 si1 rp nonlinfun fun1
if ~isempty(varargin)
    f=feval(fun1,x,varargin{:});
else
    f=feval(fun1,x);
end
if ~isempty(varargin)
    [zeta1, h1]=feval(nonlinfun,x,varargin{:});
else
    [zeta1, h1]=feval(nonlinfun,x);
end
Fp = f + rp/2*(sum(zeta1.^2) + sum(h1.^2));
Faug = Fp + sum(mu1.*zeta1) + sum(si1.*h1);

%%%%% fun1(x) ends %%%%

function [zeta1, h1]=nonlinAPI(x,varargin)
global mu1 si1 rp h1 nonlinfun

if ~isempty(varargin)
    [g1, h1]=feval(nonlinfun,x,varargin{:});
else
    [g1, h1]=feval(nonlinfun,x);
end
if(rp==1)
    zeta1=g1;
else
    zeta1=max(g1,-mu1/rp);
end

%%%%% function nonlinAPI(x) ends


function FrontNo = UpdateFront(PopObj,FrontNo,x)
% Update the front number of each solution when a solution is added or
% deleted

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [N,M] = size(PopObj);
    if nargin < 3
        %% Add a new solution (has been stored in the last of PopObj)
        FrontNo  = [FrontNo,0];
        Move     = false(1,N);
        Move(N)  = true;
        CurrentF = 1;
        % Locate the front No. of the new solution
        while true
            Dominated = false;
            for i = 1 : N-1
                if FrontNo(i) == CurrentF
                    m = 1;
                    while m <= M && PopObj(i,m) <= PopObj(end,m)
                        m = m + 1;
                    end
                    Dominated = m > M;
                    if Dominated
                        break;
                    end
                end
            end
            if ~Dominated
                break;
            else
                CurrentF = CurrentF + 1;
            end
        end
        % Move down the dominated solutions front by front
        while any(Move)
            NextMove = false(1,N);
            for i = 1 : N
                if FrontNo(i) == CurrentF
                    Dominated = false;
                    for j = 1 : N
                        if Move(j)
                            m = 1;
                            while m <= M && PopObj(j,m) <= PopObj(i,m)
                                m = m + 1;
                            end
                            Dominated = m > M;
                            if Dominated
                                break;
                            end
                        end
                    end
                    NextMove(i) = Dominated;
                end
            end
            FrontNo(Move) = CurrentF;
            CurrentF      = CurrentF + 1;
            Move          = NextMove;
        end
    else
        %% Delete the x-th solution
        Move     = false(1,N);
        Move(x)  = true;
        CurrentF = FrontNo(x) + 1;
        while any(Move)
            NextMove = false(1,N);
            for i = 1 : N
                if FrontNo(i) == CurrentF
                    Dominated = false;
                    for j = 1 : N
                        if Move(j)
                            m = 1;
                            while m <= M && PopObj(j,m) <= PopObj(i,m)
                                m = m + 1;
                            end
                            Dominated = m > M;
                            if Dominated
                                break;
                            end
                        end
                    end
                    NextMove(i) = Dominated;
                end
            end
            for i = 1 : N
                if NextMove(i)
                    Dominated = false;
                    for j = 1 : N
                        if FrontNo(j) == CurrentF-1 && ~Move(j)
                            m = 1;
                            while m <= M && PopObj(j,m) <= PopObj(i,m)
                                m = m + 1;
                            end
                            Dominated = m > M;
                            if Dominated
                                break;
                            end
                        end
                    end
                    NextMove(i) = ~Dominated;
                end
            end
            FrontNo(Move) = CurrentF - 2;
            CurrentF      = CurrentF + 1;
            Move          = NextMove;
        end
        FrontNo(x) = [];
    end
end


function rec=iEWT1D(ewt,mfb)

% ======================================================================
% function rec=iEWT1D(ewt,mfb)
%
% Perform the Inverse Empirical Wavelet Transform of ewt accordingly to
% the filter bank mfb
%
% Inputs:
%   -ewt: cell containing the EWT components
%   -mfb: filter bank used during the EWT
%
% Outputs:
%   -rec: reconstructed signal
%
% Author: Jerome Gilles
% Institution: UCLA - Department of Mathematics
% Year: 2012
% Version: 1.0
% ======================================================================

l=round(length(ewt{1})/2);

%We perform the adjoint operator to get the reconstruction
for k=1:length(ewt)
    ewt{k}=[ewt{k}(l-1:-1:1);ewt{k};ewt{k}(end:-1:end-l+1)];
    if k==1
        rec=zeros(length(ewt{1}),1);
    end
    rec=rec+real(ifft(fft(ewt{k}).*mfb{k}));
end
rec=rec(l:end-l);


function out = NL_TSTL_dimensions(y,numBins,embedParams)
% NL_TSTL_dimensions Box counting, information, and correlation dimension of a time series.
%
% Computes the box counting, information, and correlation dimension of a
% time-delay embedded time series using the TSTOOL code 'dimensions'.
% This function contains extensive code for estimating the best scaling range to
% estimate the dimension using a penalized regression procedure.
%
% cf. TSTOOL, http://www.physik3.gwdg.de/tstool/
%
%---INPUTS:
% y, column vector of time series data
% numBins, maximum number of partitions per axis
% embedParams, embedding parameters to feed BF_Embed() for embedding the
%              signal in the form {tau,m}
%
%---OUTPUTS:
% A range of statistics are returned about how each dimension estimate changes
% with m, the scaling range in r, and the embedding dimension at which the best
% fit is obtained.

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Preliminaries, check inputs
% ------------------------------------------------------------------------------
doPlot = false; % plot outputs to screen

% (1) Maximum number of bins, numBins
if nargin < 2 || isempty(numBins)
    numBins = 50; % 50 points
    fprintf(1,'Using a default of 50 bins per axis\n');
end

% (2) Set embedding parameters to defaults
if nargin < 3 || isempty(embedParams)
    embedParams = {'ac','fnnmar'};
    fprintf(1,'Using default time-delay embedding parameters: autocorrelation and cao');
else
    if length(embedParams) ~= 2
        error('Embedding parameters are incorrectly formatted -- need {tau,m}')
    end
end

% ------------------------------------------------------------------------------
%% Embed the signal
% ------------------------------------------------------------------------------
% Convert to embedded signal object for TSTOOL
s = BF_Embed(y,embedParams{1},embedParams{2},1);

if ~isa(s,'signal') && isnan(s); % embedding failed
    error('Time-delay embedding for TSTOOL failed')
end

if size(data(s),2) < 3 % embedded with dimension < 3
    % note the 'true' predicted embedding dimension
    mopt = size(data(s),2);
    % embed with dimension m = 3
    s = BF_Embed(y,embedParams{1},3,1);
    fprintf(1,'Re-embedded with embedding dimension 3\n');
else
	mopt = size(data(s),2);
end

% ------------------------------------------------------------------------------
%% Run the TSTOOL function:
% ------------------------------------------------------------------------------
% This looks for the dimensions file in the tstoolbox/@signal/dimensions directory
if ~exist(fullfile('tstoolbox','@signal','dimensions'),'file')
    error('Cannot find the code ''dimensions'' from the TSTOOL package. Is it installed and in the Matlab path?');
end
try
    [bc, ~, co] = dimensions(s,numBins);
catch me
    error('Error running TSTOOL code dimensions: %s',me.message);
end

% We now have the scaling of the boxcounting dimension, D0, the information
% dimension D1, and the correlation dimension D2.

% It seems like there's not extra information in the in dimension, D1, by these
% estimates -- focus on the bc, D0 and correlation, D2.
% Can switch this here:
compute_in = 0;

% ------------------------------------------------------------------------------
%% Convert output to vectors
% ------------------------------------------------------------------------------
% calculations for each dimension up to the maximum:
% Seems to be in units of log_2 -- log2, so actually doesn't span a very wide
% range of length scales... -- although I think the maximum length is at 1,
% so I think it might be referring to fractions of the 'attractor size'...

% (1) Boxcounting dimension (BC)
bc_logN = data(bc); % this is log(N(r)) -- number within radius (look for this to scale linearly)
bc_logr = spacing(bc); % this is log(r) -- length scale
bc_logNlogr = bc_logN./(ones(size(bc_logN,2),1)*bc_logr)'; % look for this to be constant

% (2) Information dimension (IN)
if compute_in
    in_logl = data(in); % I think this is log(l(r)), look for this to scale linearly
    in_logr = spacing(in);
    in_logllogr = in_logl./(ones(size(in_logl,2),1)*in_logr)'; % look for this to be constant
end

% (3) Correlation dimension (CO)
co_logC = data(co); % look for this to scale linearly
co_logr = spacing(co);
co_logClogr = co_logC./(ones(size(co_logC,2),1)*co_logr)'; % look for this to be constant

if doPlot
    plot(bc_logr,bc_logNlogr,'o-')
    plot(bc_logr,bc_logN,'o-')
    input('BC')
    if compute_in
        plot(in_logr,in_logllogr,'o-')
        plot(in_logr,in_logl,'o-')
        input('IN')
    end
    plot(co_logr,co_logClogr,'o-')
    plot(co_logr,co_logC,'o-')
    input('CO')
end

% *** We now have to look for scaling regimes in each of these dimensions

% ------------------------------------------------------------------------------
%% Basic statistics on curves
% ------------------------------------------------------------------------------
out = struct;

%-------------------------------------------------------------------------------
%% How do curves change with m?
%-------------------------------------------------------------------------------
% Use SUB_mch

% Box counting dimension:
out = SUB_mch(bc_logr,bc_logN,'bc',out);

% Information dimension:
if compute_in
    out = SUB_mch(bc_logr,bc_logN,'in',out);
end

% Correlation dimension:
out = SUB_mch(co_logr,co_logC,'co',out);

% ------------------------------------------------------------------------------
%% What is the scaling range in r?
% ------------------------------------------------------------------------------
% ... and how good is the fit over this range?
% Use SUB_ScalingRange

% Box counting dimension, m = 1
out = SUB_ScalingRange(bc_logr,bc_logN(:,1),'scr_bc_m1',out);

% Box counting dimension m = 2
out = SUB_ScalingRange(bc_logr,bc_logN(:,2),'scr_bc_m2',out);

% Box counting dimension m = 3
out = SUB_ScalingRange(bc_logr,bc_logN(:,3),'scr_bc_m3',out);

% Box counting dimension m = chosen/given
out = SUB_ScalingRange(bc_logr,bc_logN(:,mopt),'scr_bc_mopt',out);

if compute_in
    % Information dimension, m = 1
    out = SUB_ScalingRange(in_logr,in_logl(:,1),'scr_in_m1',out);

    % Information dimension m = 2
    out = SUB_ScalingRange(in_logr,in_logl(:,2),'scr_in_m2',out);

    % Information dimension m = 3
    out = SUB_ScalingRange(in_logr,in_logl(:,3),'scr_in_m3',out);

    % Information dimension m = chosen/given
    out = SUB_ScalingRange(in_logr,in_logl(:,mopt),'scr_in_mopt',out);
end

% Correlation dimension, m = 1
out = SUB_ScalingRange(co_logr,co_logC(:,1),'scr_co_m1',out);

% Correlation dimension m = 2
out = SUB_ScalingRange(co_logr,co_logC(:,2),'scr_co_m2',out);

% Correlation dimension m = 3
out = SUB_ScalingRange(co_logr,co_logC(:,3),'scr_co_m3',out);

% Correlation dimension m = chosen/given
out = SUB_ScalingRange(co_logr,co_logC(:,mopt),'scr_co_mopt',out);

% ------------------------------------------------------------------------------
%% What m gives best fit?
% ------------------------------------------------------------------------------
% Use SUB_bestm

% Box counting dimension
out = SUB_bestm(bc_logr,bc_logN,'bc',out);

if compute_in
    % Information dimension
    out = SUB_bestm(in_logr,in_logl,'in',out);
end

% Correlation dimension
out = SUB_bestm(co_logr,co_logC,'co',out);

% ------------------------------------------------------------------------------
% ------------------------------------------------------------------------------
function out = SUB_mch(logr,logN,prefix,out)
    % looks at how changes with m. Since m will in general be different for each
    % different time series (i.e., if choosing an automatic method for
    % determining the embedding parameters), we have that m is at least
    % 3 here so that we can do statistics on at least these ones...

    % (i) on average the raw means at each m up to m = 3
    out.([prefix,'_meanm1']) = mean(logN(:,1));
    out.([prefix,'_meanm2']) = mean(logN(:,2));
    out.([prefix,'_meanm3']) = mean(logN(:,3));
    out.([prefix,'_meanmmax']) = mean(logN(:,end));

    % (ii) raw minimum at each m up to m = 3
    out.([prefix,'_minm1']) = min(logN(:,1));
    out.([prefix,'_minm2']) = min(logN(:,2));
    out.([prefix,'_minm3']) = min(logN(:,3));
    out.([prefix,'_minmmax']) = min(logN(:,end));

    % (iii) range at each m up to m = 3
    out.([prefix,'_range1']) = range(logN(:,1));
    out.([prefix,'_range2']) = range(logN(:,2));
    out.([prefix,'_range3']) = range(logN(:,3));
    out.([prefix,'_rangemmax']) = range(logN(:,end));

    % (iv) increments with m
    out.([prefix,'_mindiff']) = mean([min(logN(:,2))-min(logN(:,1)),min(logN(:,3))-min(logN(:,2))]);
    out.([prefix,'_meandiff']) = mean([mean(logN(:,2))-mean(logN(:,1)),mean(logN(:,3))-mean(logN(:,2))]);

    % (v) slopes and goodness of fit across whole r range
    [out.([prefix,'_lfitm1']), out.([prefix,'_lfitb1']), out.([prefix,'_lfitmeansqdev1'])] = subsublinfit(logr,logN(:,1)');
    [out.([prefix,'_lfitm2']), out.([prefix,'_lfitb2']), out.([prefix,'_lfitmeansqdev2'])] = subsublinfit(logr,logN(:,2)');
    [out.([prefix,'_lfitm3']), out.([prefix,'_lfitb3']), out.([prefix,'_lfitmeansqdev3'])] = subsublinfit(logr,logN(:,3)');
    [out.([prefix,'_lfitmmax']), out.([prefix,'_lfitbmax']), out.([prefix,'_lfitmeansqdevmax'])] = subsublinfit(logr,logN(:,end)');

    function [m, b, meansqdev] = subsublinfit(x,y)
        p1 = polyfit(x,y,1);
        pfit = p1(1)*x + p1(2);
        res = y-pfit;
        m = p1(1); % gradient
        b = p1(2); % intercept
        meansqdev = mean(res.^2);
    end
end

% ------------------------------------------------------------------------------
function out = SUB_ScalingRange(logr,logN,prefix,out)
    % determines the scaling range in r for some m
    % we remove points from either extreme in r until minimize some
    % error measure
    % two dimensional optimization: over starting point and ending
    % point.

    l = length(logr);
    stptr = 1:floor(l/2)-1; % must be in the first half (not necessarily, but for here)
    endptr = ceil(l/2)+1:l; % must be in second half (not necessarily, but for here)
    mybad = zeros(length(stptr),length(endptr));
    for i = 1:length(stptr)
        for j = 1:length(endptr)
            mybad(i,j) = lfitbadness(logr(stptr(i):endptr(j)),logN(stptr(i):endptr(j))');
        end
    end
    [a,b] = find(mybad == min(min(mybad))); % this defines the 'best' scaling range
%         plot(logr,logN,'o-b'); hold on; plot(logr(stptr(a):endptr(b)),logN(stptr(a):endptr(b)),'o-r');
%         hold off
%         disp(['keep from ' num2str(stptr(a)) ' to ' num2str(endptr(b))])

    out.([prefix,'_logrmin']) = logr(stptr(a)); % minimum of scaling range
    out.([prefix,'_logrmax']) = logr(endptr(b)); % maximum of scaling range
    out.([prefix,'_logrrange']) = logr(endptr(b)) - logr(stptr(a)); % range of scaling... range
    out.([prefix,'_pgone']) = (stptr(a)-1 + l - endptr(b))/length(logr); % number of points removed in process
                                               				  % of choosing the optimum scaling range

	% Do the optimum fit again
	x = logr(stptr(a):endptr(b));
	y = logN(stptr(a):endptr(b))';
    p = polyfit(x,y,1);
    pfit = p(1)*x + p(2);
    res = pfit - y;
    out.([prefix,'_meanabsres']) = mean(abs(res));
	out.([prefix,'_meansqres']) = mean(res.^2);
    out.([prefix,'_scaling_exp']) = p(1);
	out.([prefix,'_scaling_int']) = p(2);
	out.([prefix,'_minbad']) = min(min(mybad));

    function badness = lfitbadness(x,y)
        gamma = 0.02; % reguralization parameter gamma selected empirically, could be tweaked in future work
        p = polyfit(x,y,1);
        pfit = p(1)*x+p(2);
        res = pfit - y;
        badness = mean(abs(res)) - gamma*length(x); % want to still maximize length(x)
    end
end

% ------------------------------------------------------------------------------
function out = SUB_bestm(logr,logNN,prefix,out)
    % logNN is a matrix... logN is a vector for a given m
	% determines the scaling range in r for some m
    % we remove points from either extreme in r until minimize some
    % error measure
    % two dimensional optimization: over starting point and ending
    % point.

	store_scalingexps = zeros(size(logNN,2),1);
	store_meansqres = zeros(size(logNN,2),1);
	for k = 1:size(logNN,2);
		logN = logNN(:,k); % take this element

        l = length(logr);
        stptr = 1:floor(l/2)-1; % must be in the first half (not necessarily, but for here)
        endptr = ceil(l/2)+1:l; % must be in second half (not necessarily, but for here)
        mybad = zeros(length(stptr),length(endptr));
        for i = 1:length(stptr)
            for j = 1:length(endptr)
                mybad(i,j) = lfitbadness(logr(stptr(i):endptr(j)),logN(stptr(i):endptr(j))');
            end
        end
        [a, b] = find(mybad == min(min(mybad))); % this defines the 'best' scaling range

		% Do the optimum fit again
		x = logr(stptr(a):endptr(b));
		y = logN(stptr(a):endptr(b))';
        p = polyfit(x,y,1);
        pfit = p(1)*x+p(2);
        res = pfit - y;
		% subout.meanabsres = mean(abs(res));
		% subout.meansqres = mean(res.^2);
		% subout.scaling_exp = p(1);
		% subout.scaling_int = p(2);
		% subout.minbad = min(min(mybad));

		store_scalingexps(k) = p(1);
		store_meansqres(k) = mean(res.^2);
	end

	out.([prefix,'_minscalingexp']) = min(store_scalingexps);
	out.([prefix,'_meanscalingexp']) = mean(store_scalingexps);
	out.([prefix,'_maxscalingexp']) = max(store_scalingexps);
	out.([prefix,'_mbestfit']) = find(store_meansqres == min(store_meansqres),1,'first');

    function badness = lfitbadness(x,y)
        gamma = 0.02; % reguralization parameter gamma selected empirically, could be tweaked in future work
        p = polyfit(x,y,1);
        pfit = p(1)*x + p(2);
        res = pfit - y;
        badness = mean(abs(res)) - gamma*length(x); % want to still maximize length(x)
    end
end

end


function y=nsct_fuse(I1,I2,nlevels)
%    NSCT
%    Input:
%    I1 - input image A
%    I2 - input image B
%    nlevels - number of directions in each decomposition level
%    Output:
%    y  - fused image
%
%    The code is edited by Yu Liu, 01-09-2014.

%nlevels = [2,3,3,4] ;
pfilter = 'pyrexc' ;
dfilter = 'vk' ;

I1=double(I1);
I2=double(I2);
coeffs_1 = nsctdec( I1, nlevels, dfilter, pfilter );
coeffs_2 = nsctdec( I2, nlevels, dfilter, pfilter );

[m,n]=size(I1);
coeffs=coeffs_2;
for i=2:numel(nlevels)+1

    if nlevels(i-1)==0
        E1=abs(coeffs_1{i});
        E2=abs(coeffs_2{i});
        %             map=E1>E2;
         um=3;
    A1 = ordfilt2(abs(es2(E1,floor(um/2))), um*um, ones(um));
  	A2 = ordfilt2(abs(es2(E2,floor(um/2))), um*um, ones(um));
    % second step
  	map= (conv2(double(A1 > A2), ones(um), 'valid')) > floor(um*um/2);
        coeffs{i}(map)=coeffs_1{i}(map);
    else
        for j=1:(2^nlevels(i-1))
            E1=abs(coeffs_1{i}{j});
            E2=abs(coeffs_2{i}{j});
%             map=E1>E2;
    um=3;
    A1 = ordfilt2(abs(es2(E1,floor(um/2))), um*um, ones(um));
  	A2 = ordfilt2(abs(es2(E2,floor(um/2))), um*um, ones(um));
    % second step
  	map= (conv2(double(A1 > A2), ones(um), 'valid')) > floor(um*um/2);
            coeffs{i}{j}(map)=coeffs_1{i}{j}(map);
        end
    end
end

% energy based method

coeffs{1}=(coeffs_1{1}+coeffs_2{1})/2;

y= nsctrec( coeffs, dfilter, pfilter ) ;


function [irf_record,D_record,gamma_record,struct_irf_record,irf_estimates,D_estimates,gamma_estimates,strshocks_record,strshocks_estimates]=...
    panel6irf(y,Xtilde,theta_gibbs,sigma_gibbs,B_gibbs,Xi,It,Bu,IRFperiods,IRFband,IRFt,rho,thetabar,N,n,m,p,T,d,favar)

% create first the cells storing the results
irf_record={};
irf_estimates={};

% deal with shocks in turn (there are N*n shocks with this model)
for ii=1:N*n

   % start iterating
   for jj=1:It-Bu

   % draw theta from its posterior distribution
   theta=theta_gibbs(:,jj,T);

   % draw B from its posterior distribution
   B=reshape(B_gibbs(:,jj),d,d);
   % obtain its Choleski factor: as B is diagonal, it is simply the square roots of its diagonal entries
   cholB=sparse(diag((diag(B).^0.5)));

   % create a matrix of zeros of dimension p*(N*n)
   Ysim=zeros(p,N*n);
   %  step 2: set the value of the last row, column i, equal to 1
   Ysim(p,ii)=1;

      % step 4: for each iteration kk, repeat the algorithm for periods T+1 to T+h
      for kk=1:IRFperiods-1

      % recover theta for period T+kk
      theta=(1-rho)*thetabar+rho*theta+cholB*mvnrnd(zeros(d,1),eye(d))';

      % use the function lagx to obtain a matrix temp, containing the endogenous regressors
      temp=bear.lagx(Ysim,p-1);

      % define the vector X for the period
      Xsim_t=[temp(end,:) zeros(1,m)];

      % obtain Xbar_t et Xtilde_t
      Xbar_t=kron(speye(N*n),Xsim_t);
      Xtilde_t=Xbar_t*Xi;

      % obtain the predicted value for T+kk
      yp=Xtilde_t*theta;

      % concatenate yp at the top of Y
      Ysim=[Ysim;yp'];

      % repeat until values are obtained for T+h
      end

      % record the results from current iteration in cell irf_record
      % loop over variables
      for kk=1:N*n
      % consider column kk of matrix Ysim and trim the (p-1) initial periods: what remains is the series of IRFs for period T to period T+h-1, for variable kk
      temp=Ysim(p:end,kk);
      % record these values in the corresponding matrix of irf_record
      irf_record{kk,ii}(jj,:)=temp';
      end

   % then go for next iteration
   end

% conduct the same process with shocks in other variables
end

% then apply the structural decomposition (if applicable)
% if IRFs have been set to an unrestricted VAR (IRFt=1):
if IRFt==1
% generate empty structural IRFs (just to be consistent with subsequent parts of the code
struct_irf_record=[];
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(irf_record,N*n,IRFperiods,IRFband,IRFt,[],[],favar);
% if IRFs have been set to an SVAR with Choleski identification (IRFt=2):
elseif IRFt==2
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record,D_record,gamma_record]=bear.irfchol(sigma_gibbs(:,:,T),irf_record,It,Bu,IRFperiods,N*n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,N*n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);
% if IRFs have been set to an SVAR with triangular factorisation (IRFt=3):
elseif IRFt==3
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record,D_record,gamma_record]=bear.irftrig(sigma_gibbs(:,:,T),irf_record,It,Bu,IRFperiods,N*n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,N*n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);
end

% record the structural decomposition matrices (the ones obtained from the structural decomposition function only apply to the final period, but incoming functions will require records for all the periods)
% also, if a structural identification was implemented, compute structural shocks
strshocks_record={};
strshocks_estimates={};
% in the case of no structural decomposition
if IRFt==1
% run a pseudo Gibbs sampler to obtain records for D and gamma (for the trivial SVAR)
D_record=repmat(reshape(eye(N*n),(N*n)^2,1),[1 It-Bu T]);
gamma_record=sigma_gibbs;
% obtain estimates
D_estimates=repmat(reshape(eye(N*n),(N*n)^2,1),[1 1 T]);
   for ii=1:T
   gamma_estimates(:,1,ii)=quantile(gamma_record(:,:,ii),0.5,2);
   end
elseif IRFt==2
% run the Gibbs sampler
[strshocks_record,D_record,gamma_record]=bear.strshockspan6(theta_gibbs,sigma_gibbs,y,Xtilde,N,n,T,It,Bu,IRFt);
% obtain point estimates and credibility intervals
[strshocks_estimates]=bear.strsestimates(strshocks_record,N*n,T,IRFband);
% reshape
strshocks_estimates=reshape(strshocks_estimates,[n 1 N]);
% obtain estimates
   for ii=1:T
   D_estimates(:,1,ii)=quantile(D_record(:,:,ii),0.5,2);
   end
gamma_estimates=repmat(reshape(eye(N*n),(N*n)^2,1),[1 1 T]);
elseif IRFt==3
% run the Gibbs sampler
[strshocks_record,D_record,gamma_record]=bear.strshockspan6(theta_gibbs,sigma_gibbs,y,Xtilde,N,n,T,It,Bu,IRFt);
% obtain point estimates and credibility intervals
[strshocks_estimates]=bear.strsestimates(strshocks_record,N*n,T,IRFband);
% reshape
strshocks_estimates=reshape(strshocks_estimates,[n 1 N]);
% obtain estimates
   for ii=1:T
   D_estimates(:,1,ii)=quantile(D_record(:,:,ii),0.5,2);
   gamma_estimates(:,1,ii)=quantile(gamma_record(:,:,ii),0.5,2);
   end
end


function A = sup(A)
%SUP          Supremum of long (only with error term)
%
%  C = sup(A)
%
%For A being a long scalar or column vector with error term (see long),
%  output C is the long upper bound
%

% written  11/06/99     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 09/20/04     S.M. Rump  E.sign fixed
% modified 11/19/04     S.M. Rump  exponent update for error
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 07/25/05     S.M. Rump  result improved, thanks to
%                                      Nozomu Matsuda and Nobito Yamamoto
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  INTLAB_LONG_ERROR = getappdata(0,'INTLAB_LONG_ERROR');

  if ~INTLAB_LONG_ERROR
    error('sup called for long without error term')
  end

  n = length(A.sign);
  E = long(A.error.mant);
  E.exponent = E.exponent + A.error.exp;

  A.error.mant = 0;
  A.error.exp = 0;

  A = A + E;
  index = ( A.error.mant~=0 );
  if any(index)
    %VVVV  A(index) = sup(A(index));
    s.type = '()'; s.subs = {index}; A = subsasgn(A,s,sup(subsref(A,s)));
    %AAAA  Matlab bug fix
  end

  if rndold
    setround(rndold)
  end


function op = tfocs_prox( f, prox_f, VECTOR_SCALAR )
% OP = TFOCS_PROX( F, PROX_F )
%   combines F and PROX_F into the appropriate TFOCS-compatible object.
%
%   F is any function (with known proximity operator),
%   and PROX_F is its proximity operator, defined as:
%
%   PROX_F( Y, t ) = argmin_X  F(X) + 1/(2*t)*|| X - Y ||^2
%
%   To use this, please see the file PROX_L1 as an example
%
%   The basic layout of a file like PROX_L1 is as follows:
%   ( for the function F(X) = q*||X||_1 )
%
%       function op = prox_l1(q)
%       op = tfocs_prox( @f, @prox_f )
%
%         function v = f(x)
%           ... this function calculates the function f ...
%         end
%         function v = prox_f(y,t)
%           ... this function calculates the prox-function to f ...
%         end
%       end
%
%   Note: in the above template, the "end" statements are very important.
%
% OP = TFOCS_PROX( F, PROX_F, 'vector')
%   will signal the routine that it is OK to allow "vector" stepsizes
%   (this corresponds to solving
%       PROX_F( Y, T ) = argmin_X F(X) + 1/2( X-Y )'*diag(1./T)*( X-Y )
%    where T is a vector of the same size as X ).
% ... = TFOCS_PROX( F, PROX_F, 'scalar' )
%   will throw an error if a vector stepsize is attempted. This is the default.
%
%   Also, users may wish to test their smooth function
%   with the script TEST_NONSMOOTH
%
%   See also prox_l1, test_nonsmooth

if nargin < 3 || isempty(VECTOR_SCALAR)
    VECTOR_SCALAR = 'scalar';
end

if strcmpi(VECTOR_SCALAR,'scalar')
    op = @fcn_impl;
elseif strcmpi(VECTOR_SCALAR,'vector')
    op = @fcn_impl_vector;
else
    error('bad option for VECTOR_SCALAR parameter');
end

function [ v, x ] = fcn_impl(x, t )
    if nargin < 1,
        error( 'Not enough arguments.' );
    end
    if nargin == 2,
        if numel(t) ~= 1, error('The stepsize must be a scalar'); end
        x  = prox_f(x,t);
    elseif nargout == 2,
        error( 'This function is not differentiable.' );
    end
    v = f(x);
end

function [ v, x ] = fcn_impl_vector(x, t )
    if nargin < 1,
        error( 'Not enough arguments.' );
    end
    if nargin == 2,
        x  = prox_f(x,t);
    elseif nargout == 2,
        error( 'This function is not differentiable.' );
    end
    v = f(x);
end

end

% TFOCS v1.2 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2012 California Institute of Technology and CVX Research.
% See the file TFOCS/license.{txt,pdf} for full license information.


% SP_H1_ERROR: Evaluate the error in H^1 norm.
%
%   [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)
%
% INPUT:
%
%    space:   object defining the space of discrete functions (see sp_multipatch)
%    msh:     object defining the domain partition and the quadrature rule (see msh_multipatch)
%    u:       vector of dof weights
%    uex:     function handle to evaluate the exact solution
%    graduex: function handle to evaluate the gradient of the exact solution
%
% OUTPUT:
%
%     errh1:  error in H^1 norm
%     errl2:  error in L^2 norm
%     errh1s: error in H^1 seminorm
%
% Copyright (C) 2015 Rafael Vazquez
%
% 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

function [errh1, errl2, errh1s] = sp_h1_error (space, msh, u, uex, graduex)

  if (space.npatch ~= msh.npatch)
    error ('The number of patches does not coincide')
  end

  if (numel(u) ~= space.ndof)
    error ('Wrong size of the vector of degrees of freedom')
  end

  for iptc = 1:msh.npatch
    if (isempty (space.dofs_ornt))
      u_ptc = u(space.gnum{iptc});
    else
      u_ptc = u(space.gnum{iptc}) .* space.dofs_ornt{iptc}.';
    end
    [error_h1(iptc), error_l2(iptc), error_h1s(iptc)] = ...
        sp_h1_error (space.sp_patch{iptc}, msh.msh_patch{iptc}, u_ptc, uex, graduex);
  end
  errl2 = sqrt (sum (error_l2 .* error_l2));
  errh1 = sqrt (sum (error_h1 .* error_h1));
  errh1s = sqrt (sum (error_h1s .* error_h1s));
end


classdef denoising_mppca < AbstractModel
% denoising_mppca :  4d image denoising and noise map estimation
%                      by exploiting data redundancy in the PCA domain using universal
%                      properties of the eigenspectrum or random covariance matrices,
%                      i.e. Marchenko Pastur distribution
%
% Assumptions:
%   Noise follows a rician distribution
%   image bounderies are not processed
%
% Inputs:
%   Data4D              4D data (any modality)
%  (Mask)               Binary mask with region-of-interest. (OPTIONAL)
%
% Outputs:
%   Data4D_denoised     denoised 4D data
%   sigma_g               standard deviation of the rician noise
%
% Options:
%	sampling
%   	'full'          sliding window
%       'fast'          block processing (warning: undersampled noise map will be returned)
%   kernel              window size, typically in order of [5 x 5 x 5]
%
% Example of command line usage:
%   Model = denoising_mppca;  % Create class from model
%   data.Data4D = load_nii_data('Data4D.nii.gz');  % Load data
%   FitResults = FitData(data,Model,1);  % Fit each voxel within mask
%   FitResultsSave_nii(FitResults,'Data4D.nii.gz');  % Save in local folder: FitResults/
%
% Author: Tanguy Duval, 2016
%
% References:
%   Please cite the following if you use this module:
%     Veraart, J.; Fieremans, E. & Novikov, D.S. Diffusion MRI noise mapping using random matrix theory Magn. Res. Med., 2016, early view, doi:10.1002/mrm.26059
%   In addition to citing the package:
%     Karakuzu A., Boudreau M., Duval T.,Boshkovski T., Leppert I.R., Cabana J.F.,
%     Gagnon I., Beliveau P., Pike G.B., Cohen-Adad J., Stikov N. (2020), qMRLab:
%     Quantitative MRI analysis, under one umbrella doi: 10.21105/joss.02343

    properties (Hidden=true)
        onlineData_url = 'https://osf.io/j5239/download?version=2';
    end

    properties
        MRIinputs = {'Data4D','Mask'};
        xnames = {};
        voxelwise = 0;

        % Protocol
        Prot  = struct(); % You can define a default protocol here.

        % Model options
        buttons = {'sampling',{'fast','full'},'kernel',[5 5 5]};
        options= struct(); % structure filled by the buttons. Leave empty in the code

    end

    methods

        function obj = denoising_mppca
            obj.options = button2opts(obj.buttons);
            obj = UpdateFields(obj);
        end

        function obj = UpdateFields(obj)
        end

        function FitResults = fit(obj,data)
            dims=size(data.Data4D);
            kernel = min(dims(1:3),obj.options.kernel);
            [V,Ind] = min(dims(1:3));
            if ~isfield(data,'Mask'), data.Mask = []; end
            if V<7 && kernel(Ind)>1, helpdlg(['your dataset has very few slices. To avoid loosing too many slices, kernel is set to 1 in the dimension #' num2str(Ind) '.']); kernel(Ind)=1; end
            [FitResults.Data4D_denoised, FitResults.sigma_g] = MPdenoising(data.Data4D,data.Mask,kernel, obj.options.sampling);
        end

    end

    methods(Access = protected)
        function obj = qMRpatch(obj,loadedStruct, version)
            obj = qMRpatch@AbstractModel(obj,loadedStruct, version);
            if checkanteriorver(version,[2 0 11])
                obj.buttons{2} = obj.buttons{2}([2 1]); % old: '|G| (T/m)', new Gnorm (T/m)
            end
        end
    end

end


function lik = lik_negbinztr(varargin)
%LIK_NEGBINZTR  Create a zero-truncated Negative-binomial likelihood structure
%
%  Description
%    LIK = LIK_NEGBINZTR('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates zero-truncated Negative-binomial likelihood structure
%    in which the named parameters have the specified values. Any
%    unspecified parameters are set to default values.
%
%    Zero-truncated Negative-binomial can be used as a part of Hurdle model.
%
%    LIK = LIK_NEGBINZTR(LIK,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a likelihood structure with the named parameters
%    altered with the specified values.
%
%    Parameters for zero-truncated Negative-binomial likelihood [default]
%      disper       - dispersion parameter r [10]
%      disper_prior - prior for disper [prior_logunif]
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%    The likelihood is defined as follows:
%                  __ n
%      p(y|f, z) = || i=1 [ (r/(r+mu_i))^r * gamma(r+y_i)
%                           / ( gamma(r)*gamma(y_i+1) )
%                           * (mu/(r+mu_i))^y_i / (1-(r/(r+mu_i))^r)]
%
%    where mu_i = z_i*exp(f_i) and r is the dispersion parameter. z
%    is a vector of expected mean and f the latent value vector
%    whose components are transformed to relative risk exp(f_i).
%    The last term (1-(r/(r+mu_i))^r) normalizes the truncated
%    distribution.
%
%    When using the zero-truncated Negbin likelihood you can give the
%    vector z as an extra parameter to each function that requires
%    also y. For example, you can call gp_optim as follows:
%      gp_optim(gp, x, y, 'z', z)
%    If z is not given or it is empty, then z_i=1 is used.
%
%  See also
%    GP_SET, LIK_*, PRIOR_*
%
% Copyright (c) 2007-2010 Jarno Vanhatalo & Jouni Hartikainen
% Copyright (c) 2010-2011 Aki Vehtari

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

  ip=inputParser;
  ip.FunctionName = 'LIK_NEGBINZTR';
  ip.addOptional('lik', [], @isstruct);
  ip.addParamValue('disper',10, @(x) isscalar(x) && x>0);
  ip.addParamValue('disper_prior',prior_logunif(), @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Negbinztr';
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Negbinztr')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('disper',ip.UsingDefaults)
    lik.disper = ip.Results.disper;
  end
  % Initialize prior structure
  if init
    lik.p=[];
  end
  if init || ~ismember('disper_prior',ip.UsingDefaults)
    lik.p.disper=ip.Results.disper_prior;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_negbinztr_pak;
    lik.fh.unpak = @lik_negbinztr_unpak;
    lik.fh.lp = @lik_negbinztr_lp;
    lik.fh.lpg = @lik_negbinztr_lpg;
    lik.fh.ll = @lik_negbinztr_ll;
    lik.fh.llg = @lik_negbinztr_llg;
    lik.fh.llg2 = @lik_negbinztr_llg2;
    lik.fh.llg3 = @lik_negbinztr_llg3;
    lik.fh.tiltedMoments = @lik_negbinztr_tiltedMoments;
    lik.fh.tiltedMoments2 = @lik_negbinztr_tiltedMoments2;
    lik.fh.siteDeriv = @lik_negbinztr_siteDeriv;
    lik.fh.siteDeriv2 = @lik_negbinztr_siteDeriv2;
    lik.fh.upfact = @lik_negbinztr_upfact;
    lik.fh.predy = @lik_negbinztr_predy;
    lik.fh.predprcty = @lik_negbinztr_predprcty;
    lik.fh.invlink = @lik_negbinztr_invlink;
    lik.fh.recappend = @lik_negbinztr_recappend;
  end

end

function [w,s,h] = lik_negbinztr_pak(lik)
%LIK_NEGBINZTR_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_NEGBINZTR_PAK(LIK) takes a likelihood structure LIK and
%    combines the parameters into a single row vector W. This is a
%    mandatory subfunction used for example in energy and gradient
%    computations.
%
%       w = log(lik.disper)
%
%   See also
%   LIK_NEGBINZTR_UNPAK, GP_PAK

  w=[];s={};h=[];
  if ~isempty(lik.p.disper)
    w = log(lik.disper);
    s = [s; 'log(negbinztr.disper)'];
    h = [h 0];
    [wh,sh,hh] = lik.p.disper.fh.pak(lik.p.disper);
    w = [w wh];
    s = [s; sh];
    h = [h hh];
  end
end

function [lik, w] = lik_negbinztr_unpak(lik, w)
%LIK_NEGBINZTR_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    [LIK, W] = LIK_NEGBINZTR_UNPAK(W, LIK) takes a likelihood
%    structure LIK and extracts the parameters from the vector W
%    to the LIK structure. This is a mandatory subfunction used
%    for example in energy and gradient computations.
%
%   Assignment is inverse of
%       w = log(lik.disper)
%
%   See also
%   LIK_NEGBINZTR_PAK, GP_UNPAK

  if ~isempty(lik.p.disper)
    lik.disper = exp(w(1));
    w = w(2:end);
    [p, w] = lik.p.disper.fh.unpak(lik.p.disper, w);
    lik.p.disper = p;
  end
end

function lp = lik_negbinztr_lp(lik, varargin)
%LIK_NEGBINZTR_LP  log(prior) of the likelihood parameters
%
%  Description
%    LP = LIK_NEGBINZTR_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters. This subfunction
%    is needed when there are likelihood parameters.
%
%  See also
%    LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_E

% If prior for dispersion parameter, add its contribution
  lp=0;
  if ~isempty(lik.p.disper)
    lp = lik.p.disper.fh.lp(lik.disper, lik.p.disper) +log(lik.disper);
  end

end

function lpg = lik_negbinztr_lpg(lik)
%LIK_NEGBINZTR_LPG  d log(prior)/dth of the likelihood
%                parameters th
%
%  Description
%    E = LIK_NEGBINZTR_LPG(LIK) takes a likelihood structure LIK and
%    returns d log(p(th))/dth, where th collects the parameters. This
%    subfunction is needed when there are likelihood parameters.
%
%  See also
%    LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_G

  lpg=[];
  if ~isempty(lik.p.disper)
    % Evaluate the gprior with respect to disper
    ggs = lik.p.disper.fh.lpg(lik.disper, lik.p.disper);
    lpg = ggs(1).*lik.disper + 1;
    if length(ggs) > 1
      lpg = [lpg ggs(2:end)];
    end
  end
end

function ll = lik_negbinztr_ll(lik, y, f, z)
%LIK_NEGBINZTR_LL  Log likelihood
%
%  Description
%    LL = LIK_NEGBINZTR_LL(LIK, Y, F, Z) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z, and
%    latent values F. Returns the log likelihood, log p(y|f,z).
%    This subfunction is needed when using Laplace approximation
%    or MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria (DIC, WAIC)
%    computations.
%
%  See also
%    LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, LIK_NEGBINZTR_LLG2, GPLA_E

  if numel(z)==0
    z=1;
  end

  r = lik.disper;
  mu = exp(f).*z;
  lp0=r.*(log(r) - log(r+mu));
  ll = sum(r.*(log(r) - log(r+mu)) + gammaln(r+y) - gammaln(r) - gammaln(y+1) + y.*(log(mu) - log(r+mu)) -log(1-exp(lp0)));
end

function llg = lik_negbinztr_llg(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG  Gradient of the log likelihood
%
%  Description
%    LLG = LIK_NEGBINZTR_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z and
%    latent values F. Returns the gradient of the log likelihood
%    with respect to PARAM. At the moment PARAM can be 'param' or
%    'latent'. This subfunction is needed when using Laplace
%    approximation or MCMC for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_NEGBINZTR_LL, LIK_NEGBINZTR_LLG2, LIK_NEGBINZTR_LLG3, GPLA_E

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'
      % Derivative using the psi function
      llg = sum(1 + log(r./(r+mu)) - (r+y)./(r+mu) + psi(r + y) - psi(r));
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg=llg-sum(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
      % correction for the log transformation
      llg = llg.*lik.disper;

% $$$             % Derivative using sum formulation
% $$$             llg = 0;
% $$$             for i1 = 1:length(y)
% $$$                 llg = llg + log(r/(r+mu(i1))) + 1 - (r+y(i1))/(r+mu(i1));
% $$$                 for i2 = 0:y(i1)-1
% $$$                     llg = llg + 1 / (i2 + r);
% $$$                 end
% $$$             end
% $$$             % correction for the log transformation
% $$$             llg = llg.*lik.disper;
    case 'latent'
      llg = y - (r+y).*mu./(r+mu);
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg = llg -(1./(1-exp(-lp0)).*-r./(mu + r).*mu);
  end
end

function llg2 = lik_negbinztr_llg2(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_NEGBINZTR_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z, and
%    latent values F. Returns the Hessian of the log likelihood
%    with respect to PARAM. At the moment PARAM can be only
%    'latent'. LLG2 is a vector with diagonal elements of the
%    Hessian matrix (off diagonals are zero). This subfunction
%    is needed when using Laplace approximation or EP for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_NEGBINZTR_LL, LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG3, GPLA_E

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'

    case 'latent'
      llg2 = - mu.*(r.^2 + y.*r)./(r+mu).^2;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg2=llg2+...
           (r.^2 + r.^2.*exp(-lp0).*(mu-1))./((mu + r).^2.*(exp(-lp0)-1).^2).*mu;
    case 'latent+param'
      llg2 = (y.*mu - mu.^2)./(r+mu).^2;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg2=llg2+(exp(lp0)./(exp(lp0) - 1).^2 .* (log(r) - log(mu + r) - r.*(1./(mu + r) - 1./r)) .* (-r./(mu + r)) -1./(1 - exp(-lp0)).*-mu./(mu + r).^2).*mu;

      % correction due to the log transformation
      llg2 = llg2.*lik.disper;

  end
end

function llg3 = lik_negbinztr_llg3(lik, y, f, param, z)
%LIK_NEGBINZTR_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_NEGBINZTR_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, incedence counts Y, expected counts Z and
%    latent values F and returns the third gradients of the log
%    likelihood with respect to PARAM. At the moment PARAM can be
%    only 'latent'. LLG3 is a vector with third gradients.  This
%    subfunction is needed when using Laplace approximation for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_NEGBINZTR_LL, LIK_NEGBINZTR_LLG, LIK_NEGBINZTR_LLG2, GPLA_E, GPLA_G

  if numel(z)==0
    z=1;
  end

  mu = exp(f).*z;
  r = lik.disper;
  switch param
    case 'param'

    case 'latent'
      llg3 = - mu.*(r.^2 + y.*r)./(r + mu).^2 + 2.*mu.^2.*(r.^2 + y.*r)./(r + mu).^3;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      llg3=llg3+ ...
           (exp(lp0).*(r.^2.*(r + r.*exp(2.*lp0)) + mu.^2.*r.^3 - mu.*r.^2.*(3.*r + exp(2.*lp0) + 1)) + exp(2.*lp0).*(mu.^2.*r.^3 - 2.*r.^3 + mu.*r.^2.*(3.*r + 2)))./((exp(lp0) - 1).^3.*(mu + r).^3).*mu;
    case 'latent2+param'
      llg3 = mu.*(y.*r - 2.*r.*mu - mu.*y)./(r+mu).^3;
      % add gradient of the normalization due to the truncation
      lp0=r.*(log(r) - log(r+mu));
      ip0=exp(-lp0);
      llg3=llg3+ ...
           (mu.*(2.*r + 2.*r.*ip0.*(mu - 1) + r.^2.*ip0.*(mu - 1).*(log(mu + r) - log(r) + r./(mu + r) - 1)))./((mu + r).^2.*(ip0 - 1).^2) - (2.*mu.*(r.^2 + r.^2.*ip0.*(mu - 1)))./((mu + r).^3.*(ip0 - 1).^2) - (2.*mu.*ip0.*(r.^2 + r.^2.*ip0.*(mu - 1)).*(log(mu + r) - log(r) + r./(mu + r) - 1))./((mu + r).^2.*(ip0 - 1).^3);
      % correction due to the log transformation
      llg3 = llg3.*lik.disper;
  end
end

function [logM_0, m_1, sigm2hati1] = lik_negbinztr_tiltedMoments(lik, y, i1, sigma2_i, myy_i, z)
%LIK_NEGBINZTR_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_NEGBINZTR_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, incedence counts
%    Y, expected counts Z, index I and cavity variance S2 and
%    mean MYY. Returns the zeroth moment M_0, mean M_1 and
%    variance M_2 of the posterior marginal (see Rasmussen and
%    Williams (2006): Gaussian processes for Machine Learning,
%    page 55). This subfunction is needed when using EP for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    if isscalar(sigma2_i)
      sigma2ii = sigma2_i;
    else
      sigma2ii = sigma2_i(i);
    end

    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Negative-binomial * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_negbinztr_norm(yy(i),myy_i(i),sigma2ii,avgE(i),r,1);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    if isnan(m_0)
      logM_0=NaN;
      return
    end
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigma2ii
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigma2ii
        warning('lik_negbinztr_tilted_moments: sigm2hati1 >= sigm2_i');
        %sigm2hati1=sigm2_i-1e-9;
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [logM_0, m_1, sigm2hati1] = lik_negbinztr_tiltedMoments2(lik, y, i1, sigm2_i, myy_i, z, eta)
%LIK_NEGBINZTR_TILTEDMOMENTS  Returns the marginal moments for EP algorithm
%
%  Description
%    [M_0, M_1, M2] = LIK_NEGBINZTR_TILTEDMOMENTS(LIK, Y, I, S2,
%    MYY, Z) takes a likelihood structure LIK, incedence counts
%    Y, expected counts Z, index I and cavity variance S2 and
%    mean MYY. Returns the zeroth moment M_0, mean M_1 and
%    variance M_2 of the posterior marginal (see Rasmussen and
%    Williams (2006): Gaussian processes for Machine Learning,
%    page 55). This subfunction is needed when using EP for
%    inference with non-Gaussian likelihoods.
%
%  See also
%    GPEP_E

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;
  logM_0=zeros(size(yy));
  m_1=zeros(size(yy));
  sigm2hati1=zeros(size(yy));

  for i=1:length(i1)
    % get a function handle of an unnormalized tilted distribution
    % (likelihood * cavity = Negative-binomial * Gaussian)
    % and useful integration limits
    [tf,minf,maxf]=init_negbinztr_norm(yy(i),myy_i(i),sigm2_i(i),avgE(i),r,eta);

    % Integrate with quadrature
    RTOL = 1.e-6;
    ATOL = 1.e-10;
    [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
    sigm2hati1(i) = m_2 - m_1(i).^2;

    % If the second central moment is less than cavity variance
    % integrate more precisely. Theoretically for log-concave
    % likelihood should be sigm2hati1 < sigm2_i.
    if sigm2hati1(i) >= sigm2_i(i)
      ATOL = ATOL.^2;
      RTOL = RTOL.^2;
      [m_0, m_1(i), m_2] = quad_moments(tf, minf, maxf, RTOL, ATOL);
      sigm2hati1(i) = m_2 - m_1(i).^2;
      if sigm2hati1(i) >= sigm2_i(i)
        warning('lik_negbinztr_tilted_moments: sigm2hati1 >= sigm2_i');
        %sigm2hati1=sigm2_i-1e-9;
      end
    end
    logM_0(i) = log(m_0);
  end
end

function [g_i] = lik_negbinztr_siteDeriv(lik, y, i1, sigm2_i, myy_i, z)
%LIK_NEGBINZTR_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_NEGBINZTR_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, incedence counts Y, expected
%    counts Z, index I and cavity variance S2 and mean MYY.
%    Returns E_f [d log p(y_i|f_i) /d a], where a is the
%    likelihood parameter and the expectation is over the
%    marginal posterior. This term is needed when evaluating the
%    gradients of the marginal likelihood estimate Z_EP with
%    respect to the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Negative-binomial * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, 1);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*r;

  function g = deriv(f)
    mu = avgE.*exp(f);
    % Derivative using the psi function
    g = 1 + log(r./(r+mu)) - (r+yy)./(r+mu) + psi(r + yy) - psi(r);
    lp0=r.*(log(r) - log(r+mu));
    g = g -(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
  end
end

function [g_i] = lik_negbinztr_siteDeriv2(lik, y, i1, sigm2_i, myy_i, z, eta, lnZhat)
%LIK_NEGBINZTR_SITEDERIV  Evaluate the expectation of the gradient
%                      of the log likelihood term with respect
%                      to the likelihood parameters for EP
%
%  Description [M_0, M_1, M2] =
%    LIK_NEGBINZTR_SITEDERIV(LIK, Y, I, S2, MYY, Z) takes a
%    likelihood structure LIK, incedence counts Y, expected
%    counts Z, index I and cavity variance S2 and mean MYY.
%    Returns E_f [d log p(y_i|f_i) /d a], where a is the
%    likelihood parameter and the expectation is over the
%    marginal posterior. This term is needed when evaluating the
%    gradients of the marginal likelihood estimate Z_EP with
%    respect to the likelihood parameters (see Seeger (2008):
%    Expectation propagation for exponential families). This
%    subfunction is needed when using EP for inference with
%    non-Gaussian likelihoods and there are likelihood parameters.
%
%  See also
%    GPEP_G

  if numel(z)==0
    avgE = ones(size(i1));
  else
    avgE = z(i1);
  end

  yy = y(i1);
  r = lik.disper;

  % get a function handle of an unnormalized tilted distribution
  % (likelihood * cavity = Negative-binomial * Gaussian)
  % and useful integration limits
  [tf,minf,maxf]=init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, eta);
  % additionally get function handle for the derivative
  td = @deriv;

  % Integrate with quadgk
  [m_0, fhncnt] = quadgk(tf, minf, maxf);
  [g_i, fhncnt] = quadgk(@(f) td(f).*tf(f)./m_0, minf, maxf);
  g_i = g_i.*r;

  function g = deriv(f)
    mu = avgE.*exp(f);
    % Derivative using the psi function
    g = 1 + log(r./(r+mu)) - (r+yy)./(r+mu) + psi(r + yy) - psi(r);
    lp0=r.*(log(r) - log(r+mu));
    g = g -(1./(1 - exp(-lp0)).*(log(r./(mu + r)) - r./(mu + r) + 1));
%     g = eta.*g;
  end
end

function upfact = lik_negbinztr_upfact(gp, y, mu, ll, z)
  r = gp.lik.disper;
  sll = sqrt(ll);

  fh_e = @(f) negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll);
  EE = quadgk(fh_e, max(mu-6*sll,-30), min(mu+6*sll,30));

  fm = @(f) f.*negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll)./EE;
  mm  = quadgk(fm, max(mu-6*sll,-30), min(mu+6*sll,30));

  fV = @(f) (f - mm).^2.*negbinztr_pdf(y, exp(f).*z', r).*norm_pdf(f, mu, sll)./EE;
  Varp = quadgk(fV, max(mu-6*sll,-30), min(mu+6*sll,30));

  upfact = -(Varp - ll)./ll^2;
end

function [lpy, Ey, Vary] = lik_negbinztr_predy(lik, Ef, Varf, yt, zt)
%LIK_NEGBINZTR_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_NEGBINZTR_PREDY(LIK, EF, VARF YT, ZT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | zt) = \int p(yt | f, zt) p(f|y) df.
%    This requires also the incedence counts YT, expected counts ZT.
%    This subfunction is needed when computing posterior predictive
%    distributions for future observations.
%
%    [LPY, EY, VARY] = LIK_NEGBINZTR_PREDY(LIK, EF, VARF) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  if numel(zt)==0
    zt=ones(size(Ef));
  end

  avgE = zt;
  r = lik.disper;

  lpy = zeros(size(Ef));
  Ey = zeros(size(Ef));
  EVary = zeros(size(Ef));
  VarEy = zeros(size(Ef));

  if nargout > 1
      % Evaluate Ey and Vary
      for i1=1:length(Ef)
        %%% With quadrature
        myy_i = Ef(i1);
        sigm_i = sqrt(Varf(i1));
        minf=myy_i-6*sigm_i;
        maxf=myy_i+6*sigm_i;

        F = @(f) exp(log(avgE(i1))+f+norm_lpdf(f,myy_i,sigm_i));
        Ey(i1) = quadgk(F,minf,maxf);

        F2 = @(f) exp(log(avgE(i1).*exp(f)+((avgE(i1).*exp(f)).^2/r))+norm_lpdf(f,myy_i,sigm_i));
        EVary(i1) = quadgk(F2,minf,maxf);

        F3 = @(f) exp(2*log(avgE(i1))+2*f+norm_lpdf(f,myy_i,sigm_i));
        VarEy(i1) = quadgk(F3,minf,maxf) - Ey(i1).^2;
      end
      Vary = EVary + VarEy;
  end

  % Evaluate the posterior predictive densities of the given observations
  lpy = zeros(length(yt),1);
  if (size(Ef,2) > 1) && (size(Ef,2) > 1) && size(yt,2) == 1
    % Approximate integral with sum of grid points when using corrected
    % marginal posterior pf
    for i1=1:length(yt)
      py = arrayfun(@(f) exp(lik.fh.ll(lik, yt(i1), f, zt(i1))), Ef(i1,:));
      pf = Varf(i1,:)./sum(Varf(i1,:));
      lpy(i1) = log(sum(py.*pf));
    end
  else
    for i1=1:length(yt)
      % get a function handle of the likelihood times posterior
      % (likelihood * posterior = Negative-binomial * Gaussian)
      % and useful integration limits
      [pdf,minf,maxf]=init_negbinztr_norm(...
        yt(i1),Ef(i1),Varf(i1),avgE(i1),r);
      % integrate over the f to get posterior predictive distribution
      lpy(i1) = log(quadgk(pdf, minf, maxf));
    end
  end
end

function [df,minf,maxf] = init_negbinztr_norm(yy,myy_i,sigm2_i,avgE,r, eta)
%INIT_NEGBINZTR_NORM
%
%  Description
%    Return function handle to a function evaluating
%    Negative-Binomial * Gaussian which is used for evaluating
%    (likelihood * cavity) or (likelihood * posterior) Return
%    also useful limits for integration. This is private function
%    for lik_negbinztr. This subfunction is needed by subfunctions
%    tiltedMoments, siteDeriv and predy.
%
%  See also
%    LIK_NEGBINZTR_TILTEDMOMENTS, LIK_NEGBINZTR_SITEDERIV,
%    LIK_NEGBINZTR_PREDY

% avoid repetitive evaluation of constant part
  ldconst = eta*(-gammaln(r)-gammaln(yy+1)+gammaln(r+yy))...
            - log(sigm2_i)/2 - log(2*pi)/2;
  % Create function handle for the function to be integrated
  df = @negbinztr_norm;
  % use log to avoid underflow, and derivates for faster search
  ld = @log_negbinztr_norm;
  ldg = @log_negbinztr_norm_g;
  ldg2 = @log_negbinztr_norm_g2;

  % Set the limits for integration
  % Negative-binomial likelihood is log-concave so the negbinztr_norm
  % function is unimodal, which makes things easier
  if yy==0
    % with yy==0, the mode of the likelihood is not defined
    % use the mode of the Gaussian (cavity or posterior) as a first guess
    modef = myy_i;
  else
    % use precision weighted mean of the Gaussian approximation
    % of the Negative-Binomial likelihood and Gaussian
    mu=log(yy/avgE);
    s2=(yy+r)./(yy.*r);
    modef = (myy_i/sigm2_i + mu/s2)/(1/sigm2_i + 1/s2);
  end
  % find the mode of the integrand using Newton iterations
  % few iterations is enough, since the first guess in the right direction
  niter=4;       % number of Newton iterations
  mindelta=1e-6; % tolerance in stopping Newton iterations
  for ni=1:niter
    g=ldg(modef);
    h=ldg2(modef);
    delta=-g/h;
    modef=modef+delta;
    if abs(delta)<mindelta
      break
    end
  end
  % integrand limits based on Gaussian approximation at mode
  modes=sqrt(-1/h);
  minf=modef-8*modes;
  maxf=modef+8*modes;
  modeld=ld(modef);
  iter=0;
  % check that density at end points is low enough
  lddiff=20; % min difference in log-density between mode and end-points
  minld=ld(minf);
  step=1;
  while minld<(modeld-lddiff) && minf<modef;
    % sometimes minf is too small
    minf=minf+step*modes;
    minld=ld(minf);
  end
  while minld>(modeld-lddiff)
    minf=minf-step*modes;
    minld=ld(minf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbinztr -> init_negbinztr_norm: ' ...
             'integration interval minimun not found ' ...
             'even after looking hard!'])
    end
  end
  maxld=ld(maxf);
  iter=0;
  step=1;
  while maxld>(modeld-lddiff)
    maxf=maxf+step*modes;
    maxld=ld(maxf);
    iter=iter+1;
    step=step*2;
    if iter>100
      error(['lik_negbinztr -> init_negbinztr_norm: ' ...
             'integration interval maximum not found ' ...
             'even after looking hard!'])
    end
  end

  function integrand = negbinztr_norm(f)
  % Negative-binomial * Gaussian
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      if yy==1
        integrand = exp(eta*(log(avgE)+f)...
                        -0.5*(f-myy_i).^2./sigm2_i -log(sigm2_i)/2 -log(2*pi)/2);
      else
        integrand = 0;
      end
    else
      integrand = exp(ldconst ...
                      +eta*(yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu))) ...
                      -0.5*(f-myy_i).^2./sigm2_i ...
                      -eta*log(1-exp(lp0)));
    end
  end

  function log_int = log_negbinztr_norm(f)
  % log(Negative-binomial * Gaussian)
  % log_negbinztr_norm is used to avoid underflow when searching
  % integration interval
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      if yy==1
        log_int = eta*(log(avgE)+f) ...
                  -0.5*(f-myy_i).^2./sigm2_i - log(sigm2_i)/2 - log(2*pi)/2;
      else
        log_int=-Inf;
      end
    else
      log_int = ldconst...
                +eta*(yy.*(log(mu)-log(r+mu))+r.*(log(r)-log(r+mu)) -gammaln(r)-gammaln(yy+1)+gammaln(r+yy)) ...
                -0.5*(f-myy_i).^2./sigm2_i ...
                -eta*log(1-exp(lp0));
    end
  end

  function g = log_negbinztr_norm_g(f)
  % d/df log(Negative-binomial * Gaussian)
  % derivative of log_negbinztr_norm
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      g = eta+(myy_i - f)./sigm2_i;
    else
      g = -eta*(r.*(mu - yy))./(mu.*(mu + r)).*mu ...
          + (myy_i - f)./sigm2_i ...
          -eta*(1/(1 - exp(-lp0))*-r/(mu + r)*mu);
    end
  end

  function g2 = log_negbinztr_norm_g2(f)
  % d^2/df^2 log(Negative-binomial * Gaussian)
  % second derivate of log_negbinztr_norm
    mu = avgE.*exp(f);
    lp0=r.*(log(r) - log(r+mu));
    if lp0==0
      % exp(lp0)->1, that is, almost all the mass is in the zero part
      % approximate if yy=1, and give up if yy>1
      g2 = -1/sigm2_i;
    else
      g2 = -eta*(r*(r + yy))/(mu + r)^2.*mu ...
           -1/sigm2_i ...
           + eta*(r^2 + r^2*exp(-lp0)*(mu - 1))/((mu + r)^2*(exp(-lp0) - 1)^2)*mu;
    end
  end

end

function prctys = lik_negbinztr_predprcty(lik, Ef, Varf, zt, prcty)
%LIK_BINOMIAL_PREDPRCTY  Returns the percentiled of predictive density of y
%
%  Description
%    PRCTY = LIK_BINOMIAL_PREDPRCTY(LIK, EF, VARF YT, ZT)
%    Returns percentiles of the predictive density PY of YT, that is
%    This requires also the succes counts YT, numbers of trials ZT.
%    This subfunction is needed when using function gp_predprcty.
%
%  See also
%    GP_PREDPCTY

  if numel(zt)==0
    zt=ones(size(Ef));
  end

  opt=optimset('TolX',1e-7,'Display','off');
  nt=size(Ef,1);
  prctys = zeros(nt,numel(prcty));
  prcty=prcty/100;
  r = lik.disper;
  mu = zt.*exp(Ef);
  for i1=1:nt
    ci = sqrt(Varf(i1));
    for i2=1:numel(prcty)
      minf = floor(fminbnd(@(b) (quadgk(@(y) llvec(lik,y,Ef(i1)-1.96*ci,zt(i1)), 0, b)-prcty(i2)).^2,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))-5,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))+5,opt));
      if minf<0
        minf = 0;
      end
      maxf = floor(fminbnd(@(b) (quadgk(@(y) llvec(lik,y,Ef(i1)+1.96*ci,zt(i1)), 0, b)-prcty(i2)).^2,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))-5,nbininv(prcty(i2), r, r./(r+zt(i1).*exp(Ef(i1))))+5,opt));
      if maxf<0
        maxf = 0;
      end
%       j=0;
%       figure;
%       for a=-2:0.1:50
%         j=j+1;
%         testi(j) = (quadgk(@(f) quadgk(@(y) llvec(lik,y,Ef(i1),zt(i1)), 0, a).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2;
%       end
%       plot(-2:0.1:50,testi)
%       if minf<maxf
%         set(gca,'XTick',[minf maxf])
%       else
%         set(gca,'XTick',[minf minf+1])
%       end
%       hold on;
%       a=floor(fminbnd(@(a) (quadgk(@(f) quadgk(@(y) llvec(lik,y,Ef(i1),zt(i1)), 0, a) ...
%              .*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2, minf, maxf,opt));
      a=floor(fminbnd(@(a) (quadgk(@(f) sum(llvec(lik,0:1:a,Ef(i1),zt(i1))) ...
             .*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4)-prcty(i2)).^2, minf, maxf,opt));
      if quadgk(@(f) sum(llvec(lik,0:1:a,Ef(i1),zt(i1))).*norm_pdf(f,Ef(i1),ci),Ef(i1)-6*ci,Ef(i1)+6*ci,'AbsTol',1e-4) < prcty(i2)
        a=a+1;
      end
      prctys(i1,i2)=a;
    end
  end

  function expll = llvec(lik,yt,f,z)
    % Compute vector of likelihoods of single predictions
    n = length(yt);
    if n>0
      for i=1:n
        expll(i) = exp(lik.fh.ll(lik, yt(i), f, z));
      end
    else
      expll = 0;
    end
  end

end

function mu = lik_negbinztr_invlink(lik, f, z)
%LIK_NEGBINZTR_INVLINK  Returns values of inverse link function
%
%  Description
%    MU = LIK_NEGBINZTR_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values MU of inverse link function.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_NEGBINZTR_LL, LIK_NEGBINZTR_PREDY

  if numel(z)==0
    z=1;
  end
  mu = bsxfun(@times,z,exp(f));
end

function reclik = lik_negbinztr_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = GPCF_NEGBINZTR_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK.  This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    % Initialize the record
    reclik.type = 'Negbinztr';

    % Initialize parameter
    reclik.disper = [];

    % Set the function handles
    reclik.fh.pak = @lik_negbinztr_pak;
    reclik.fh.unpak = @lik_negbinztr_unpak;
    reclik.fh.lp = @lik_negbinztr_lp;
    reclik.fh.lpg = @lik_negbinztr_lpg;
    reclik.fh.ll = @lik_negbinztr_ll;
    reclik.fh.llg = @lik_negbinztr_llg;
    reclik.fh.llg2 = @lik_negbinztr_llg2;
    reclik.fh.llg3 = @lik_negbinztr_llg3;
    reclik.fh.tiltedMoments = @lik_negbinztr_tiltedMoments;
    reclik.fh.tiltedMoments2 = @lik_negbinztr_tiltedMoments2;
    reclik.fh.siteDeriv = @lik_negbinztr_siteDeriv;
    reclik.fh.siteDeriv2 = @lik_negbinztr_siteDeriv2;
    reclik.fh.predy = @lik_negbinztr_predy;
    reclik.fh.predprcty = @lik_negbinztr_predprcty;
    reclik.fh.invlink = @lik_negbinztr_invlink;
    reclik.fh.recappend = @lik_negbinztr_recappend;
    reclik.p=[];
    reclik.p.disper=[];
    if ~isempty(ri.p.disper)
      reclik.p.disper = ri.p.disper;
    end
  else

    % Append to the record
    reclik.disper(ri,:)=lik.disper;
    if ~isempty(lik.p.disper)
      reclik.p.disper = lik.p.disper.fh.recappend(reclik.p.disper, ri, lik.p.disper);
    end
  end
end


function q_out=qmult(q1,q2)
% QMULT(Q1,Q2) calculates the product of two quaternions Q1 and Q2.
%    Inputs can be vectors of quaternions, but they must either have the
%    same number of component quaternions, or one input must be a single
%    quaternion.  QMULT will determine whether the component quaternions of
%    the inputs are row or column vectors according to ISQ.
%
%    The output will have the same shape as Q1.  If the component
%    quaternions of either Q1 or Q2 (but not both) are of indeterminate
%    shape (see ISQ), then the shapes will be assumed to be the same for
%    both inputs.  If both Q1 and Q2 are of indeterminate shape, then both
%    are assumed to be composed of row vector quaternions.
%
% See also ISQ.

% Release: $Name: quaternions-1_2_2 $
% $Revision: 1.12 $
% $Date: 2001/05/01 20:20:31 $

% Copyright (C) 2001, Jay A. St. Pierre.  All rights reserved.

if nargin~=2
  error('qmult() requires two input arguments');
else
  q1type = isq(q1);
  if ( q1type == 0 )
    error(['Invalid input: q1 must be a quaternion or a vector of' ...
          ' quaternions'])
  end
  q2type = isq(q2);
  if ( q2type == 0 )
    error(['Invalid input: q2 must be a quaternion or a vector of' ...
          ' quaternions'])
  end
end

% Make sure q1 is a column of quaternions (components are rows)
if ( q1type==1 | (q1type==3 & q2type==1) )
  q1=q1.';
end

% Make sure q2 is a column of quaternions (components are rows)
if ( q2type==1 | (q2type==3 & q1type==1) )
  q2=q2.';
end

num_q1=size(q1,1);
num_q2=size(q2,1);

if (  num_q1~=num_q2 & num_q1~=1 & num_q2~=1 )
  error(['Inputs do not have the same number of elements:', 10, ...
         '   number of quaternions in q1 = ', num2str(num_q1), 10,...
         '   number of quaternions in q2 = ', num2str(num_q2), 10,...
         'Inputs must have the same number of elements, or', 10, ...
         'one of the inputs must be a single quaternion (not a', 10, ...
         'vector of quaternions).'])
end

% Build up full quaternion vector if one input is a single quaternion
if ( num_q1 ~= num_q2 )
  ones_length = ones(max(num_q1,num_q2),1);
  if ( num_q1 == 1 )
    q1 = [q1(1)*ones_length ...
          q1(2)*ones_length ...
          q1(3)*ones_length ...
          q1(4)*ones_length ];
  else % num_q2 == 1
    q2 = [q2(1)*ones_length ...
          q2(2)*ones_length ...
          q2(3)*ones_length ...
          q2(4)*ones_length ];
  end
end

% Products

% If q1 and q2 are not vectors of quaternions, then:
%
%   q1*q2 = q1*[ q2(4) -q2(3)  q2(2) -q2(1)
%                q2(3)  q2(4) -q2(1) -q2(2)
%               -q2(2)  q2(1)  q2(4) -q2(3)
%                q2(1)  q2(2)  q2(3)  q2(4) ]
%
% But to deal with vectorized quaternions, we have to use the ugly
% commands below.

prod1 = ...
    [ q1(:,1).*q2(:,4) -q1(:,1).*q2(:,3)  q1(:,1).*q2(:,2) -q1(:,1).*q2(:,1)];
prod2 = ...
    [ q1(:,2).*q2(:,3)  q1(:,2).*q2(:,4) -q1(:,2).*q2(:,1) -q1(:,2).*q2(:,2)];
prod3 = ...
    [-q1(:,3).*q2(:,2)  q1(:,3).*q2(:,1)  q1(:,3).*q2(:,4) -q1(:,3).*q2(:,3)];
prod4 = ...
    [ q1(:,4).*q2(:,1)  q1(:,4).*q2(:,2)  q1(:,4).*q2(:,3)  q1(:,4).*q2(:,4)];

q_out = prod1 + prod2 + prod3 + prod4;

% Make sure output is same format as q1
if ( q1type==1 | (q1type==3 & q2type==1) )
  q_out=q_out.';
end

% NOTE that the following algorithm proved to be slower than the one used
% above:
%
% q_out = zeros(size(q1));
%
% q_out(:,1:3) = ...
%     [q1(:,4) q1(:,4) q1(:,4)].*q2(:,1:3) + ...
%     [q2(:,4) q2(:,4) q2(:,4)].*q1(:,1:3) + ...
%     cross(q1(:,1:3), q2(:,1:3));
%
% q_out(:,4) = q1(:,4).*q2(:,4) - dot(q1(:,1:3), q2(:,1:3), 2);


function [bodyX, bodyY, bodyZ, R_body_2_inertial] = computeInertialBodyAxesFromFrameAeroAngles(ut, rVect, vVect, bodyInfo, bankAng, angOfAttack, angOfSideslip, baseFrame)
    arguments
        ut(1,1) double
        rVect(3,1) double
        vVect(3,1) double
        bodyInfo(1,1) KSPTOT_BodyInfo
        bankAng(1,1) double
        angOfAttack(1,1) double
        angOfSideslip(1,1) double
        baseFrame(1,1) AbstractReferenceFrame
    end

    frame = bodyInfo.getBodyCenteredInertialFrame();
    ce = CartesianElementSet(ut, rVect, vVect, frame);
    ce = ce.convertToFrame(baseFrame, true);
    rVectFrame = ce.rVect;
    vVectFrame = ce.vVect;

    [R_wind_2_frame, ~, ~, ~] = computeWindFrame(rVectFrame, vVectFrame);
    R_body_2_wind = eul2rotmARH([angOfSideslip,angOfAttack,bankAng],'zyx');
    [~,~,~, R_frame_2_inertial] = baseFrame.getOffsetsWrtInertialOrigin(ut, ce);
    R_body_2_inertial = real(R_frame_2_inertial * R_wind_2_frame * R_body_2_wind);

    bodyX = R_body_2_inertial(:,1);
    bodyY = R_body_2_inertial(:,2);
    bodyZ = R_body_2_inertial(:,3);
end


function fixed = p11_fixed_points ( m, fixed_num )

%*****************************************************************************80
%
%% P11_FIXED_POINTS returns the fixed points in problem 11.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer FIXED_NUM, the number of fixed points.
%
%    Output, real FIXED(M,FIXED_NUM), the fixed points.
%
  fixed = [    ...
    0.0,  0.0; ...
    1.0,  0.0; ...
    1.0,  0.5; ...
    0.5,  0.5; ...
    0.5,  1.0; ...
    0.0,  1.0 ]';

  return
end


function K = lfmjXrbfKernCompute(lfmKern, rbfKern, t1, t2)

% LFMJXRBFKERNCOMPUTE Compute cross kernel between the LFMJ and RBF kernels.
% FORMAT
% DESC computes cross kernel terms between LFMJ and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFMJ
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% FORMAT
% DESC computes cross kernel terms between LFMJ and RBF kernels for
% the multiple output kernel.
% ARG lfmKern : the kernel structure associated with the LFMJ
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

if nargin < 4
  t2 = t1;
end
if size(t1, 2) > 1 || size(t2, 2) > 1
  error('Input can only have one column');
end

if lfmKern.inverseWidth ~= rbfKern.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Get length scale out.
sigma2 = 2/lfmKern.inverseWidth;
sigma = sqrt(sigma2);

% Parameters of the kernel
alpha = lfmKern.damper./(2*lfmKern.mass);
omega = sqrt(lfmKern.spring./lfmKern.mass - alpha.*alpha);

gamma1 = alpha + j*omega;
gamma2 = alpha - j*omega;

sK = lfmjpComputeUpsilonMatrix(gamma2,sigma2,t1, t2, 0) - ...
    lfmjpComputeUpsilonMatrix(gamma1,sigma2,t1, t2, 0);

if lfmKern.isNormalised
   K0 = lfmKern.sensitivity/(j*4*sqrt(2)*lfmKern.mass*omega);
else
   K0 = sqrt(pi)*sigma*lfmKern.sensitivity/(j*4*lfmKern.mass*omega);
end

K = K0*sK;


function c8_normal_01_test ( )

%*****************************************************************************80
%
%% C8_NORMAL_01_TEST tests C8_NORMAL_01.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 July 2006
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_NORMAL_01_TEST\n' );
  fprintf ( 1, '  C8_NORMAL_01 computes pseudorandom complex values\n' );
  fprintf ( 1, '  normally distributed in the unit circle.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The initial seed is %d\n', seed );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    [ r, seed ] = c8_normal_01 ( seed );
    fprintf ( 1, '  %6d  (%14f,  %14f)\n', i, real ( r ), imag ( r ) );
  end

  return
end


function gmsh_mesh3d_write ( gmsh_filename, m, node_num, node_x, ...
  element_order, element_num, element_node )

%*****************************************************************************80
%
%% GMSH_MESH3D_WRITE writes 3D mesh data as a Gmsh mesh file.
%
%  Discussion:
%
%    The node ordering for the 20 node element is not standard.
%
%    Assuming the vertices are A, B, C and D, Gmsh uses the following ordering:
%
%    1:    a
%    2:        b
%    3:            c
%    4:                d
%    5: (2*a  +b        )/3
%    6: (  a+2*b        )/3
%    7: (    2*b+  c    )/3
%    8: (      b+2*c    )/3
%    9: (  a    +2*c    )/3
%   10: (2*a    +  c    )/3
%   11: (2*a        +  d)/3
%   12: (  a        +2*d)/3
%   13: (     b     +2*d)/3
%   14: (   2*b     +  d)/3
%   15: (       +  c+2*d)/3
%   16: (       +2*c+  d)/3
%   17: (  a+  b+  c    )/3
%   18: (  a+  b    +  d)/3
%   19: (      b+  c+  d)/3
%   20: (  a+      c+  d)/3
%
%    Leo Rebholz used the following ordering:
%
%    1:    a
%    2:        b
%    3:            c
%    4:                d
%    5: (2*a  +b        )/3
%    6: (2*a    +  c    )/3
%    7: (  a+2*b        )/3
%    8: (  a    +2*c    )/3
%    9: (  a+  b+  c    )/3
%   10: (    2*b+  c    )/3
%   11: (      b+2*c    )/3
%   12: (2*a        +  d)/3
%   13: (   2*b     +  d)/3
%   14: (       +2*c+  d)/3
%   15: (  a+  b    +  d)/3
%   16: (      b+  c+  d)/3
%   17: (  a+      c+  d)/3
%   18: (  a        +2*d)/3
%   19: (     b     +2*d)/3
%   20: (       +  c+2*d)/3
%
%    Since the only 20 node data we have is from Leo, we will assume that
%    all 20 node input data is in Leo's format, and needs to be converted
%    to the Gmsh convention.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Christophe Geuzaine, Jean-Francois Remacle,
%    Gmsh: a three-dimensional finite element mesh generator with
%    built-in pre- and post-processing facilities,
%    International Journal for Numerical Methods in Engineering,
%    Volume 79, Number 11, pages 1309-1331, 2009.
%
%  Parameters:
%
%    Input, string GMSH_FILENAME, the name of the Gmsh file.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_X(M,NODE_NUM), the node coordinates.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM), the nodes
%    that make up each element.
%
  leo_to_gmsh = [...
     1,  2,  3,  4,  5, ...
     7, 10, 11,  8,  6, ...
    12, 18, 19, 13, 20, ...
    14,  9, 15, 16, 17 ];
%
%  Enforce 1-based indexing.
%
  element_node = mesh_base_one ( node_num, element_order, element_num, ...
    element_node );
%
%  Open the file.
%
  gmsh = fopen ( gmsh_filename, 'wt' );

  if ( gmsh < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GMSH_MESH3D_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'GMSH_MESH3D_WRITE - Error!' );
  end
%
%  Write the data.
%
  fprintf ( gmsh, '$MeshFormat\n' );
  fprintf ( gmsh, '2.2 0 8\n' );
  fprintf ( gmsh, '$EndMeshFormat\n' );

  fprintf ( gmsh, '$Nodes\n' );
  fprintf ( gmsh, '%d\n', node_num );
  for node = 1 : node_num
    fprintf ( gmsh, '%d', node );
    for dim = 1 : 3
      if ( dim <= m )
        fprintf ( gmsh, '  %g', node_x(dim,node) );
      else
        fprintf ( gmsh, '  %g', 0.0 );
      end
    end
    fprintf ( gmsh, '\n' );
  end
  fprintf ( gmsh, '$EndNodes\n' );
%
%  These are the Gmsh codes for 4, 10 and 20 node tetrahedral elements.
%
  if ( element_order == 4 )
    element_type = 4;
  elseif ( element_order == 10 )
    element_type = 11;
  elseif ( element_order == 20 )
    element_type = 29;
  end

  tag_num = 2;
  tag1 = 0;
  fprintf ( gmsh, '$Elements\n' );
  fprintf ( gmsh, '%d\n', element_num );
  for element = 1 : element_num
    fprintf ( gmsh, '%d  %d  %d  %d  %d', ...
      element, element_type, tag_num, tag1, element );
    for vertex = 1 : element_order
      if ( element_order == 20 )
        v = leo_to_gmsh(vertex);
      else
        v = vertex;
      end
      fprintf ( gmsh, '  %d', element_node(v,element) );
    end
    fprintf ( gmsh, '\n' );
  end
  fprintf ( gmsh, '$EndElements\n' );

  fclose ( gmsh );

  return
end


function y = feval(f, x)
%FEVAL   Evaluate a CHEBTECH.
%   Y = FEVAL(F, X) evaluates of the CHEBTECH F at points X.
%
%   If size(F, 2) > 1 then FEVAL returns values in the form [F_1(X), F_2(X),
%   ...], where size(F_k(X)) = size(X).
%
%   Example:
%     f = chebtech2(@(x) 1./( 1 + 25*x.^2 ) );
%     x = linspace(-1, 1, 1000);
%     [xx, yy] = meshgrid(x, x);
%     ff = feval(f, xx + 1i*yy);
%     h = surf(xx, yy, 0*xx, angle(-ff));
%     set(h, 'edgealpha', 0)
%     view(0, 90), shg
%     colormap(hsv)
%
% See also BARY, CLENSHAW, NUDCT.

% Developer note: We use either Clenshaw's algorithm or a nonuniform DCT,
% depending on the dimensions of F and X (determined heuristically).

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

if ( isempty(f) )
    y = [];
    return
end

% Reshape x to be a column vector
[n, m] = size(f);
sizex = size(x);
ndimsx = ndims(x);
x = x(:);

if ( (m > 1) && (ndimsx > 2) )
    error('CHEBFUN:CHEBTECH:feval:evalArrayAtNDArray', ...
        ['Evaluation of a CHEBTECH with more than one column at inputs ' ...
         'with more than two dimensions is not supported.']);
end

if ( n <= 4000 || numel(x) <= 4000 )
    % Evaluate using Clenshaw's algorithm:
    y = f.clenshaw(x, f.coeffs);
else
    % Use fast transform for high degree Chebyshev expansions:
    y = chebfun.ndct(x, f.coeffs);
end

% Reshape the output if possible:
if ( (m == 1) && ( (ndimsx > 2) || (sizex(2) > 1) ) )
    y = reshape(y, sizex);
elseif ( (m > 1) && ( (ndimsx == 2) || (sizex(2) > 1) ) )
    y = reshape(y, sizex(1), m*numel(x)/sizex(1));
end

end


function w = m_ilut_2dir(v,aparams,mparams)
%m_ilut_2dir     two-directional ILU preconditioning
%   w = m_ilut_2dir(v,aparams,mparams)
%   input
%          v            operand for preconditioning operator
%          aparams      structure defining coefficient matrix
%          mparams      structure defining preconditioning matrix
%                          factored splitting operators L*U, Lv*Uv
%                          permutation vectors p, ip
%   output
%          w            result of preconditioning operation
%
%   IFISS function: HCE; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

w = mparams.U\(mparams.L\v);
r = v - aparams.A*w;
rp = r(mparams.ip);
rp = mparams.Uv\(mparams.Lv\rp);
rpp = rp(mparams.p);
w = w + rpp;