function [x,f,exitflag,output] = minFunc(funObj,x0,options,varargin)
% minFunc(funObj,x0,options,varargin)
%
% Unconstrained optimizer using a line search strategy
%
% Uses an interface very similar to fminunc
%   (it doesn't support all of the optimization toolbox options,
%       but supports many other options).
%
% It computes descent directions using one of ('Method'):
%   - 'sd': Steepest Descent
%       (no previous information used, not recommended)
%   - 'csd': Cyclic Steepest Descent
%       (uses previous step length for a fixed length cycle)
%   - 'bb': Barzilai and Borwein Gradient
%       (uses only previous step)
%   - 'cg': Non-Linear Conjugate Gradient
%       (uses only previous step and a vector beta)
%   - 'scg': Scaled Non-Linear Conjugate Gradient
%       (uses previous step and a vector beta,
%           and Hessian-vector products to initialize line search)
%   - 'pcg': Preconditionined Non-Linear Conjugate Gradient
%       (uses only previous step and a vector beta, preconditioned version)
%   - 'lbfgs': Quasi-Newton with Limited-Memory BFGS Updating
%       (default: uses a predetermined nunber of previous steps to form a
%           low-rank Hessian approximation)
%   - 'newton0': Hessian-Free Newton
%       (numerically computes Hessian-Vector products)
%   - 'pnewton0': Preconditioned Hessian-Free Newton
%       (numerically computes Hessian-Vector products, preconditioned
%       version)
%   - 'qnewton': Quasi-Newton Hessian approximation
%       (uses dense Hessian approximation)
%   - 'mnewton': Newton's method with Hessian calculation after every
%   user-specified number of iterations
%       (needs user-supplied Hessian matrix)
%   - 'newton': Newton's method with Hessian calculation every iteration
%       (needs user-supplied Hessian matrix)
%   - 'tensor': Tensor
%       (needs user-supplied Hessian matrix and Tensor of 3rd partial derivatives)
%
% Several line search strategies are available for finding a step length satisfying
%   the termination criteria ('LS'):
%   - 0: Backtrack w/ Step Size Halving
%   - 1: Backtrack w/ Quadratic/Cubic Interpolation from new function values
%   - 2: Backtrack w/ Cubic Interpolation from new function + gradient
%   values (default for 'bb' and 'sd')
%   - 3: Bracketing w/ Step Size Doubling and Bisection
%   - 4: Bracketing w/ Cubic Interpolation/Extrapolation with function +
%   gradient values (default for all except 'bb' and 'sd')
%   - 5: Bracketing w/ Mixed Quadratic/Cubic Interpolation/Extrapolation
%   - 6: Use Matlab Optimization Toolbox's line search
%           (requires Matlab's linesearch.m to be added to the path)
%
%   Above, the first three find a point satisfying the Armijo conditions,
%   while the last four search for find a point satisfying the Wolfe
%   conditions.  If the objective function overflows, it is recommended
%   to use one of the first 3.
%   The first three can be used to perform a non-monotone
%   linesearch by changing the option 'Fref'.
%
% Several strategies for choosing the initial step size are avaiable ('LS_init'):
%   - 0: Always try an initial step length of 1 (default for all except 'cg' and 'sd')
%       (t = 1)
%   - 1: Use a step similar to the previous step (default for 'cg' and 'sd')
%       (t = t_old*min(2,g'd/g_old'd_old))
%   - 2: Quadratic Initialization using previous function value and new
%   function value/gradient (use this if steps tend to be very long)
%       (t = min(1,2*(f-f_old)/g))
%   - 3: The minimum between 1 and twice the previous step length
%       (t = min(1,2*t)
%   - 4: The scaled conjugate gradient step length (may accelerate
%   conjugate gradient methods, but requires a Hessian-vector product)
%       (t = g'd/d'Hd)
%
% Inputs:
%   funObj is a function handle
%   x0 is a starting vector;
%   options is a struct containing parameters
%  (defaults are used for non-existent or blank fields)
%   all other arguments are passed to funObj
%
% Outputs:
%   x is the minimum value found
%   f is the function value at the minimum found
%   exitflag returns an exit condition
%   output returns a structure with other information
%
% Supported Input Options
%   Display - Level of display [ off | final | (iter) | full | excessive ]
%   MaxFunEvals - Maximum number of function evaluations allowed (1000)
%   MaxIter - Maximum number of iterations allowed (500)
%   TolFun - Termination tolerance on the first-order optimality (1e-5)
%   TolX - Termination tolerance on progress in terms of function/parameter changes (1e-9)
%   Method - [ sd | csd | bb | cg | scg | pcg | {lbfgs} | newton0 | pnewton0 |
%       qnewton | mnewton | newton | tensor ]
%   c1 - Sufficient Decrease for Armijo condition (1e-4)
%   c2 - Curvature Decrease for Wolfe conditions (.2 for cg methods, .9 otherwise)
%   LS_init - Line Search Initialization -see above (2 for cg/sd, 4 for scg, 0 otherwise)
%   LS - Line Search type -see above (2 for bb, 4 otherwise)
%   Fref - Setting this to a positive integer greater than 1
%       will use non-monotone Armijo objective in the line search.
%       (20 for bb, 10 for csd, 1 for all others)
%   numDiff - compute derivative numerically
%       (default: 0) (this option has a different effect for 'newton', see below)
%   useComplex - if 1, use complex differentials when computing numerical derivatives
%       to get very accurate values (default: 0)
%   DerivativeCheck - if 'on', computes derivatives numerically at initial
%       point and compares to user-supplied derivative (default: 'off')
%   outputFcn - function to run after each iteration (default: []).  It
%       should have the following interface:
%       outputFcn(x,infoStruct,state,varargin{:})
%   useMex - where applicable, use mex files to speed things up (default: 1)
%
% Method-specific input options:
%   newton:
%       HessianModify - type of Hessian modification for direct solvers to
%       use if the Hessian is not positive definite (default: 0)
%           0: Minimum Euclidean norm s.t. eigenvalues sufficiently large
%           (requires eigenvalues on iterations where matrix is not pd)
%           1: Start with (1/2)*||A||_F and increment until Cholesky succeeds
%           (an approximation to method 0, does not require eigenvalues)
%           2: Modified LDL factorization
%           (only 1 generalized Cholesky factorization done and no eigenvalues required)
%           3: Modified Spectral Decomposition
%           (requires eigenvalues)
%           4: Modified Symmetric Indefinite Factorization
%           5: Uses the eigenvector of the smallest eigenvalue as negative
%           curvature direction
%       cgSolve - use conjugate gradient instead of direct solver (default: 0)
%           0: Direct Solver
%           1: Conjugate Gradient
%           2: Conjugate Gradient with Diagonal Preconditioner
%           3: Conjugate Gradient with LBFGS Preconditioner
%           x: Conjugate Graident with Symmetric Successive Over Relaxation
%           Preconditioner with parameter x
%               (where x is a real number in the range [0,2])
%           x: Conjugate Gradient with Incomplete Cholesky Preconditioner
%           with drop tolerance -x
%               (where x is a real negative number)
%       numDiff - compute Hessian numerically
%                 (default: 0, done with complex differentials if useComplex = 1)
%       LS_saveHessiancomp - when on, only computes the Hessian at the
%       first and last iteration of the line search (default: 1)
%   mnewton:
%       HessianIter - number of iterations to use same Hessian (default: 5)
%   qnewton:
%       initialHessType - scale initial Hessian approximation (default: 1)
%       qnUpdate - type of quasi-Newton update (default: 3):
%           0: BFGS
%           1: SR1 (when it is positive-definite, otherwise BFGS)
%           2: Hoshino
%           3: Self-Scaling BFGS
%           4: Oren's Self-Scaling Variable Metric method
%           5: McCormick-Huang asymmetric update
%       Damped - use damped BFGS update (default: 1)
%   newton0/pnewton0:
%       HvFunc - user-supplied function that returns Hessian-vector products
%           (by default, these are computed numerically using autoHv)
%           HvFunc should have the following interface: HvFunc(v,x,varargin{:})
%       useComplex - use a complex perturbation to get high accuracy
%           Hessian-vector products (default: 0)
%           (the increased accuracy can make the method much more efficient,
%               but gradient code must properly support complex inputs)
%       useNegCurv - a negative curvature direction is used as the descent
%           direction if one is encountered during the cg iterations
%           (default: 1)
%       precFunc (for pnewton0 only) - user-supplied preconditioner
%           (by default, an L-BFGS preconditioner is used)
%           precFunc should have the following interfact:
%           precFunc(v,x,varargin{:})
%   lbfgs:
%       Corr - number of corrections to store in memory (default: 100)
%           (higher numbers converge faster but use more memory)
%       Damped - use damped update (default: 0)
%   pcg:
%       cgUpdate - type of update (default: 2)
%   cg/scg/pcg:
%       cgUpdate - type of update (default for cg/scg: 2, default for pcg: 1)
%           0: Fletcher Reeves
%           1: Polak-Ribiere
%           2: Hestenes-Stiefel (not supported for pcg)
%           3: Gilbert-Nocedal
%       HvFunc (for scg only)- user-supplied function that returns Hessian-vector
%           products
%           (by default, these are computed numerically using autoHv)
%           HvFunc should have the following interface:
%           HvFunc(v,x,varargin{:})
%       precFunc (for pcg only) - user-supplied preconditioner
%           (by default, an L-BFGS preconditioner is used)
%           precFunc should have the following interfact:
%           precFunc(v,x,varargin{:})
%   bb:
%       bbType - type of bb step (default: 1)
%           0: min_alpha ||delta_x - alpha delta_g||_2
%           1: min_alpha ||alpha delta_x - delta_g||_2
%           2: Conic BB
%           3: Gradient method with retards
%   csd:
%       cycle - length of cycle (default: 3)
%
% Supported Output Options
%   iterations - number of iterations taken
%   funcCount - number of function evaluations
%   algorithm - algorithm used
%   firstorderopt - first-order optimality
%   message - exit message
%   trace.funccount - function evaluations after each iteration
%   trace.fval - function value after each iteration
%
% Author: Mark Schmidt (2006)
% Web: http://www.cs.ubc.ca/~schmidtm
%
% Sources (in order of how much the source material contributes):
%   J. Nocedal and S.J. Wright.  1999.  "Numerical Optimization".  Springer Verlag.
%   R. Fletcher.  1987.  "Practical Methods of Optimization".  Wiley.
%   J. Demmel.  1997.  "Applied Linear Algebra.  SIAM.
%   R. Barret, M. Berry, T. Chan, J. Demmel, J. Dongarra, V. Eijkhout, R.
%   Pozo, C. Romine, and H. Van der Vost.  1994.  "Templates for the Solution of
%   Linear Systems: Building Blocks for Iterative Methods".  SIAM.
%   J. More and D. Thuente.  "Line search algorithms with guaranteed
%   sufficient decrease".  ACM Trans. Math. Softw. vol 20, 286-307, 1994.
%   M. Raydan.  "The Barzilai and Borwein gradient method for the large
%   scale unconstrained minimization problem".  SIAM J. Optim., 7, 26-33,
%   (1997).
%   "Mathematical Optimization".  The Computational Science Education
%   Project.  1995.
%   C. Kelley.  1999.  "Iterative Methods for Optimization".  Frontiers in
%   Applied Mathematics.  SIAM.

if nargin < 3
    options = [];
end

% Get Parameters
[verbose,verboseI,debug,doPlot,maxFunEvals,maxIter,tolFun,tolX,method,...
    corrections,c1,c2,LS_init,LS,cgSolve,qnUpdate,cgUpdate,initialHessType,...
    HessianModify,Fref,useComplex,numDiff,LS_saveHessianComp,...
    DerivativeCheck,Damped,HvFunc,bbType,cycle,...
    HessianIter,outputFcn,useMex,useNegCurv,precFunc] = ...
    minFunc_processInputOptions(options);

% Constants
SD = 0;
CSD = 1;
BB = 2;
CG = 3;
PCG = 4;
LBFGS = 5;
QNEWTON = 6;
NEWTON0 = 7;
NEWTON = 8;
TENSOR = 9;

% Initialize
p = length(x0);
d = zeros(p,1);
x = x0;
t = 1;

% If necessary, form numerical differentiation functions
funEvalMultiplier = 1;
if numDiff && method ~= TENSOR
    varargin(3:end+2) = varargin(1:end);
    varargin{1} = useComplex;
    varargin{2} = funObj;
    if method ~= NEWTON
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoGrad;
    else
        if debug
            if useComplex
                fprintf('Using complex differentials for gradient computation\n');
            else
                fprintf('Using finite differences for gradient computation\n');
            end
        end
        funObj = @autoHess;
    end

    if method == NEWTON0 && useComplex == 1
        if debug
            fprintf('Turning off the use of complex differentials\n');
        end
        useComplex = 0;
    end

    if useComplex
        funEvalMultiplier = p;
    else
        funEvalMultiplier = p+1;
    end
end

% Evaluate Initial Point
if method < NEWTON
    [f,g] = funObj(x,varargin{:});
else
    [f,g,H] = funObj(x,varargin{:});
    computeHessian = 1;
end
funEvals = 1;

if strcmp(DerivativeCheck,'on')
    if numDiff
        fprintf('Can not do derivative checking when numDiff is 1\n');
    end
    % Check provided gradient/hessian function using numerical derivatives
    fprintf('Checking Gradient:\n');
    [f2,g2] = autoGrad(x,useComplex,funObj,varargin{:});

    fprintf('Max difference between user and numerical gradient: %f\n',max(abs(g-g2)));
    if max(abs(g-g2)) > 1e-4
        fprintf('User NumDif:\n');
        [g g2]
        diff = abs(g-g2)
        pause;
    end

    if method >= NEWTON
        fprintf('Check Hessian:\n');
        [f2,g2,H2] = autoHess(x,useComplex,funObj,varargin{:});

        fprintf('Max difference between user and numerical hessian: %f\n',max(abs(H(:)-H2(:))));
        if max(abs(H(:)-H2(:))) > 1e-4
            H
            H2
            diff = abs(H-H2)
            pause;
        end
    end
end

% Output Log
if verboseI
    fprintf('%10s %10s %15s %15s %15s\n','Iteration','FunEvals','Step Length','Function Val','Opt Cond');
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'init',0,funEvals,f,[],[],g,[],sum(abs(g)),varargin{:});
end

% Initialize Trace
trace.fval = f;
trace.funcCount = funEvals;

% Check optimality of initial point
if sum(abs(g)) <= tolFun
    exitflag=1;
    msg = 'Optimality Condition below TolFun';
    if verbose
        fprintf('%s\n',msg);
    end
    if nargout > 3
        output = struct('iterations',0,'funcCount',1,...
            'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
    end
    return;
end

% Perform up to a maximum of 'maxIter' descent steps:
for i = 1:maxIter

    % ****************** COMPUTE DESCENT DIRECTION *****************

    switch method
        case SD % Steepest Descent
            d = -g;

        case CSD % Cyclic Steepest Descent

            if mod(i,cycle) == 1 % Use Steepest Descent
                alpha = 1;
                LS_init = 2;
                LS = 4; % Precise Line Search
            elseif mod(i,cycle) == mod(1+1,cycle) % Use Previous Step
                alpha = t;
                LS_init = 0;
                LS = 2; % Non-monotonic line search
            end
            d = -alpha*g;

        case BB % Steepest Descent with Barzilai and Borwein Step Length

            if i == 1
                d = -g;
            else
                y = g-g_old;
                s = t*d;
                if bbType == 0
                    yy = y'*y;
                    alpha = (s'*y)/(yy);
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 1
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                elseif bbType == 2 % Conic Interpolation ('Modified BB')
                    sy = s'*y;
                    ss = s'*s;
                    alpha = ss/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    alphaConic = ss/(6*(myF_old - f) + 4*g'*s + 2*g_old'*s);
                    if alphaConic > .001*alpha && alphaConic < 1000*alpha
                        alpha = alphaConic;
                    end
                elseif bbType == 3 % Gradient Method with retards (bb type 1, random selection of previous step)
                    sy = s'*y;
                    alpha = (s'*s)/sy;
                    if alpha <= 1e-10 || alpha > 1e10
                        alpha = 1;
                    end
                    v(1+mod(i-2,5)) = alpha;
                    alpha = v(ceil(rand*length(v)));
                end
                d = -alpha*g;
            end
            g_old = g;
            myF_old = f;

        case CG % Non-Linear Conjugate Gradient

            if i == 1
                d = -g; % Initially use steepest descent direction
            else
                gtgo = g'*g_old;
                gotgo = g_old'*g_old;

                if cgUpdate == 0
                    % Fletcher-Reeves
                    beta = (g'*g)/(gotgo);
                elseif cgUpdate == 1
                    % Polak-Ribiere
                    beta = (g'*(g-g_old)) /(gotgo);
                elseif cgUpdate == 2
                    % Hestenes-Stiefel
                    beta = (g'*(g-g_old))/((g-g_old)'*d);
                else
                    % Gilbert-Nocedal
                    beta_FR = (g'*(g-g_old)) /(gotgo);
                    beta_PR = (g'*g-gtgo)/(gotgo);
                    beta = max(-beta_FR,min(beta_PR,beta_FR));
                end

                d = -g + beta*d;

                % Restart if not a direction of sufficient descent
                if g'*d > -tolX
                    if debug
                        fprintf('Restarting CG\n');
                    end
                    beta = 0;
                    d = -g;
                end

                % Old restart rule:
                %if beta < 0 || abs(gtgo)/(gotgo) >= 0.1 || g'*d >= 0

            end
            g_old = g;

        case PCG % Preconditioned Non-Linear Conjugate Gradient

            % Apply preconditioner to negative gradient
            if isempty(precFunc)
                % Use L-BFGS Preconditioner
                if i == 1
                    old_dirs = zeros(length(g),0);
                    old_stps = zeros(length(g),0);
                    Hdiag = 1;
                    s = -g;
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);

                    if useMex
                        s = lbfgsC(-g,old_dirs,old_stps,Hdiag);
                    else
                        s = lbfgs(-g,old_dirs,old_stps,Hdiag);
                    end
                end
            else % User-supplied preconditioner
                s = precFunc(-g,x,varargin{:});
            end

            if i == 1
                d = s;
            else

                if cgUpdate == 0
                    % Preconditioned Fletcher-Reeves
                    beta = (g'*s)/(g_old'*s_old);
                elseif cgUpdate < 3
                    % Preconditioned Polak-Ribiere
                    beta = (g'*(s-s_old))/(g_old'*s_old);
                else
                    % Preconditioned Gilbert-Nocedal
                    beta_FR = (g'*s)/(g_old'*s_old);
                    beta_PR = (g'*(s-s_old))/(g_old'*s_old);
                    beta = max(-beta_FR,min(beta_PR,beta_FR));
                end
                d = s + beta*d;

                if g'*d > -tolX
                    if debug
                        fprintf('Restarting CG\n');
                    end
                    beta = 0;
                    d = s;
                end

            end
            g_old = g;
            s_old = s;
        case LBFGS % L-BFGS

            % Update the direction and step sizes

            if i == 1
                d = -g; % Initially use steepest descent direction
                old_dirs = zeros(length(g),0);
                old_stps = zeros(length(d),0);
                Hdiag = 1;
            else
                if Damped
                    [old_dirs,old_stps,Hdiag] = dampedUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                else
                    [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                end

                if useMex
                    d = lbfgsC(-g,old_dirs,old_stps,Hdiag);
                else
                    d = lbfgs(-g,old_dirs,old_stps,Hdiag);
                end
            end
            g_old = g;

        case QNEWTON % Use quasi-Newton Hessian approximation

            if i == 1
                d = -g;
            else
                % Compute difference vectors
                y = g-g_old;
                s = t*d;

                if i == 2
                    % Make initial Hessian approximation
                    if initialHessType == 0
                        % Identity
                        if qnUpdate <= 1
                            R = eye(length(g));
                        else
                            H = eye(length(g));
                        end
                    else
                        % Scaled Identity
                        if debug
                            fprintf('Scaling Initial Hessian Approximation\n');
                        end
                        if qnUpdate <= 1
                            % Use Cholesky of Hessian approximation
                            R = sqrt((y'*y)/(y'*s))*eye(length(g));
                        else
                            % Use Inverse of Hessian approximation
                            H = eye(length(g))*(y'*s)/(y'*y);
                        end
                    end
                end

                if qnUpdate == 0 % Use BFGS updates
                    Bs = R'*(R*s);
                    if Damped
                        eta = .02;
                        if y'*s < eta*s'*Bs
                            if debug
                                fprintf('Damped Update\n');
                            end
                            theta = min(max(0,((1-eta)*s'*Bs)/(s'*Bs - y'*s)),1);
                            y = theta*y + (1-theta)*Bs;
                        end
                        R = cholupdate(cholupdate(R,y/sqrt(y'*s)),Bs/sqrt(s'*Bs),'-');
                    else
                        if y'*s > 1e-10
                            R = cholupdate(cholupdate(R,y/sqrt(y'*s)),Bs/sqrt(s'*Bs),'-');
                        else
                            if debug
                                fprintf('Skipping Update\n');
                            end
                        end
                    end
                elseif qnUpdate == 1 % Perform SR1 Update if it maintains positive-definiteness

                    Bs = R'*(R*s);
                    ymBs = y-Bs;
                    if abs(s'*ymBs) >= norm(s)*norm(ymBs)*1e-8 && (s-((R\(R'\y))))'*y > 1e-10
                        R = cholupdate(R,-ymBs/sqrt(ymBs'*s),'-');
                    else
                        if debug
                            fprintf('SR1 not positive-definite, doing BFGS Update\n');
                        end
                        if Damped
                            eta = .02;
                            if y'*s < eta*s'*Bs
                                if debug
                                    fprintf('Damped Update\n');
                                end
                                theta = min(max(0,((1-eta)*s'*Bs)/(s'*Bs - y'*s)),1);
                                y = theta*y + (1-theta)*Bs;
                            end
                            R = cholupdate(cholupdate(R,y/sqrt(y'*s)),Bs/sqrt(s'*Bs),'-');
                        else
                            if y'*s > 1e-10
                                R = cholupdate(cholupdate(R,y/sqrt(y'*s)),Bs/sqrt(s'*Bs),'-');
                            else
                                if debug
                                    fprintf('Skipping Update\n');
                                end
                            end
                        end
                    end
                elseif qnUpdate == 2 % Use Hoshino update
                    v = sqrt(y'*H*y)*(s/(s'*y) - (H*y)/(y'*H*y));
                    phi = 1/(1 + (y'*H*y)/(s'*y));
                    H = H + (s*s')/(s'*y) - (H*y*y'*H)/(y'*H*y) + phi*v*v';

                elseif qnUpdate == 3 % Self-Scaling BFGS update
                    ys = y'*s;
                    Hy = H*y;
                    yHy = y'*Hy;
                    gamma = ys/yHy;
                    v = sqrt(yHy)*(s/ys - Hy/yHy);
                    H = gamma*(H - Hy*Hy'/yHy + v*v') + (s*s')/ys;
                elseif qnUpdate == 4 % Oren's Self-Scaling Variable Metric update

                    % Oren's method
                    if (s'*y)/(y'*H*y) > 1
                        phi = 1; % BFGS
                        omega = 0;
                    elseif (s'*(H\s))/(s'*y) < 1
                        phi = 0; % DFP
                        omega = 1;
                    else
                        phi = (s'*y)*(y'*H*y-s'*y)/((s'*(H\s))*(y'*H*y)-(s'*y)^2);
                        omega = phi;
                    end

                    gamma = (1-omega)*(s'*y)/(y'*H*y) + omega*(s'*(H\s))/(s'*y);
                    v = sqrt(y'*H*y)*(s/(s'*y) - (H*y)/(y'*H*y));
                    H = gamma*(H - (H*y*y'*H)/(y'*H*y) + phi*v*v') + (s*s')/(s'*y);

                elseif qnUpdate == 5 % McCormick-Huang asymmetric update
                    theta = 1;
                    phi = 0;
                    psi = 1;
                    omega = 0;
                    t1 = s*(theta*s + phi*H'*y)';
                    t2 = (theta*s + phi*H'*y)'*y;
                    t3 = H*y*(psi*s + omega*H'*y)';
                    t4 = (psi*s + omega*H'*y)'*y;
                    H = H + t1/t2 - t3/t4;
                end

                if qnUpdate <= 1
                    d = -R\(R'\g);
                else
                    d = -H*g;
                end

            end
            g_old = g;

        case NEWTON0 % Hessian-Free Newton

            cgMaxIter = min(p,maxFunEvals-funEvals);
            cgForce = min(0.5,sqrt(norm(g)))*norm(g);

            % Set-up preconditioner
            precondFunc = [];
            precondArgs = [];
            if cgSolve == 1
                if isempty(precFunc) % Apply L-BFGS preconditioner
                    if i == 1
                        old_dirs = zeros(length(g),0);
                        old_stps = zeros(length(g),0);
                        Hdiag = 1;
                    else
                        [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                        if useMex
                            precondFunc = @lbfgsC;
                        else
                            precondFunc = @lbfgs;
                        end
                        precondArgs = {old_dirs,old_stps,Hdiag};
                    end
                    g_old = g;
                else
                    % Apply user-defined preconditioner
                    precondFunc = precFunc;
                    precondArgs = {x,varargin{:}};
                end
            end

            % Solve Newton system using cg and hessian-vector products
            if isempty(HvFunc)
                % No user-supplied Hessian-vector function,
                % use automatic differentiation
                HvFun = @autoHv;
                HvArgs = {x,g,useComplex,funObj,varargin{:}};
            else
                % Use user-supplid Hessian-vector function
                HvFun = HvFunc;
                HvArgs = {x,varargin{:}};
            end

            if useNegCurv
                [d,cgIter,cgRes,negCurv] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFun,HvArgs);
            else
                [d,cgIter,cgRes] = conjGrad([],-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFun,HvArgs);
            end

            funEvals = funEvals+cgIter;
            if debug
                fprintf('newtonCG stopped on iteration %d w/ residual %.5e\n',cgIter,cgRes);

            end

            if useNegCurv
                if ~isempty(negCurv)
                    %if debug
                    fprintf('Using negative curvature direction\n');
                    %end
                    d = negCurv/norm(negCurv);
                    d = d/sum(abs(g));
                end
            end

        case NEWTON % Newton search direction

            if cgSolve == 0
                if HessianModify == 0
                    % Attempt to perform a Cholesky factorization of the Hessian
                    [R,posDef] = chol(H);

                    % If the Cholesky factorization was successful, then the Hessian is
                    % positive definite, solve the system
                    if posDef == 0
                        d = -R\(R'\g);

                    else
                        % otherwise, adjust the Hessian to be positive definite based on the
                        % minimum eigenvalue, and solve with QR
                        % (expensive, we don't want to do this very much)
                        if debug
                            fprintf('Adjusting Hessian\n');
                        end
                        H = H + eye(length(g)) * max(0,1e-12 - min(real(eig(H))));
                        d = -H\g;
                    end
                elseif HessianModify == 1
                    % Modified Incomplete Cholesky
                    R = mcholinc(H,debug);
                    d = -R\(R'\g);
                elseif HessianModify == 2
                    % Modified Generalized Cholesky
                    if useMex
                        [L D perm] = mcholC(H);
                    else
                        [L D perm] = mchol(H);
                    end
                    d(perm) = -L' \ ((D.^-1).*(L \ g(perm)));

                elseif HessianModify == 3
                    % Modified Spectral Decomposition
                    [V,D] = eig((H+H')/2);
                    D = diag(D);
                    D = max(abs(D),max(max(abs(D)),1)*1e-12);
                    d = -V*((V'*g)./D);
                elseif HessianModify == 4
                    % Modified Symmetric Indefinite Factorization
                    [L,D,perm] = ldl(H,'vector');
                    [blockPos junk] = find(triu(D,1));
                    for diagInd = setdiff(setdiff(1:p,blockPos),blockPos+1)
                        if D(diagInd,diagInd) < 1e-12
                            D(diagInd,diagInd) = 1e-12;
                        end
                    end
                    for blockInd = blockPos'
                        block = D(blockInd:blockInd+1,blockInd:blockInd+1);
                        block_a = block(1);
                        block_b = block(2);
                        block_d = block(4);
                        lambda = (block_a+block_d)/2 - sqrt(4*block_b^2 + (block_a - block_d)^2)/2;
                        D(blockInd:blockInd+1,blockInd:blockInd+1) = block+eye(2)*(lambda+1e-12);
                    end
                    d(perm) = -L' \ (D \ (L \ g(perm)));
                else
                    % Take Newton step if Hessian is pd,
                    % otherwise take a step with negative curvature
                    [R,posDef] = chol(H);
                    if posDef == 0
                        d = -R\(R'\g);
                    else
                        if debug
                            fprintf('Taking Direction of Negative Curvature\n');
                        end
                        [V,D] = eig(H);
                        u = V(:,1);
                        d = -sign(u'*g)*u;
                    end
                end

            else
                % Solve with Conjugate Gradient
                cgMaxIter = p;
                cgForce = min(0.5,sqrt(norm(g)))*norm(g);

                % Select Preconditioner
                if cgSolve == 1
                    % No preconditioner
                    precondFunc = [];
                    precondArgs = [];
                elseif cgSolve == 2
                    % Diagonal preconditioner
                    precDiag = diag(H);
                    precDiag(precDiag < 1e-12) = 1e-12 - min(precDiag);
                    precondFunc = @precondDiag;
                    precondArgs = {precDiag.^-1};
                elseif cgSolve == 3
                    % L-BFGS preconditioner
                    if i == 1
                        old_dirs = zeros(length(g),0);
                        old_stps = zeros(length(g),0);
                        Hdiag = 1;
                    else
                        [old_dirs,old_stps,Hdiag] = lbfgsUpdate(g-g_old,t*d,corrections,debug,old_dirs,old_stps,Hdiag);
                    end
                    g_old = g;
                    if useMex
                        precondFunc = @lbfgsC;
                    else
                        precondFunc = @lbfgs;
                    end
                    precondArgs = {old_dirs,old_stps,Hdiag};
                elseif cgSolve > 0
                    % Symmetric Successive Overelaxation Preconditioner
                    omega = cgSolve;
                    D = diag(H);
                    D(D < 1e-12) = 1e-12 - min(D);
                    precDiag = (omega/(2-omega))*D.^-1;
                    precTriu = diag(D/omega) + triu(H,1);
                    precondFunc = @precondTriuDiag;
                    precondArgs = {precTriu,precDiag.^-1};
                else
                    % Incomplete Cholesky Preconditioner
                    opts.droptol = -cgSolve;
                    opts.rdiag = 1;
                    R = cholinc(sparse(H),opts);
                    if min(diag(R)) < 1e-12
                        R = cholinc(sparse(H + eye*(1e-12 - min(diag(R)))),opts);
                    end
                    precondFunc = @precondTriu;
                    precondArgs = {R};
                end

                % Run cg with the appropriate preconditioner
                if isempty(HvFunc)
                    % No user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs);
                else
                    % Use user-supplied Hessian-vector function
                    [d,cgIter,cgRes] = conjGrad(H,-g,cgForce,cgMaxIter,debug,precondFunc,precondArgs,HvFunc,{x,varargin{:}});
                end
                if debug
                    fprintf('CG stopped after %d iterations w/ residual %.5e\n',cgIter,cgRes);
                    %funEvals = funEvals + cgIter;
                end
            end

        case TENSOR % Tensor Method

            if numDiff
                % Compute 3rd-order Tensor Numerically
                [junk1 junk2 junk3 T] = autoTensor(x,useComplex,funObj,varargin{:});
            else
                % Use user-supplied 3rd-derivative Tensor
                [junk1 junk2 junk3 T] = funObj(x,varargin{:});
            end
            options_sub.Method = 'newton';
            options_sub.Display = 'none';
            options_sub.TolX = tolX;
            options_sub.TolFun = tolFun;
            d = minFunc(@taylorModel,zeros(p,1),options_sub,f,g,H,T);

            if any(abs(d) > 1e5) || all(abs(d) < 1e-5) || g'*d > -tolX
                if debug
                    fprintf('Using 2nd-Order Step\n');
                end
                [V,D] = eig((H+H')/2);
                D = diag(D);
                D = max(abs(D),max(max(abs(D)),1)*1e-12);
                d = -V*((V'*g)./D);
            else
                if debug
                    fprintf('Using 3rd-Order Step\n');
                end
            end
    end

    if ~isLegal(d)
        fprintf('Step direction is illegal!\n');
        pause;
        return
    end

    % ****************** COMPUTE STEP LENGTH ************************

    % Directional Derivative
    gtd = g'*d;

    % Check that progress can be made along direction
    if gtd > -tolX
        exitflag=2;
        msg = 'Directional Derivative below TolX';
        break;
    end

    % Select Initial Guess
    if i == 1
        if method < NEWTON0
            t = min(1,1/sum(abs(g)));
        else
            t = 1;
        end
    else
        if LS_init == 0
            % Newton step
            t = 1;
        elseif LS_init == 1
            % Close to previous step length
            t = t*min(2,(gtd_old)/(gtd));
        elseif LS_init == 2
            % Quadratic Initialization based on {f,g} and previous f
            t = min(1,2*(f-f_old)/(gtd));
        elseif LS_init == 3
            % Double previous step length
            t = min(1,t*2);
        elseif LS_init == 4
            % Scaled step length if possible
            if isempty(HvFunc)
                % No user-supplied Hessian-vector function,
                % use automatic differentiation
                dHd = d'*autoHv(d,x,g,0,funObj,varargin{:});
            else
                % Use user-supplid Hessian-vector function
                dHd = d'*HvFunc(d,x,varargin{:});
            end

            funEvals = funEvals + 1;
            if dHd > 0
                t = -gtd/(dHd);
            else
                t = min(1,2*(f-f_old)/(gtd));
            end
        end

        if t <= 0
            t = 1;
        end
    end
    f_old = f;
    gtd_old = gtd;

    % Compute reference fr if using non-monotone objective
    if Fref == 1
        fr = f;
    else
        if i == 1
            old_fvals = repmat(-inf,[Fref 1]);
        end

        if i <= Fref
            old_fvals(i) = f;
        else
            old_fvals = [old_fvals(2:end);f];
        end
        fr = max(old_fvals);
    end

    computeHessian = 0;
    if method >= NEWTON
        if HessianIter == 1
            computeHessian = 1;
        elseif i > 1 && mod(i-1,HessianIter) == 0
            computeHessian = 1;
        end
    end

    % Line Search
    f_old = f;
    if LS < 3 % Use Armijo Bactracking
        % Perform Backtracking line search
        if computeHessian
            [t,x,f,g,LSfunEvals,H] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,x,f,g,LSfunEvals] = ArmijoBacktrack(x,t,d,f,fr,g,gtd,c1,LS,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;

    elseif LS < 6
        % Find Point satisfying Wolfe

        if computeHessian
            [t,f,g,LSfunEvals,H] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,LS_saveHessianComp,funObj,varargin{:});
        else
            [t,f,g,LSfunEvals] = WolfeLineSearch(x,t,d,f,g,gtd,c1,c2,LS,25,tolX,debug,doPlot,1,funObj,varargin{:});
        end
        funEvals = funEvals + LSfunEvals;
        x = x + t*d;

    else
        % Use Matlab optim toolbox line search
        [t,f_new,fPrime_new,g_new,LSexitFlag,LSiter]=...
            lineSearch({'fungrad',[],funObj},x,p,1,p,d,f,gtd,t,c1,c2,-inf,maxFunEvals-funEvals,...
            tolX,[],[],[],varargin{:});
        funEvals = funEvals + LSiter;
        if isempty(t)
            exitflag = -2;
            msg = 'Matlab LineSearch failed';
            break;
        end

        if method >= NEWTON
            [f_new,g_new,H] = funObj(x + t*d,varargin{:});
            funEvals = funEvals + 1;
        end
        x = x + t*d;
        f = f_new;
        g = g_new;
    end

    % Output iteration information
    if verboseI
        fprintf('%10d %10d %15.5e %15.5e %15.5e\n',i,funEvals*funEvalMultiplier,t,f,sum(abs(g)));
    end

    % Output Function
    if ~isempty(outputFcn)
        callOutput(outputFcn,x,'iter',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
    end

    % Update Trace
    trace.fval(end+1,1) = f;
    trace.funcCount(end+1,1) = funEvals;

    % Check Optimality Condition
    if sum(abs(g)) <= tolFun
        exitflag=1;
        msg = 'Optimality Condition below TolFun';
        break;
    end

    % ******************* Check for lack of progress *******************

    if sum(abs(t*d)) <= tolX
        exitflag=2;
        msg = 'Step Size below TolX';
        break;
    end

    if abs(f-f_old) < tolX
        exitflag=2;
        msg = 'Function Value changing by less than TolX';
        break;
    end

    % ******** Check for going over iteration/evaluation limit *******************

    if funEvals*funEvalMultiplier > maxFunEvals
        exitflag = 0;
        msg = 'Exceeded Maximum Number of Function Evaluations';
        break;
    end

    if i == maxIter
        exitflag = 0;
        msg='Exceeded Maximum Number of Iterations';
        break;
    end

end

if verbose
    fprintf('%s\n',msg);
end
if nargout > 3
    output = struct('iterations',i,'funcCount',funEvals*funEvalMultiplier,...
        'algorithm',method,'firstorderopt',sum(abs(g)),'message',msg,'trace',trace);
end

% Output Function
if ~isempty(outputFcn)
    callOutput(outputFcn,x,'done',i,funEvals,f,t,gtd,g,d,sum(abs(g)),varargin{:});
end

end


function value = r8_besy0 ( x )

%*****************************************************************************80
%
%% R8_BESY0 evaluates the Bessel function Y of order 0 of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 October 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the Bessel function Y of order 0 of X.
%
  persistent alnhaf
  persistent by0cs
  persistent nty0
  persistent twodpi
  persistent xsml

  alnhaf = -0.69314718055994530941723212145818;
  twodpi = 0.636619772367581343075535053490057;

  if ( isempty ( nty0 ) )
    by0cs = [ ...
      -0.1127783939286557321793980546028E-01, ...
      -0.1283452375604203460480884531838, ...
      -0.1043788479979424936581762276618, ...
      +0.2366274918396969540924159264613E-01, ...
      -0.2090391647700486239196223950342E-02, ...
      +0.1039754539390572520999246576381E-03, ...
      -0.3369747162423972096718775345037E-05, ...
      +0.7729384267670667158521367216371E-07, ...
      -0.1324976772664259591443476068964E-08, ...
      +0.1764823261540452792100389363158E-10, ...
      -0.1881055071580196200602823012069E-12, ...
      +0.1641865485366149502792237185749E-14, ...
      -0.1195659438604606085745991006720E-16, ...
      +0.7377296297440185842494112426666E-19, ...
      -0.3906843476710437330740906666666E-21, ...
      +0.1795503664436157949829120000000E-23, ...
      -0.7229627125448010478933333333333E-26, ...
      +0.2571727931635168597333333333333E-28, ...
      -0.8141268814163694933333333333333E-31 ]';

    nty0 = r8_inits ( by0cs, 19, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( 4.0 * r8_mach ( 3 ) );

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_BESY0 - Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R8_BESY0 - Fatal error!' )
  elseif ( x <= xsml )
    y = 0.0;
    value = twodpi * ( alnhaf + log ( x ) ) * r8_besj0 ( x ) ...
      + 0.375 + r8_csevl ( 0.125 * y - 1.0, by0cs, nty0 );
  elseif ( x <= 4.0 )
    y = x * x;
    value = twodpi * ( alnhaf + log ( x ) ) * r8_besj0 ( x ) ...
      + 0.375 + r8_csevl ( 0.125 * y - 1.0, by0cs, nty0 );
  else
    [ ampl, theta ] = r8_b0mp ( x );
    value = ampl * sin ( theta );
  end

  return
end


function [w b err] = myFitPlane(pc, reg, typ, inlierThresh)
% function [w b err] = myFitPlane(pc, reg, typ, inlierThresh)
% Output: Returns w'x + b = 0;
% Input: pc is Nx3 matrix.

	if(~exist('inlierThresh','var'))
		inlierThresh = 3;
	end
	X = pc(:,:,1);
	Y = pc(:,:,2);
	Z = pc(:,:,3);

	A(:,1) = X(reg);
	A(:,2) = Y(reg);
	A(:,3) = Z(reg);
	ind = ~isnan(A(:,1));
	A = A(ind,:);

	if(size(A,1) < 10),
		w = NaN(3,1); b = NaN; err = NaN(3,1);
		return;
	end

	switch typ,
		case 'hack',
				B = A;
				b = 100*ones(size(B,1),1);
				nTmp = linsolve(B,b);
				w = nTmp./norm(nTmp);
				b = -b(1)./norm(nTmp);

		case 'disparity',
				% Following XYZ paper which wants to treat disparity as the random variable!
				B = A;
				b = 100*ones(size(B,1),1)./B(:,3);
				B = bsxfun(@times,B,1./B(:,3));
				nTmp = linsolve(B,b);
				w = nTmp./norm(nTmp);
				b = -100./norm(nTmp);

		case 'ransachack',
				B = A(randperm(size(A,1)),:);
				B = A(1:min(500,end),:);
				[wb, P, inliers] = ransacfitplane(B', inlierThresh, false);
				w = wb(1:3)./norm(wb(1:3));
				b = wb(4)./norm(wb(1:3));
	end

	%Reorienting the normals of the floor.
	sg = sign(sum(sign(A*w)));
	if(isnan(sg) || sg == 0)
		sg = 1;
	end
	w = w.*sg;
	b = b*sg;

	%Find the errors - fraction of inliers, mean distance from all points, mean distance from inliers.
	dist = A*w+b;
	inliers = find(abs(dist) < inlierThresh);
	err(1) = length(inliers)/size(A,1);
	err(2) = mean(abs(dist));
	err(3) = mean(abs(dist(inliers)));
end


function tests = test_omp
  tests = functiontests(localfunctions);
end

function [A, x, b, k] = problem_1()
    m = 100;
    n = 1000;
    k = 4;
    A = spx.dict.simple.gaussian_dict(m, n);
    gen = spx.data.synthetic.SparseSignalGenerator(n, k);
    % create a sparse vector
    x =  gen.biGaussian();
    b = A*x;
end

function [dict, reps, signals, k] = problem_2()
    m = 200;
    n = 1000;
    k = 10;
    s = 500;
    dict = spx.dict.simple.gaussian_dict(m, n);
    gen = spx.data.synthetic.SparseSignalGenerator(n, k, s);
    % create a sparse vector
    reps =  gen.biGaussian();
    signals = dict*reps;
end

function test_naive_omp_1(testCase)
    [A, x, b, k] = problem_1();
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(A, k);
    result = solver.solve(b);
    cmpare = spx.commons.SparseSignalsComparison(x, result.z, k);
    %cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end

function test_naive_omp_2(testCase)
    [dict, reps, signals, k] = problem_2();
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(dict, k);
    ns = size(signals, 2);
    dd = size(dict, 2);
    recovered = zeros(dd, ns);
    for s=1:ns
        signal = signals(:, s);
        result = solver.solve(signal);
        recovered(:, s) = result.z;
    end
    cmpare = spx.commons.SparseSignalsComparison(reps, recovered, k);
    % cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end

function test_omp_qr_1(testCase)
    [A, x, b, k] = problem_1();
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(A, k);
    result = solver.solve_qr(b);
    cmpare = spx.commons.SparseSignalsComparison(x, result.z, k);
    %cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end

function test_omp_qr_2(testCase)
    [dict, reps, signals, k] = problem_2();
    solver = spx.pursuit.single.OrthogonalMatchingPursuit(dict, k);
    ns = size(signals, 2);
    dd = size(dict, 2);
    recovered = zeros(dd, ns);
    for s=1:ns
        signal = signals(:, s);
        result = solver.solve_qr(signal);
        recovered(:, s) = result.z;
    end
    cmpare = spx.commons.SparseSignalsComparison(reps, recovered, k);
    % cmpare.summarize();
    verifyTrue(testCase, cmpare.all_have_matching_supports(1.0));
end


function varargout = Fading_BER_Curves(SNRs)
%ConvCode_BER_Curves Bit Error Rate plots for convolutional coded system.
%   h = ConvCode_BER_Curves plots an upper bound of the bit error rate
%   (BER) verus SNR per information bit (Eb/No) for a constraint length 7
%   (K=7), rate 1/2 code (R=1/2) over an AWGN channel with BPSK modulation.
%
%   The theoretical results are produced using the BERCODING function
%   from the Communications Toolbox, which uses expressions taken from:
%   [1] J. G. Proakis, Digital Communications, McGraw-Hill, 4th edition, 2001.

%   Written by Idin Motedayen-Aval
%   Applications Engineer
%   The MathWorks, Inc.
%   zq=[4 2 5 -15 -1 -3 24 -57 45 -12 19 -12 15 -8 3 -7 8 -69 53 12 -2];
%   char(filter(1,[1,-1],[105 zq])), clear zq

x = SNRs;    % Eb/No range

figure1 = figure;
k = 1;
for L = [1, 2, 3, 4, 6, 8];
    ber(k,:) = berfading(x,'psk',2,L);
    k = k+1;
end
ber(k,:) = berawgn(x,'psk',2,'nondiff'); % AWGN BER curve for reference

% Plot the results
line_h = semilogy(x,ber,':');
grid on
ylim([1e-006 1]);
xlim([min(SNRs) max(SNRs)]);
% legend show

% Create title
myT = sprintf('BER, Rayleigh Flat Fading Channel with diversity');
title(myT,'Interpreter','latex');
% Create xlabel
xlabel('SNR per transmitted bit, $^{E_b}/_{N_0}$ (dB)','Interpreter','latex');
% Create ylabel
ylabel('Bit Error Rate, BER');

% Create annotations

hold off

if nargout
    varargout{1} = figure1;
    if nargout > 1
        varargout{2} = line_h;
    end
end


function error = verifyquadquadpts(n)
%% VERIFYQUADQUADPTS verify the quadrature formula on [0,1]^2
%
% Author: Huayi Wei < huayiwei1984@gmail.com>
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if (n>10), n=10; end

a = 0;
b = 1;

c = 0;
d = 1;

l = 1;

% get quadrature points
[pts,weight] = quadquadpts(n);
nQuad = size(pts,1);
t1 = 0;
t2 = 0;
for p = 1:nQuad
    % quadrature points in the x-y coordinate
    px = pts(p,1)*a + (1-pts(p,1))*b;
    py = pts(p,2)*c + (1-pts(p,2))*d;

    t1 = t1 + weight(p)*f1(px,py, n);
    t2 = t2 + weight(p)*f2(px,py);
end
t1 = t1*l;
t2 = t2*l;
error(1) = abs(t1 - 1/(4*n*n));
error(2) = abs(t2 - (- cos(1)+1)*(-cos(1)+1));
end

function z = f1(x, y, n)
z = x.^(2*n-1).*y.^(2*n-1);
end

function z = f2(x,y)
z = sin(x)*sin(y);
end


function [xvals, yvals, color] = hintmat(w);
%HINTMAT Evaluates the coordinates of the patches for a Hinton diagram.
%
%	Description
%	[xvals, yvals, color] = hintmat(w)
%	  takes a matrix W and returns coordinates XVALS, YVALS for the
%	patches comrising the Hinton diagram, together with a vector COLOR
%	labelling the color (black or white) of the corresponding elements
%	according to their sign.
%
%	See also
%	HINTON
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Set scale to be up to 0.9 of maximum absolute weight value, where scale
% defined so that area of box proportional to weight value.

w = flipud(w);
[nrows, ncols] = size(w);

scale = 0.45*sqrt(abs(w)/max(max(abs(w))));
scale = scale(:);
color = 0.5*(sign(w(:)) + 3);

delx = 1;
dely = 1;
[X, Y] = meshgrid(0.5*delx:delx:(ncols-0.5*delx), 0.5*dely:dely:(nrows-0.5*dely));

% Now convert from matrix format to column vector format, and then duplicate
% columns with appropriate offsets determined by normalized weight magnitudes.

xtemp = X(:);
ytemp = Y(:);

xvals = [xtemp-delx*scale, xtemp+delx*scale, ...
         xtemp+delx*scale, xtemp-delx*scale];
yvals = [ytemp-dely*scale, ytemp-dely*scale, ...
         ytemp+dely*scale, ytemp+dely*scale];


function [MHz] = GHz2MHz(GHz)
% Convert frequency from gigahertz to megahertz.
% Chad A. Greene 2012
MHz = GHz*1e+3;


%TRINTERP Interpolate homogeneous transformations
%
% T = TRINTERP(T0, T1, S) is a homogeneous transform (4x4) interpolated
% between T0 when S=0 and T1 when S=1.  T0 and T1 are both homogeneous
% transforms (4x4).  Rotation is interpolated using quaternion spherical
% linear interpolation (slerp).  If S (Nx1) then T (4x4xN) is a sequence of
% homogeneous transforms corresponding to the interpolation values in S.
%
% T = TRINTERP(T1, S) as above but interpolated between the identity matrix
% when S=0 to T1 when S=1.
%
% See also CTRAJ, QUATERNION.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function T = trinterp(A, B, C)

    if nargin == 3
        %	TR = TRINTERP(T0, T1, r)
        T0 = A; T1 = B; r = C;

        if length(r) > 1
            T = [];
            for rr=r(:)'
                TT = trinterp(T0, T1, rr);
                T = cat(3, T, TT);
            end
            return;
        end

        q0 = Quaternion(T0);
        q1 = Quaternion(T1);

        p0 = transl(T0);
        p1 = transl(T1);

        qr = q0.interp(q1, r);
        pr = p0*(1-r) + r*p1;
    elseif nargin == 2
    %	TR = TRINTERP(T, r)
        T0 = A; r = B;

        if length(r) > 1
            T = [];
            for rr=r(:)'
                TT = trinterp(T0, rr);
                T = cat(3, T, TT);
            end
            return;
        end

        q0 = Quaternion(T0);
        p0 = transl(T0);

        qr = q0.scale(r);
        pr = r*p0;
    else
        error('must be 2 or 3 arguments');
    end
    T = rt2tr(qr.R, pr);


function [cal] = eV2cal(eV)
% Convert energy or work from electron volts to calories.
% Chad A. Greene 2012
cal = eV*3.8267347377e-20;


function out = mergeScore(x)

s1 = var(x(x>mean(x)));
s2 = var(x(x<mean(x)));

mu1 = mean(x(x>mean(x)));
mu2 = mean(x(x<mean(x)));
p  = mean(x>mean(x));

logp = zeros(numel(x), 2);

rs = zeros(numel(x), 2);
rs(x<0, 1) = 1;
rs(x>0, 2) = 1;

for k = 1:20
    logp(:,1) = -1/2*log(s1) - (x-mu1).^2/(2*s1) + log(p);
    logp(:,2) = -1/2*log(s2) - (x-mu2).^2/(2*s2) + log(1-p);

    lMax = max(logp,[],2);
    logp = logp - lMax;

    rs = exp(logp);

    pval = log(sum(rs,2)) + lMax;
    logP(k) = mean(pval);

    rs = rs./sum(rs,2);

    p = mean(rs(:,1));
    mu1 = (rs(:,1)' * x )/sum(rs(:,1));
    mu2 = (rs(:,2)' * x )/sum(rs(:,2));

    s1 = (rs(:,1)' * (x-mu1).^2 )/sum(rs(:,1));
    s2 = (rs(:,2)' * (x-mu2).^2 )/sum(rs(:,2));

end

ilow = rs(:,1)>rs(:,2);

plow = mean(rs(ilow,1));
phigh = mean(rs(~ilow,2));

% when do I split
out =  ~(plow>.9 && phigh>.9);

% if sign(mu1*mu2)>0
%     out = 0;
% end


function [m3] = in32m3(in3)
% Convert volume from cubic inches to cubic meters.
% Chad Greene 2012
m3 = in3*0.000016387064;


function odf = calcBinghamODF(ori,varargin)
% calculate ODF from individuel orientations via kernel density estimation
%
% *calcODF* is one of the core function of the MTEX toolbox.
% It estimates an ODF from a set of individual crystal orientations by
% <EBSD2ODF.html kernel density estimation>.
%
% Input
%  ori  - @orientation
%
% Output
%  odf - @SO3Fun
%
% See also
% ebsd_demo EBSD2odf EBSDSimulation_demo EBSD/load EBSD/calcKernel kernel/kernel

% maybe there is nothing to do
if isempty(ori), odf = ODF; return, end

% estimate Bingham parameters
[~,~,lambda,ev] = mean(ori,varargin{:});
kappa = evalkappa(lambda,varargin{:});

% set up Bingham ODF
odf = BinghamODF(kappa,ev,ori.CS,ori.SS);


function [ mK ] = CreateConvMtx2D( mH, numRows, numCols, convShape )
% ----------------------------------------------------------------------------------------------- %
% [ mK ] = CreateConvMtx2D( mH, numRows, numCols, convShape )
% Generates a Convolution Matrix for the 2D Kernel (The Matrix mH) with
% support for different convolution shapes (Full / Same / Valid).
% Input:
%   - mH                -   Input 2D Convolution Kernel.
%                           Structure: Matrix.
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - numRows           -   Number of Rows.
%                           Number of rows of the image to be convolved.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - numCols           -   Number of Columns.
%                           Number of columns of the image to be convolved.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3, ...}.
%   - convShape         -   Convolution Shape.
%                           The shape of the convolution which the output
%                           convolution matrix should represent. The
%                           options should match MATLAB's conv2() function
%                           - Full / Same / Valid.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: {1, 2, 3}.
% Output:
%   - mK                -   Convolution Matrix.
%                           The output convolution matrix. The product of
%                           the matrix 'mK' and and image 'mI' in its
%                           column stack form ('mK * mI(:)') is equivalent
%                           to the convolution of 'mI' with the kernel 'mH'
%                           using the corresponding convolution shape
%                           ('conv2(mI, mH, convShapeString)').
%                           Structure: Matrix (Sparse).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References:
%   1.  MATLAB's 'convmtx2()' - https://www.mathworks.com/help/images/ref/convmtx2.html.
% Remarks:
%   1.  In caes the same convolution is applied on many images, stacking
%       them into a matrix (Each image as columns stacked vector) and
%       applying convolution on each column by matrix multiplication might
%       be more efficient than applying classic convolution per image.
%   2.  The output matrux has the form of doubly block toeplitz matrix. The
%       convolution shape sets where the diagonal of the first column to
%       appear.
% TODO:
%   1.
%   Release Notes:
%   -   1.0.001     22/01/2018  Royi Avital
%       *   Fixed issue with the creatioon of the sparse diagonal matrix.
%           The vector to initialize the diagonal was much bigger than
%           needed. Improved performance in the Unit Test by factor of 10.
%   -   1.0.000     16/01/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

CONVOLUTION_SHAPE_FULL  = 1;
CONVOLUTION_SHAPE_SAME  = 2;
CONVOLUTION_SHAPE_VALID = 3;

numColsKernel   = size(mH, 2);
numBlockMtx     = numColsKernel;

cBlockMtx = cell(numBlockMtx, 1);

for ii = 1:numBlockMtx
    cBlockMtx{ii} = CreateConvMtx1D(mH(:, ii), numRows, convShape);
end

switch(convShape)
    case(CONVOLUTION_SHAPE_FULL)
        % For convolution shape - 'full' the Doubly Block Toeplitz Matrix
        % has the first column as its main diagonal.
        diagIdx     = 0;
        numRowsKron = numCols + numColsKernel - 1;
    case(CONVOLUTION_SHAPE_SAME)
        % For convolution shape - 'same' the Doubly Block Toeplitz Matrix
        % has the first column shifted by the kernel horizontal radius.
        diagIdx     = floor(numColsKernel / 2);
        numRowsKron = numCols;
    case(CONVOLUTION_SHAPE_VALID)
        % For convolution shape - 'valid' the Doubly Block Toeplitz Matrix
        % has the first column shifted by the kernel horizontal length.
        diagIdx     = numColsKernel - 1;
        numRowsKron = numCols - numColsKernel + 1;
end

vI = ones(min(numRowsKron, numCols), 1);
mK = kron(spdiags(vI, diagIdx, numRowsKron, numCols), cBlockMtx{1});
for ii = 2:numBlockMtx
    diagIdx = diagIdx - 1;
    mK = mK + kron(spdiags(vI, diagIdx, numRowsKron, numCols), cBlockMtx{ii});
end

end


%% Analyzing Neural Time Series Data
% Matlab code for Chapter 16
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 16.1

load sampleEEGdata

channel2plot = 'o1';
timewin      = 400; % in ms

timewinidx = round(timewin/(1000/EEG.srate));
tapers     = dpss(timewinidx,5); % this line will crash without matlab signal processing toolbox

% extract a bit of EEG data
d = detrend(squeeze(EEG.data(strcmpi(channel2plot,{EEG.chanlocs.labels}),200:200+timewinidx-1,10)));

% plot EEG data snippet
figure
subplot(5,2,1)
plot(d)
axis tight,axis off

% plot tapers
for i=1:5
    subplot(5,2,(2*(i-1))+2)
    plot(tapers(:,i))
    axis tight,axis off
end

% plot taper.*data
figure
for i=1:5
    subplot(5,2,(2*(i-1))+1)
    plot(tapers(:,i).*d')
    axis tight,axis off
end

% plot fft of taper.*data
f=zeros(5,timewinidx);
for i=1:5
    subplot(5,2,(2*(i-1))+2)
    f(i,:)=fft(tapers(:,i).*d');
    plot(abs(f(i,1:timewinidx/2)).^2)
    axis tight,axis off
end

figure
subplot(5,2,2)
plot(mean(abs(f(:,1:timewinidx/2)).^2,1))
axis tight, axis off

subplot(5,2,3)
hann = .5*(1-cos(2*pi*(1:timewinidx)/(timewinidx-1)));
plot(hann)
axis tight, axis off

subplot(525)
plot(hann.*d)
axis tight, axis off

subplot(526)
ff=fft(hann.*d);
plot(mean(abs(ff(1:timewinidx/2)).^2,1))
axis tight, axis off

%% Figure 16.2

channel2plot    = 'p7';
frequency2plot  = 15;  % in Hz
timepoint2plot  = 200; % ms

nw_product      = 3;  % determines the frequency smoothing, given a specified time window
times2save      = -300:50:1000;
baseline_range  = [-200 -00];
timewin         = 400; % in ms

% convert time points to indices
times2saveidx = dsearchn(EEG.times',times2save');
timewinidx    = round(timewin/(1000/EEG.srate));

% find baselinetimepoints
baseidx = zeros(size(baseline_range));
[~,baseidx(1)] = min(abs(times2save-baseline_range(1)));
[~,baseidx(2)] = min(abs(times2save-baseline_range(2)));
% note that the following line is equivalent to the previous three
%baseidx = dsearchn(times2save',baseline_range');

% define tapers
tapers = dpss(timewinidx,nw_product); % note that in practice, you'll want to set the temporal resolution to be a function of frequency
% define frequencies for FFT
f = linspace(0,EEG.srate/2,floor(timewinidx/2)+1);

% find logical channel index
chanidx = strcmpi(channel2plot,{EEG.chanlocs.labels});

% initialize output matrix
multitaper_tf = zeros(floor(timewinidx/2)+1,length(times2save));

% loop through time bins
for ti=1:length(times2saveidx)

    % initialize power vector (over tapers)
    taperpow = zeros(floor(timewinidx/2)+1,1);

    % loop through tapers
    for tapi = 1:size(tapers,2)-1

        % window and taper data, and get power spectrum
        data      = bsxfun(@times,squeeze(EEG.data(chanidx,times2saveidx(ti)-floor(timewinidx/2)+1:times2saveidx(ti)+ceil(timewinidx/2),:)),tapers(:,tapi));
        pow       = fft(data,timewinidx)/timewinidx;
        pow       = pow(1:floor(timewinidx/2)+1,:);
        taperpow  = taperpow + mean(pow.*conj(pow),2);
    end

    % finally, get power from closest frequency
    multitaper_tf(:,ti) = taperpow/tapi;
end

% db-correct
db_multitaper_tf = 10*log10( multitaper_tf ./ repmat(mean(multitaper_tf(:,baseidx(1):baseidx(2)),2),1,length(times2save)) );

% plot time courses at one frequency band
figure
subplot(121)
[junk,freq2plotidx]=min(abs(f-frequency2plot)); % can replace "junk" with "~"
plot(times2save,mean(log10(multitaper_tf(freq2plotidx-2:freq2plotidx+2,:)),1))
title([ 'Sensor ' channel2plot ', ' num2str(frequency2plot) ' Hz' ])
axis square
set(gca,'xlim',[times2save(1) times2save(end)])

subplot(122)
[junk,time2plotidx]=min(abs(times2save-timepoint2plot));
plot(f,log10(multitaper_tf(:,time2plotidx)))
title([ 'Sensor ' channel2plot ', ' num2str(timepoint2plot) ' ms' ])
axis square
set(gca,'xlim',[f(1) 40])

% plot full TF map
figure
contourf(times2save,f,db_multitaper_tf,40,'linecolor','none')
set(gca,'clim',[-2 2])
xlabel('Time (ms)'), ylabel('Frequency (Hz)')
title([ 'Power via multitaper from channel ' channel2plot ])

%% end.


function dn = dudiv(dn0,dn1)

% DDIV dual number division
%
%   DN = DDIV(DN0,DN1) returns the dual number DN which is the dual number
%     division DN0/DN1 of the dual numbers DN0 and DN1
%      - DN0 (resp. DN1) is a dual number (DN0 = a0 + eps*b0, eps^2 = 0).
%         It is a 2-vector or a 2*N array (column i represents dual number
%         i) where N is the number of dual numbers. DN0 and DN1 must have
%         the same size. The non-dual part of DN1 must be different from 0.
%      - DN is a dual number. It is a 2*N array (each column is the dual
%          multiplication of the corresponding columns in DN0 and DN1)

s0 = size(dn0);
s1 = size(dn1);
if s0 == [1 2]
    dn0 = dn0.';
    s0 = size(dn0);
end
if s1 == [1 2]
    dn1 = dn1.';
    s1 = size(dn1);
end

% wrong format
if s0(1) ~= 2 || s1(1) ~= 2
    error('DualQuaternion:Ddiv:wrongsize',...
        '%d rows in the DN0 array and %d rows in the DN1 array. It should be 2 for both.',...
        s0(1),s1(1));
end

% sizes do not match
n1 = s0(2);
n2 = s1(2);
if n1 ~= n2
    error('DualQuaternion:Ddiv:notMatch',...
        '%d dual numbers in DN0 array and %d dual numbers in DN1 array.They should be equal.',...
        n1,n2);
end

dn = sym(zeros(2,n1));
dn(1,:) = dn0(1,:)./dn1(1,:);
dn(2,:) = dn0(2,:)./dn1(1,:)-dn0(1,:).*dn1(2,:)./(dn1(1,:).^2);


function d = distance_of_point_to_line(line, x)
% function d = distance_of_point_to_line(line, x)
%
% returns d = ax+by+c
% line: [3x1] (a,b,c)
% x: [2x1] (x,y)

xh = [x(:); 1];
d = line(:)' * xh;


% Author: Jai Juneja
% Date: 22/03/2013
%
% Find the shortest Euclidean distance from a point to a line.
% Inputs:
%   pt  :   point in space
%   v1  :   first point along line
%   v2  :   second point along line
%           pt, v1 and v2 must all be at least 3x1 vectors
% Outputs:
%   d   :   shortest distance from point pt to line
function d = pointToLine(pt, v1, v2)
    a = v1 - v2;
    b = pt - v2;
    d = norm(cross(a,b)) / norm(a);
end


function [D,W,Lw,Lb]=BuildPatch(fea,gnd,k1,k2,zeta)

% Build two patches for MD-NMF and NDLA by using KNN + sparse binary model
%   data: nSample x nFeature matrix,
%   zeta: perturbution coefficient.
% Written by Naiyang Guan (ny.guan@gmail.com)
% Copyright by Naiyang Guan and Dacheng Tao

% Initialization: assume data is MxN matrix
N=size(fea, 1);
Sw=zeros(N);
Sb=zeros(N);

% Extreme position
if k2<=0
    fprintf('error! k2 cannot be smaller than 1\n');
    return;
end

% Euclidean distance between sample points in the PCA space
dist=EuDist2(fea,fea,0);

% Construct sparse Laplacian matrix on (k1,k2)-patch
for i=1:N
    % k1 nearest neighbors within same class
    sam_idx=find(gnd==gnd(i));
    sam_dist=dist(i,sam_idx');
    [junk,index]=sort(sam_dist);
    sam_idx=sam_idx(index);
    if k1>=1
        Fi=sam_idx(2:(k1+1));
    else
        Fi=i;
    end
    Sw(i,Fi)=1;
    Sw(Fi,i)=1;

    % k2 nearest neighbors between classes
    diff_idx=find(gnd~=gnd(i));
    diff_dist=dist(i,diff_idx');
    [junk,index]=sort(diff_dist);
    diff_idx=diff_idx(index);
    Fi=diff_idx(1:k2);
    Sb(i,Fi)=1;
    Sb(Fi,i)=1;
end

% Laplacian regularization and normalization
Dw=diag(sum(Sw));
Db=diag(sum(Sb));
Lw=Dw-Sw;
Lb=Db-Sb+eye(N)*sum(sum(Sb))*zeta;

[U,S,V]=svd(Lb);
S=diag(S);
S=diag(1./sqrt(S));
D=U*S*V'*Dw*V*S*U';
W=U*S*V'*Sw*V*S*U';

return;


function price = PROJ_GMDB_ComboExpos_Linear( P, Pbar, S_0, W, call, r, params_levy, params_mort, T)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% About: Pricing Function for Gauranteed Minimum Death Benefit Options using PROJ method
%        This version assumes a comination of exponentials model of mortality (see first reference below)
% Models Supported: Levy Processes, including jump diffusions and Black-Scholes model
% Returns: price of contract
%
% Author: Zhimin Zhang  (Original Code)
%         Justin Lars Kirkby (Convert into common framework)
%
% References:  (1) Valuing Equity-Linked Death Benefits in General Exponential
%               Levy Models, J. Comput. and Appl. Math. 2019 (Z. Zhang, Y. Yong, W. Yu)
%              (2) Efficient Option Pricing By Frame Duality with The Fast
%               Fourier Transform, SIAM J. Financial Math., 2015 (J.L. Kirkby)
%
% ----------------------
% Contract/Model Params
% ----------------------
% S_0 = initial stock price (e.g. 100)
% W   = strike  (e.g. 100)
% r   = interest rate (e.g. 0.05)
% T   = time remaining until maturity (in years, e.g. T=10)
%       NOTE: set T=-1 to price a perpetual contract (no expiry)
% call  = 1 for call (else put)
% params_levy = parameters of Levy Model
% params_mort = mortality params
%
% ----------------------
% Numerical (PROJ) Params
% ----------------------
% P  = resolution parameter (increase P to use more basis elements)
% Pbar = gridwidth parameter (increase Pbar to increase the truncated density support)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set Contract / Numerical Params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
kk=log(W/S_0);

a=2^P;
hat_a=2^Pbar;

N=hat_a*a;
Delta=1/a;Delta_s=2*pi/hat_a;

% Deterime if there is a Time to expiry in contract, else it's perpetual
if nargin < 8
    T = -1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parse Death Model Params
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lambda = params_mort.lambda;  % expo rates
A = params_mort.A; % coefficients in expo combo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Levy Parse Model Params, set model inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model = params_levy.model;

if model == 1 %BSM (Black Scholes Merton)
    sigma = params_levy.sigmaBSM;

    Psi=@(x)-1/2*sigma^2*x.^2;
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2;

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c)./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c)./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 3 %NIG
    alpha = params_levy.alpha;
    beta = params_levy.beta;
    NIG_delta = params_levy.delta;
    sigma = params_levy.sigma;

    gamma=sqrt(alpha^2-beta^2);

    Psi=@(x)-1/2*sigma^2*x.^2-NIG_delta*(sqrt(alpha^2-(beta+1i*x).^2)-gamma);
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2-NIG_delta*(sqrt(alpha^2-(beta+1i*s).^2)-gamma);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+NIG_delta*sqrt(alpha^2-(beta+c).^2).*(beta+c))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+NIG_delta*sqrt(alpha^2-(beta+c).^2).*(beta+c))./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 4 %MJD (Merton Jump Diffusion)
    sigma = params_levy.sigma;
    lambda_J = params_levy.lam;
    mu_J = params_levy.muj;
    sigma_J = params_levy.sigmaj;

    Psi=@(x)-sigma^2/2*x.^2+lambda_J*(exp(1i*x*mu_J-sigma_J^2/2*x.^2)-1);
    mu=r-Psi(-1i);

    phi=@(s)1i*s*mu-sigma^2/2*s.^2+lambda_J*(exp(1i*s*mu_J-sigma_J^2/2*s.^2)-1);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c+lambda_J*exp(mu_J*c+1/2*sigma_J^2*c).*(mu_J+sigma_J^2*c))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c+lambda_J*exp(mu_J*c+1/2*sigma_J^2*c).*(mu_J+sigma_J^2*c))./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 5 %Kou Double Expo

    sigma = params_levy.sigma;
    lam_pois = params_levy.lam;
    p = params_levy.p_up;    omega=p*lam_pois;  nv=(1-p)*lam_pois;
    v = params_levy.eta1;
    w = params_levy.eta2;

    D=1/2*sigma^2;

    %risk-neutral condition
    Psi=@(x)-D*x.^2-nv.*1i.*x./(v+1i.*x)+omega.*1i.*x./(w-1i.*x);
    mu=r-Psi(-1i);

    phi=@(s)mu.*1i.*s-D.*s.^2-nv.*1i.*s./(v+1i.*s)+omega.*1i.*s./(w-1i.*s);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-nv*v./(v+c).^2+omega*w./(w-c).^2)./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-nv*v./(v+c).^2+omega*w./(w-c).^2)./(r+lambda-phi(-1i*c)).^2);
    end

elseif model == 8 % Variance Gamma
    sigma = params_levy.sigmaGBM; % geometric brownian motion add-on
    VG_mu = params_levy.theta;
    VG_sigma = params_levy.sigma;
    nv = params_levy.nu;

    Psi=@(x)-1/2*sigma^2*x.^2-1/nv*log(1-1i*nv*VG_mu*x+nv*VG_sigma^2/2*x.^2);
    mu=r-Psi(-1i);

    phi=@(s)mu*1i*s-1/2*sigma^2*s.^2-1/nv*log(1-1i*nv*VG_mu*s+nv*VG_sigma^2/2*s.^2);

    if T > 0
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-(VG_mu*1i*c-VG_sigma^2*c)./(1-nv*VG_mu*c-1/2*nv*VG_sigma^2*c.^2))./(r+lambda-phi(-1i*c)).^2.*(1-exp(-(r+lambda-phi(-1i*c))*T)-T*exp(-(r+lambda-phi(-1i*c))*T).*(r+lambda-phi(-1i*c))));
    else
        diffFgc=@(c)sum(A.*lambda*(mu+sigma^2*c-(VG_mu*1i*c-VG_sigma^2*c)./(1-nv*VG_mu*c-1/2*nv*VG_sigma^2*c.^2))./(r+lambda-phi(-1i*c)).^2);
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

delta1=@(j,k)(j==k);

S=(0:(N-1))*Delta_s;
v=@(j)1-(delta1(j,1)+delta1(j,N))/2;
Fphi=@(s)12.*sin(s./2).^2./(s.^2.*(2+cos(s)));
Fphi1=Fphi(S/a); Fphi1(1)=1;

%coefs
if T > 0
    Ak=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2)).*(1-exp(-(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2))*T)),2);
    Ck=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2)).*(1-exp(-(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2))*T)),2);

    Fgc=@(c)sum(A.*lambda./(r+lambda-phi(-1i*c)).*(1-exp(-(r+lambda-phi(-1i*c))*T)));
else
    Ak=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S).',1,2)),2);
    Ck=sum(repmat(A.*lambda,length(S),1)./(r+repmat(lambda,length(S),1)-repmat(phi(S-1i).',1,2)),2);

    Fgc=@(c)sum((A.*lambda)./(r+lambda-phi(-1i*c)));
end

% Compute Projections
x10=real(double(diffFgc(0))/Fgc(0))-Delta*(N/2-1);
X0=x10+(0:(N-1))*Delta;
beta0=a^(-1/2)/pi*real(fft((Ak.').*Fphi1.*v(1:N).*Delta_s.*exp(-1i*x10.*S)));

x11=real(double(diffFgc(1))/Fgc(1))-Delta*(N/2-1);
X1=x11+(0:(N-1))*Delta;
beta1=a^(-1/2)/pi*real(fft((Ck.').*Fphi1.*v(1:N).*Delta_s.*exp(-1i*x11.*S)));

% Compute Final Payoff and Value
nn=find((kk<X0)==1);
n=nn(1);
PHi0=zeros(1,length(S));

nn1=find((kk<X1)==1);
n1=nn1(1);
PHi1=zeros(1,length(S));

if call == 1
    PHi0(n)=a^(1/2)*(X0(n)-kk)-a^(3/2)/2*(kk-X0(n))^2+1/2*a^(-1/2);
    PHi0(n-1)=a^(1/2)*(X0(n-1)+1/a-kk)-a^(3/2)/2*(1/(a^2)-(kk-X0(n-1))^2);
    PHi0(n+1:length(S))=1/sqrt(a);

    PHi1(n1)=a^(1/2)*(X1(n1)-kk)-a^(3/2)/2*(kk-X1(n1))^2+1/2*a^(-1/2);
    PHi1(n1-1)=a^(1/2)*(X1(n1-1)+1/a-kk)-a^(3/2)/2*(1/(a^2)-(kk-X1(n1-1))^2);
    PHi1(n1+1:length(S))=1/sqrt(a);

    price=S_0*beta1*PHi1.'-W*beta0*PHi0.';
else
    PHi0(n)=a^(1/2)*(kk-X0(n)+1/a)-a^(3/2)/2*(1/(a^2)-(kk-X0(n))^2);
    PHi0(n-1)=a^(1/2)*(kk-X0(n-1))-a^(3/2)/2*(kk-X0(n-1))^2+1/2*a^(-1/2);
    PHi0(1:n-2)=1/sqrt(a);

    PHi1(n1)=a^(1/2)*(kk-X1(n1)+1/a)-a^(3/2)/2*(1/(a^2)-(kk-X1(n1))^2);
    PHi1(n1-1)=a^(1/2)*(kk-X1(n1-1))-a^(3/2)/2*(kk-X1(n1-1))^2+1/2*a^(-1/2);
    PHi1(1:n1-2)=1/sqrt(a);

    price=W*beta0*PHi0.'-S_0*beta1*PHi1.';
end

end


function [yn,en,S] = PRAadapt(un,dn,S)

% PRAadapt          Partial Rank Algorithm (See Section 6.3)
%
% Arguments:
% un                Input signal
% dn                Desired signal
% S                 Adptive filter parameters as defined in PRAinit.m
% yn                History of output signal
% en                History of error signal
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

M = length(S.coeffs);        % Length of FIR filter
mu = S.step;                 % Step size
P = S.order;                 % Projection order
alpha = S.alpha;             % Small constant
AdaptStart = S.AdaptStart;
w = S.coeffs;                % Weight vector of FIR filter
u = zeros(M,1);              % Input signal vector
A = zeros(M,P);              % Projection matrix

ITER = length(un);           % Length of input sequence
yn = zeros(1,ITER);          % Initialize output sequence to zero
en = zeros(1,ITER);          % Initialize error sequence to zero

if isfield(S,'unknownsys')
    b = S.unknownsys;
    norm_b = norm(b);
    eml = zeros(1,ITER);
    ComputeEML = 1;
else
    ComputeEML = 0;
end

for n = 1:ITER
    u = [un(n); u(1:end-1)]; % Input signal vector [u(n),u(n-1),...,u(n-M+1)]'
    A = [u A(:,1:end-1)];    % Projection matrix
    yn(n) = u'*w;            % Output of adaptive filter
    en(n) = dn(n) - yn(n);
    if ComputeEML == 1;
        eml(n) = norm(b-w)/norm_b;        % System error norm (normalized)
    end
    if (mod(n-1,P)==0)&(n >= AdaptStart)
        e = en(n:-1:n-P+1)';
        w = w + mu*A*inv(A'*A + alpha)*e; % Tap-weight adaptation for the next cycle
    end
end

S.coeffs = w;                             % Coefficient values at the final iteration
if ComputeEML == 1;
    S.eml = eml;
end


function rollPitchYaw = rollPitchYawFromRotation(R)

    % ROLLPITCHYAWFROMROTATION converts a rotation matrix into Euler angles.
    %                          The Euler angles convention follows the one
    %                          of iDyntree and is such that the rotation
    %                          matrix is:  R = Rz(yaw)*Ry(pitch)*Rx(roll).
    %
    % FORMAT: rollPitchYaw = rollPitchYawFromRotation(R)
    %
    % INPUT:  - R = [3 * 3] rotation matrix
    %
    % OUTPUT: - rollPitchYaw = [3 * 1] vector of Euler angles [rad]
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    % For documentation, see also:
    %
    % http://wiki.icub.org/codyco/dox/html/idyntree/html/classiDynTree_1_1Rotation.html#a600352007d9250f7f227f21db85611f2
    %
    % http://www.geometrictools.com/Documentation/EulerAngles.pdf
    %
    rollPitchYaw = zeros(3,1);

    if (R(3,1) < +1)

        if (R(3,1) > -1)
            rollPitchYaw(2) = asin(-R(3,1));
            rollPitchYaw(3) = atan2(R(2,1),R(1,1));
            rollPitchYaw(1) = atan2(R(3,2), R(3,3));
        else
            % Not a unique solution : roll − yaw = atan2(−R23,R22)
            rollPitchYaw(2) = pi/2;
            rollPitchYaw(3) =-atan2(-R(2,3),R(2,2));
            rollPitchYaw(1) = 0;
        end
    else
        % Not a unique solution : roll − yaw = atan2(−R23,R22)
        rollPitchYaw(2) = -pi/2;
        rollPitchYaw(3) = atan2(-R(2,3),R(2,2));
        rollPitchYaw(1) = 0;
    end
end


function pass = test_sample( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e2*pref.techPrefs.chebfuneps;

% Example 1
f = ballfun(@(x,y,z)x);
X = sample(f);
Y = zeros(2,3,3);
Y(2,:,2) = [-1,0.5,0.5];
pass(1) = norm( X(:)-Y(:) ) < tol;

% Example 2
f = ballfun(@(x,y,z)z);
X = sample(f, 3, 1, 4);
r = [0;sqrt(0.5);1];
cosT = [1,0.5,-0.5,-1];
Y = reshape(r*cosT,3,1,4);
pass(2) = norm( X(:)-Y(:) ) < tol;

% Example 3
f = ballfun(@(x,y,z)z);
pass(3) = norm(ballfun(sample(f,4,4,4))-f) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function [] = test11()
% function test11()
%
% Try out centroid function in Nelder-Mead solver.
%

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    % Pick the manifold
    n = 10;
    k = 2;
%     M = euclideanfactory(n);
    M = obliquefactory(n, k);

    % Generate a bunch of points
    m = 25;
    x = cell(m, 1);
    for i = 1 : m
        x{i} = M.rand();
    end

    y = centroid(M, x);

    % This is for Euclidean only
    if strfind(M.name(), 'Euclidean')
        xx = zeros(n, 1);
        for i = 1 : m
            xx = xx + x{i}/m;
        end
        fprintf('Distance between mean point and centroid found: %e.\n',...
                 norm(xx-y));
    end

end


function r8vec_norm_li_test ( )

%*****************************************************************************80
%
%% R8VEC_NORM_LI_TEST tests R8VEC_NORM_LI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_NORM_LI_TEST\n' );
  fprintf ( 1, '  For an R8VEC:\n' );
  fprintf ( 1, '  R8VEC_NORM_LI:   L-infinity norm.\n' );

  b = -n;
  c = n;

  seed = 123456789;

  [ a, seed ] = r8vec_uniform_ab ( n, b, c, seed );

  r8vec_print ( n, a, '  Input vector:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  L-Infinity norm: %f\n', r8vec_norm_li ( n, a ) );

  return
end


% More complex example

% Written by Benjamin Irving 2013/03/25

% CREATING THE MATLAB EXAMPLE SURFACE
[X, Y, Z] = peaks;
Z=Z/2;
colormap hsv;

% example plot in matlab (just to illustrate the matlab output)
surf(X,Y,Z)
axis equal;

% CONVERT SURFACE TO A PATCH (faces and vertices)
fvc=surf2patch(X, Y, Z, Z, 'triangles');

%CONVERT THE COLORMAP INTO A COLORMAP FOR THE VERTICES (optional if you
%want to include color)
cmap=colormap;
%getting cdata
cdata=fvc.facevertexcdata;
%normalising cdata rangle
cdata=(cdata-min(cdata));cdata=cdata/max(cdata);
%scaling by cmap range and converting to integers
cdata=round(cdata.*(size(cmap,1)-1));
%getting cdata values
cdata=cmap(cdata+1,:);

% Create a second set of vertices with z positions inverted (as an example
% of a deformation)
fvc2=fvc;
fvc2.vertices(:,3)=-fvc2.vertices(:,3);

% EXPORT THE MESH TO HTML USING X3MESH
x3mesh_deform(fvc.faces, fvc.vertices, fvc2.vertices, 'name', 'Example2', 'color', cdata, 'speed', 5)


%LINEARPOLAR  Remaps an image to polar coordinates space
%
%     dst = cv.linearPolar(src, center, maxRadius)
%     dst = cv.linearPolar(..., 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ Source image.
% * __center__ The transformation center.
% * __maxRadius__ The radius of the bounding circle to transform. It
%   determines the inverse magnitude scale parameter too.
%
% ## Output
% * __dst__ Destination image. It will have same size and type as `src`.
%
% ## Options
% * __Interpolation__ Interpolation method, default 'Linear'. One of:
%   * __Nearest__ nearest neighbor interpolation
%   * __Linear__ bilinear interpolation
%   * __Cubic__ bicubic interpolation
%   * __Lanczos4__ Lanczos interpolation over 8x8 neighborhood
% * __FillOutliers__ flag, fills all of the destination image pixels. If some
%   of them correspond to outliers in the source image, they are set to zero.
%   default true
% * __InverseMap__ flag, inverse transformation, default false. For example,
%   polar transforms:
%   * flag is not set: Forward transformation `dst(rho,phi) = src(x,y)`
%   * flag is set: Inverse transformation `dst(x,y) = src(rho,phi)`
%
% Transform the source image using the following transformation:
%
%     dst(rho,phi) = src(x,y)
%     size(dst) <- size(src)
%
% where:
%
%     I = (dx,dy) = (x-center(2), y-center(1))
%     rho = Kx * magnitude(I)
%     phi = Ky * angle(I)_{0..360 deg}
%
% and:
%
%     Kx = size(src,2) / maxRadius
%     Ky = size(src,1) / 360
%
% Polar remaps reference:
%
% ![image](https://docs.opencv.org/3.3.1/polar_remap_doc.png)
%
% Note: To calculate magnitude and angle in degrees, cv.cartToPolar is used
% internally thus angles are measured from 0 to 360 with accuracy about 0.3
% degrees.
%
% See also: cv.logPolar, cv.remap
%


function toms097_test02 ( )

%*****************************************************************************80
%
%% TOMS097_TEST02 tests R8MAT_SHORTEST_PATH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2014
%
%  Author:
%
%    John Burkardt
%
  n = 6;

  a = [ ...
     0.0, -1.0, -1.0, -1.0, -1.0, -1.0; ...
     2.0,  0.0, -1.0, -1.0, -1.0,  5.0; ...
     5.0,  7.0,  0.0, -1.0,  2.0, -1.0; ...
    -1.0,  1.0,  4.0,  0.0, -1.0,  2.0; ...
    -1.0, -1.0, -1.0,  3.0,  0.0,  4.0; ...
    -1.0,  8.0, -1.0, -1.0,  3.0,  0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS097_TEST02\n' );
  fprintf ( 1, '  R8MAT_SHORTEST_PATH uses Floyd''s algorithm to find the\n' );
  fprintf ( 1, '  shortest distance between all pairs of nodes\n' );
  fprintf ( 1, '  in a directed graph, starting from the initial array\n' );
  fprintf ( 1, '  of direct node-to-node distances.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  In the initial direct distance array, if\n' );
  fprintf ( 1, '    A(I,J) = -1,\n' );
  fprintf ( 1, '  this indicates there is NO directed link from\n' );
  fprintf ( 1, '  node I to node J.  In that case, the value of\n' );
  fprintf ( 1, '  of A(I,J) is essentially "infinity".\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial direct-link distance matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '%10.4f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  for j = 1 : n
    for i = 1 : n
      if ( a(i,j) == -1.0 )
        a(i,j) = Inf;
      end
    end
  end

  a = r8mat_shortest_path ( n, a );

  for j = 1 : n
    for i = 1 : n
      if ( a(i,j) == Inf )
        a(i,j) = -1.0;
      end
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  In the final shortest distance array, if\n' );
  fprintf ( 1, '    A(I,J) = -1,\n' );
  fprintf ( 1, '  this indicates there is NO directed path from\n' );
  fprintf ( 1, '  node I to node J.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Final distance matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '%10.4f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function [w,mu,P]=RunnalsGaussMixRed(w,mu,P,K,gammaBound,KMax)
%%RUNNALSGAUSSMIXRED Perform Gaussian mixture reduction using the greedy
%                    merging algorithm by Runnals in [1].
%
%INPUTS: w An NX1 or 1XN vector of weights of the components of the
%          original Gaussian mixture.
%       mu An xDimXN matrix of the means of the vector components of the
%          original Gaussian mixture.
%        P An xDim XxDim XN hypermatrix of the covariance matrices for the
%          components of the original Gaussian mixture.
%        K The minimum desired number of components desired in the mixture
%          after reduction. This will be always achieved unless gammaBound
%          is set. Often, if gammaBound is set, one will just make K=1.
% gammaBound If the number of components is less than or equal to KMax,
%          then reduction will continue until the lowest cost between two
%          components is greater than gammaBound or there are K components
%          left. The default if this parameter is omitted or an empty
%          matrix is passed is Inf, which means that reduction will
%          continue until K components is reached.
%     KMax Regardless of the gammaBound setting, it won't be allowed that
%          more than KMax components are present. The default if omitted or
%          an empty matrix is passed is Inf.
%
%OUTPUTS: w The length K vector of weights of the mixture after reduction.
%           (the length can be less than K if the input mixture had fewer
%           than K components).
%        mu The xDimXK means of the mixture after reduction.
%         P The xDimXxDimXK covariance matrices of the mixture after
%           reduction.
%
%This implements the suboptimal Gaussian mixture reduction algorithm of
%[1]. Care is taken in the implementation to avoid copies and reallocations
%associated with actually resizing the cost array every time something is
%eliminated.
%
%EXAMPLE 1:
%This scalar example plots a full 9-component PDF and then the PDF reduced
%to 5 components using sequential brute-force reduction and Runnals'
%algorithm. Increasing the number of components maintained to 6 makes
%the results of Runnals' algorithm much closer to that of the brute-force
%solution.
% w=[0.03, 0.18, 0.12, 0.19, 0.02, 0.16, 0.06, 0.1, 0.08, 0.06];
% mu=[1.45, 2.20, 0.67, 0.48, 1.49, 0.91, 1.01, 1.42, 2.77, 0.89];
% P=reshape([0.0487, 0.0305, 0.1171, 0.0174, 0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679],[1,1,10]);
%
% K=6;
% %Runnals' cost function.
% [wRed,muRed,PRed]=RunnalsGaussMixRed(w,mu,P,K,[],[],0);
% %Sequential brute-force reduction.
% [wRedBF,muRedBF,PRedBF]=bruteForceGaussMixRed(w,mu,P,K,true);
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRedBF,muRedBF,PRedBF);
% PDFVals3=GaussianMixtureD.PDF(xVals,wRed,muRed,PRed);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'--r','linewidth',2)
% plot(xVals,PDFVals3,'-m','linewidth',1)
% legend('Full Mixture','Brute-Force','Cost Function of Runnalls')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%EXAMPLE 2:
%This is similar to example 1, but we only consider Runnal's cost function
%and we set K=1 and use gammaVal to control how many elements it ultimately
%has in the end. In this example, it reduced to from 10 to KNew=6
%components.
% w=[0.03, 0.18, 0.12, 0.19, 0.02, 0.16, 0.06, 0.1, 0.08, 0.06];
% mu=[1.45, 2.20, 0.67, 0.48, 1.49, 0.91, 1.01, 1.42, 2.77, 0.89];
% P=reshape([0.0487, 0.0305, 0.1171, 0.0174, 0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679],[1,1,10]);
%
% K=1;
% gammaVal=0.05;
% %Runnals' cost function.
% [wRed,muRed,PRed]=RunnalsGaussMixRed(w,mu,P,K,gammaVal);
% KNew=length(wRed)%The number of reduced components.
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRed,muRed,PRed);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'-m','linewidth',1)
% legend('Full Mixture','Reduced Mixture')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] A. R. Runnalls, "Kullback-Leibler approach to Gaussian mixture
%    reduction," IEEE Transactions on Aerospace and Electronic Systems,
%    vol. 43, no. 3, pp. 989-999, Jul. 2007.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<5||isempty(gammaBound))
        gammaBound=Inf;
    end

    if(nargin<6||isempty(KMax))
        KMax=Inf;
    end

    N=length(w);

    %If no reduction is necessary.
    if(N<=K)
        return;
    end

    %We will only be using one triangular of this matrix.
    M=Inf*ones(N,N);%This is the cost matrix.
    wLogDetPVals=zeros(N,1);
    for k=1:N
        wLogDetPVals(k)=w(k)*log(det(P(:,:,k)));
    end

    %We shall fill the cost matrix with the cost of all pairs.
    for cur1=1:(N-1)
        for cur2=(cur1+1):N
            M(cur1,cur2)=BDist(w(cur1),w(cur2),mu(:,cur1),mu(:,cur2),P(:,:,cur1),P(:,:,cur2),wLogDetPVals(cur1),wLogDetPVals(cur2));
        end
    end

    KCur=N;
    selIdxPresent=true(N,1);
    for mergeRound=1:(N-K)
        [Mm,minRows]=min(M);%Minimize over the rows
        [minVal,minCol]=min(Mm);%Minimize over the columns.
        minRow=minRows(minCol);

        if(minVal>gammaBound&&KCur<=KMax)
            %If the distribution has been sufficiently reduced in terms of
            %cost.
            break;
        end

        %Now we know which two hypotheses to merge: The ones with indices
        %minRow and minCol. We will merge those hypotheses and put the
        %results in minRow.
        wSum=w(minRow)+w(minCol);
        w1=w(minRow)/wSum;
        w2=w(minCol)/wSum;
        muMerged=w1*mu(:,minRow)+w2*mu(:,minCol);
        diff1=mu(:,minRow)-muMerged;
        diff2=mu(:,minCol)-muMerged;
        PMerged=w1*(P(:,:,minRow)+diff1*diff1')+w2*(P(:,:,minCol)+diff2*diff2');
        w(minRow)=wSum;
        mu(:,minRow)=muMerged;
        P(:,:,minRow)=PMerged;
        wLogDetPVals(minRow)=wSum*log(det(PMerged));

        %The column is removed.
        selIdxPresent(minCol)=false;

        %Make all the costs Inf so that nothing will be assigned to the
        %removed column.
        M(minCol,:)=Inf;
        M(:,minCol)=Inf;

        %We must now fill in the costs for the merged estimate, which is in
        %minRow. We shall fill the cost matrix with the cost of all pairs.
        for cur1=1:(minRow-1)
            if(selIdxPresent(cur1))
                M(cur1,minRow)=BDist(w(cur1),w(minRow),mu(:,cur1),mu(:,minRow),P(:,:,cur1),P(:,:,minRow),wLogDetPVals(cur1),wLogDetPVals(minRow));
            end
        end

        for cur2=(minRow+1):N
            if(selIdxPresent(cur2))
                M(minRow,cur2)=BDist(w(minRow),w(cur2),mu(:,minRow),mu(:,cur2),P(:,:,minRow),P(:,:,cur2),wLogDetPVals(minRow),wLogDetPVals(cur2));
            end
        end

        KCur=KCur-1;
    end

    %The merged items are marked with selIdxPresent and can now be all
    %grouped together to be returned.
    w=w(selIdxPresent);
    w=w/sum(w);%Guarantee continued normalization.
    mu=mu(:,selIdxPresent);
    P=P(:,:,selIdxPresent);
end

function val=BDist(w1,w2,mu1,mu2,P1,P2,wLogDetP1,wLogDetP2)
%%BDIST This is the distance measure given in Equation 21 in Section VI-B
%       in [1]. This is a bound related the Kullback-Leibler divergence.
%
%REFERENCES:
%[1] A. R. Runnalls, "Kullback-Leibler approach to Gaussian mixture
%    reduction," IEEE Transactions on Aerospce and Electronic Systems,
%    vol. 43, no. 3, pp. 989-999, Jul. 2007.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

    wSum=w1+w2;
    w1m=w1/wSum;
    w2m=w2/wSum;

    %In [1], there is a constant factor of (1/2) out front, but since all
    %components have it, we omit it here. Also, the dterminant is a slow
    %step, so we put in explicit determinants for things less than 4D.
    switch(size(mu1,1))
        case 1
            diff=mu1-mu2;
            P=w1m*P1+w2m*P2+w1m*w2m*(diff*diff);
            val=wSum*log(P)-wLogDetP1-wLogDetP2;
        case 2
            diff1=mu1(1)-mu2(1);
            diff2=mu1(2)-mu2(2);
            wmProd=w1m*w2m;
            val=wSum*log((w1m*P1(1,1)+w2m*P2(1,1)+wmProd*(diff1*diff1))*(w1m*P1(2,2)+w2m*P2(2,2)+wmProd*(diff2*diff2))-(w1m*P1(1,2)+w2m*P2(1,2)+wmProd*(diff1*diff2)).^2)-wLogDetP1-wLogDetP2;
        case 3
            diff=mu1-mu2;
            P=w1m*P1+w2m*P2+w1m*w2m*(diff*diff');
            detVal=-P(1,3)*P(2,2)*P(3,1)+P(1,2)*P(2,3)*P(3,1)+P(1,3)*P(2,1)*P(3,2)-P(1,1)*P(2,3)*P(3,2)-P(1,2)*P(2,1)*P(3,3)+P(1,1)*P(2,2)*P(3,3);
            val=wSum*log(detVal)-wLogDetP1-wLogDetP2;
        otherwise
            diff=mu1-mu2;
            P12=w1m*P1+w2m*P2+w1m*w2m*(diff*diff');
            val=wSum*log(det(P12))-wLogDetP1-wLogDetP2;
    end

    %Deal with the case where w1 and w2 are both essentially zero.
    if(~isfinite(val))
        val=0;
    end
end

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


function days = days_before_month_common ( y, m )

%*****************************************************************************80
%
%% DAYS_BEFORE_MONTH_COMMON returns the number of days before a Common month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of
%    days in the year before the first day of the given month.
%
  mdays = [ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_common ( y2, m2 );

  if ( ierror ~= 0 )
    days = 0;
    return
  end

  days = mdays ( m2 );

  if ( 2 < m2 && year_is_leap_common ( y2 ) )
    days = days + 1;
  end

  return
end