function [HR, HRS, HlR, HlRS, HiR, HiRS, ChiR, HshR, HshRS] = gaussian_method_v7_1_0(R, pars)
%GAUSSIAN_METHOD_V7_1_0 Estimate entropy values using the Gaussian Method.
%
% ---------
% ARGUMENTS
% ---------
% R    - response matrix
% pars - parameters structure

%   Copyright (C) 2009 Cesare Magri
%   Version 7.1.0

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

persistent previous_Nt previous_mask;

% HiR, HiRS and ChiR are not computed for the gaussian case, null values
% are returned instead:
HiR  = NaN;
HiRS = NaN;
ChiR = NaN;

% We have to recompute Nc, Nt, maxNt (in the case this routine is called by
% quadratic extrapolation)
Nt    = pars.Nt;
Nc    = size(R,1);
maxNt = size(R,2);
totNt = sum(Nt);
Ns    = pars.Ns;

logOf2times2 = 2*log(2);
logOf2piTimesExp1 = log(2*pi*exp(1));

if ~isequal(previous_Nt, Nt)
  mask = build_logical_mask(Nt, maxNt, Ns);
else
	mask = previous_mask;
end
R(:,~mask) = nan;

% H(R) and H_lin(R) -------------------------------------------------------
if pars.doHR || pars.doHlR

    % Removing mean:
    %meanRvec = mean(R(:,mask),2);
		meanRvec = nansum(nansum(R,3),2)./totNt; %JM: this is faster, some juggling is needed to account for the -1's

	  %Rvec = R(:,mask) - meanRvec(:,ones(totNt,1));
    Rvec = R(:,mask) - meanRvec*ones(1,totNt);

    covPr = (Rvec * Rvec') ./ (totNt-1);
end

% H(R)
if pars.doHR
    HR = (Nc * logOf2piTimesExp1 + log(det(covPr))) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HRbias = gaussian_bias(totNt, Nc);
        HR = HR - HRbias;
    end
else
    HR = 0;
end

% H_lin(R)
if pars.doHlR
    HlR = (Nc * logOf2piTimesExp1 + nansum(log(diag(covPr)), 1)) / logOf2times2;

    % Bias correction:
    if pars.biasCorrNum==3
        HlRbias = Nc * gaussian_bias(totNt, 1);
        HlR = HlR - HlRbias;
    end
else
    HlR = 0;
end

% H(R|S) and H_lin(R|S) ---------------------------------------------------
if pars.doHRS || pars.doHlRS

    % Removing mean
    meanR = nansum(R.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
    Rzeromean = R - meanR(:, ones(maxNt,1), :);

    if Nc==1
        % HlRS is never computed for Nc==1, if we got here it's cause we
        % need to compute H(R|S):

				%covPrsMat = covPrsForSingletonNc(Rzeromean, Nt, mask, maxNt, Ns);
        covPrsMat = reshape(nansum(Rzeromean.*Rzeromean,2),1,[])./(Nt'-1); %JM:

        HRS  = (Nc * logOf2piTimesExp1 + log(covPrsMat(:))) / logOf2times2;

        HlRS = 0;

        % Bias correction:
        if pars.biasCorrNum==3
            HRSbias = gaussian_bias(Nt, 1);
            HRS = HRS - HRSbias;
        end
    else
        HRS  = zeros(Ns,1);
        HlRS = zeros(Ns,1);

        for s=1:Ns
            covPrs = Rzeromean(:, 1:Nt(s), s) * Rzeromean(:, 1:Nt(s), s).' ./ (Nt(s)-1);

            if pars.doHRS
                detCovPrs = det(covPrs);

                HRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPrs)) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HRSbias = gaussian_bias(Nt(s), Nc);
                    HRS(s) = HRS(s) - HRSbias;
                end
            end;

            if pars.doHlRS
                diagCovPrs = diag(covPrs);

                HlRS(s) = (length(diagCovPrs) * logOf2piTimesExp1 + sum(log(diagCovPrs))) / logOf2times2;

                % Bias correction:
                if pars.biasCorrNum==3
                    HlRSbias = Nc * gaussian_bias(Nt(s), 1);
                    HlRS(s) = HlRS(s) - HlRSbias;
                end
            end
        end
    end

    if ~pars.doHRS
        HRS = 0;
    end

    if ~pars.doHlRS
        HlRS = 0;
    end

else
    HRS  = 0;
    HlRS = 0;
end

% H_sh(R) and H_sh(R|S) ---------------------------------------------------
if pars.doHshR || pars.doHshRS

    Rsh = shuffle_R_across_cells(R, Nt);

    if pars.doHshR
        % Removing mean:
        meanRshvec = mean(Rsh(:,mask),2);
        Rshvec = Rsh(:,mask) - meanRshvec(:,ones(totNt,1));

        covPshr = (Rshvec * Rshvec') ./ (totNt-1);

        HshR = (Nc * logOf2piTimesExp1 + log(det(covPshr))) / logOf2times2;

        % Bias correction:
        if pars.biasCorrNum==3
            HshRbias = gaussian_bias(totNt, Nc);
            HshR = HshR - HshRbias;
        end
    else
        HshR = 0;
    end

    % Remark: HshRS not computed for Nc==1
    if pars.doHshRS
        % Removing mean
        meanRsh = sum(Rsh.*permute(mask(:,:,ones(Nc,1)), [3 1 2]), 2) ./ permute(Nt(:, ones(Nc,1)), [2 3 1]);
        Rsh = Rsh - meanRsh(:,ones(maxNt,1),:);

        HshRS = zeros(Ns,1);
        for s=1:Ns
            covPshrs = Rsh(:, 1:Nt(s), s) * Rsh(:, 1:Nt(s), s).' ./ (Nt(s)-1);
            detCovPshrs = det(covPshrs);

            HshRS(s) = (Nc * logOf2piTimesExp1 + log(detCovPshrs)) / logOf2times2;
        end

        % Bias correction:
        if pars.biasCorrNum==3
            HshRSbias = gaussian_bias(Nt(:), Nc);
            HshRS = HshRS - HshRSbias;
        end
    else
        HshRS = 0;
    end

else
    HshR  = 0;
    HshRS = 0;
end

previous_Nt   = Nt;
previous_mask = mask;

function covPrs = covPrsForSingletonNc(R, Nt, mask, maxNt, Ns)

% Removing first singleton dimension:
R2D = zeros(maxNt, Ns);
R2D(mask) = R(1,mask);

% Computing the variance (R.*R faster than R.^2):
covPrs = (sum(R2D.*R2D,1)) ./ (Nt.' - 1);


%% Physics-informed dynamic mode decompositions
%
% Computes a dynamic mode decomposition when the solution matrix is
% constrained to lie in a matrix manifold.
% The options available for the "method" so far are
%
% - "exact", "exactSVDS"
% - "orthogonal"
% - "uppertriangular, "lowertriangular"
% - "diagonal", "diagonalpinv", "diagonaltls", "symtridiagonal"
% - "circulant", "circulantTLS", "circulantunitary", "circulantsymmetric",
% "circulantskewsymmetric"
% - "BCCB", "BCCBtls", "BCCBskewsymmetric",
% "BCCBunitary", "hankel", "toeplitz"
% - "symmetric", "skewsymmetric"
%
function [A, varargout] = piDMD(X,Y,method,varargin)

[nx, nt] = size(X);

if strcmp(method,'exact') || strcmp(method,'exactSVDS')

    if nargin>3
        r = varargin{1};
    else
        r = min(nx,nt);
    end

    if strcmp(method,'exact')
        [Ux,Sx,Vx] = svd(X,0);
        Ux = Ux(:,1:r); Sx = Sx(1:r,1:r); Vx = Vx(:,1:r);
    elseif strcmp(method,'exactSVDS')
        [Ux,Sx,Vx] = svds(X,r);
    end

    Atilde = (Ux'*Y)*Vx*pinv(Sx);
    A = @(v) Ux*(Atilde*(Ux'*v));

    if nargout==2
    varargout{1} = eig(Atilde);
    elseif nargout>2
    [eVecs,eVals] = eig(Atilde);
    eVals = diag(eVals);
    eVecs = Y*Vx*pinv(Sx)*eVecs./eVals.';
    varargout{1} = eVals;
    varargout{2} = eVecs;
    end

elseif strcmp(method,'orthogonal')

    if nargin>3
        r = varargin{1};
    else
        r = min(nx,nt);
    end
    [Ux,~,~] = svd(X,0); Ux = Ux(:,1:r);
    Yproj = Ux'*Y; Xproj = Ux'*X; % Project X and Y onto principal components
    [Uyx, ~, Vyx] = svd(Yproj*Xproj',0);
    Aproj = Uyx*Vyx';
    A = @(x) Ux*(Aproj*(Ux'*x));

    if nargout==2
        eVals = eig(Aproj);
        varargout{1} = eVals;
    elseif nargout>2
        [eVecs,eVals] = eig(Aproj);
        varargout{1} = diag(eVals);
        varargout{2} = Ux*eVecs;
        if nargout > 3; varargout{3} = Aproj; end
    end

elseif strcmp(method,'uppertriangular')

    [R,Q] = rq(X); % Q*Q' = I
    Ut = triu(Y*Q');
    A = Ut/R;

elseif strcmp(method,'lowertriangular')

    A = rot90(piDMD(flipud(X),flipud(Y),'uppertriangular'),2);

% The codes allows for matrices of variable banded width. The fourth input,
% a 2xn matrix called d, specifies the upper and lower bounds of the
% indices of the non-zero elements. The first column corresponds to the width of
% the band below the diagonal and the second column is the width of the
% band above. For example, a diagonal matrix would have d = ones(nx,2) and
% a tridiagonal matrix would have d = [2 2]+zeros(nx,2). If you only specify
% d as a scalar then the algorithm converts the input to obtain a banded
% diagonal matrix of width d.
elseif startsWith(method,'diagonal')
    if nargin>3
        d = varargin{1}; % arrange d into an nx-by-2 matrix
        if numel(d) == 1
            d = d*ones(nx,2);
        elseif numel(d) == nx
             d = repmat(d,[1,2]);
        elseif any(size(d)~=[nx,2])
            error('Diagonal number is not in an allowable format.')
        end
    else
        d = ones(nx,2); % default is for a diagonal matrix
    end
    % Allocate cells to build sparse matrix
    Icell = cell(1,nx); Jcell = cell(1,nx); Rcell = cell(1,nx);
    for j = 1:nx
    l1 = max(j-(d(j,1)-1),1); l2 = min(j+(d(j,2)-1),nx);
    C = X(l1:l2,:); b = Y(j,:); % preparing to solve min||Cx-b|| along each row
    if strcmp(method,'diagonal')
            sol = b/C;
    elseif strcmp(method,'diagonalpinv')
            sol = b*pinv(C);
    elseif strcmp(method,'diagonaltls')
            sol = tls(C.',b.').';
    end
    Icell{j} = j*ones(1,1+l2-l1); Jcell{j} = l1:l2; Rcell{j} = sol;
    end
    Imat = cell2mat(Icell); Jmat = cell2mat(Jcell); Rmat = cell2mat(Rcell);
    Asparse = sparse(Imat,Jmat,Rmat,nx,nx);
    A = @(v) Asparse*v;

    if nargout==2
        eVals = eigs(Asparse,nx);
        varargout{1} = eVals;
    elseif nargout>2
        [eVecs, eVals] = eigs(Asparse,nx);
        varargout{1} = diag(eVals); varargout{2} = eVecs;
    end

elseif strcmp(method,'symmetric') || strcmp(method,'skewsymmetric')

[Ux,S,V] = svd(X,0);
C = Ux'*Y*V;
C1 = C;
if nargin>3; r = varargin{1}; else; r = rank(X); end
Ux = Ux(:,1:r);
Yf = zeros(r);
    if strcmp(method,'symmetric')
    for i = 1:r
        Yf(i,i) = real(C1(i,i))/S(i,i);
        for j = i+1:r
            Yf(i,j) = (S(i,i)*conj(C1(j,i)) + S(j,j)*C1(i,j)) / (S(i,i)^2 + S(j,j)^2);
        end
    end
    Yf = Yf + Yf' - diag(diag(real(Yf)));
    elseif strcmp(method,'skewsymmetric')
    for i = 1:r
        Yf(i,i) = 1i*imag(C1(i,i))/S(i,i);
        for j = i+1:nx
            Yf(i,j) = (-S(i,i)*conj(C1(j,i)) + S(j,j)*(C1(i,j))) / (S(i,i)^2 + S(j,j)^2);
        end
    end
    Yf = Yf - Yf' - 1i*diag(diag(imag(Yf)));
    end

    A = @(v) Ux*Yf*(Ux'*v);

    if nargout==2
        varargout{1} = eig(Yf);
    elseif nargout>2
        [eVecs,eVals] = eig(Yf);
        eVals = diag(eVals);
        eVecs = Ux*eVecs;
        varargout{1} = eVals;
        varargout{2} = eVecs;
    end

elseif strcmp(method,'toeplitz') || strcmp(method,'hankel')
   if  strcmp(method,'toeplitz'); J = eye(nx);
   elseif strcmp(method,'hankel'); J = fliplr(eye(nx)); end
    Am = fft([eye(nx) zeros(nx)].',[],1)'/sqrt(2*nx); % Define the left matrix
    B = fft([(J*X)' zeros(nt,nx)].',[],1)'/sqrt(2*nx); % Define the right matrix
    BtB = B'*B;
    AAt = ifft(fft([eye(nx) zeros(nx); zeros(nx,2*nx)]).').'; % Fast computation of A*A'
    y = diag(Am'*conj(Y)*B)'; % Construct the RHS of the linear system
    L = (AAt.*BtB.')'; % Construct the matrix for the linear system
    d = [y(1:end-1)/L(1:end-1,1:end-1) 0]; % Solve the linear system
    newA = ifft(fft(diag(d)).').'; % Convert the eigenvalues into the circulant matrix
    A = newA(1:nx,1:nx)*J; % Extract the Toeplitz matrix from the circulant matrix

elseif startsWith(method,'circulant')

 fX = fft(X); fY = fft(conj(Y));

 d = zeros(nx,1);
    if endsWith(method,'TLS') % Solve in the total least squares sense
        for j = 1:nx
        d(j) = tls(fX(j,:)',fY(j,:)');
        end
    elseif ~endsWith(method,'TLS') % Solve the other cases
    d = diag(fX*fY')./vecnorm(fX,2,2).^2;
        if endsWith(method,'unitary'); d = exp(1i*angle(d));
        elseif endsWith(method,'symmetric'); d = real(d);
        elseif endsWith(method,'skewsymmetric'); d = 1i*imag(d);
        end
    end
  eVals = d; % These are the eigenvalues
  eVecs = fft(eye(nx)); % These are the eigenvectors
 if nargin>3
    r = varargin{1}; % Rank constraint
    res = diag(abs(fX*fY'))./vecnorm(fX')'; % Identify least important eigenvalues
    [~,idx] = mink(res,nx-r); % Remove least important eigenvalues
    d(idx) = 0; eVals(idx) = []; eVecs(:,idx) = [];
 end

 if nargout>1; varargout{1} = eVals; end
 if nargout>2; varargout{2} = eVecs; end

 A = @(v) fft(d.*ifft(v)); % Reconstruct the operator in terms of FFTs

elseif strcmp(method,'BCCB') || strcmp(method,'BCCBtls') || strcmp(method,'BCCBskewsymmetric') || strcmp(method,'BCCBunitary')

    if isempty(varargin); error('Need to specify size of blocks.'); end
    s = varargin{1}; p = prod(s);
    % Equivalent to applying the block-DFT matrix F
    % defined by F = kron(dftmtx(M),dftmtx(N)) to the
    % matrix X
    aF =  @(x) reshape(     fft2(reshape(x ,[s,size(x,2)])) ,[p,size(x,2)])/sqrt(p);
    aFt = @(x) conj(aF(conj(x)));
    fX = aF(conj(X)); fY = aF(conj(Y));
    d = zeros(p,1);

    if strcmp(method,'BCCB')
    for j = 1:p; d(j) = conj(fX(j,:)*fY(j,:)')/norm(fX(j,:)').^2; end
    elseif strcmp(method,'BCCBtls')
    for j = 1:p; d(j) = tls(fX(j,:)',fY(j,:)')'; end
    elseif strcmp(method,'BCCBskewsymmetric')
    for j = 1:p; d(j) = 1i*imag(fY(j,:)/fX(j,:)); end
    elseif strcmp(method,'BCCBsymmetric')
    for j = 1:p; d(j) = real(fY(j,:)/fX(j,:)); end
    elseif strcmp(method,'BCCBunitary')
    for j = 1:p; d(j) = exp(1i*angle(fY(j,:)/fX(j,:))); end
    end

    % Returns a function handle that applies A
     if nargin>4
        r = varargin{2};
        res = diag(abs(fX*fY'))./vecnorm(fX')';
        [~,idx] = mink(res,nx-r);
        d(idx) = 0;
    end
    A = @(x) aF((conj(d).*aFt(x)));
    varargout{1} = d;
    % Eigenvalues are given by d

elseif strcmp(method,'BC') || strcmp(method,'BCtri') || strcmp(method,'BCtls')

    s = varargin{1}; p = prod(s);
        M = s(2); N = s(1);
    if isempty(s); error('Need to specify size of blocks.'); end
    % Equivalent to applying the block-DFT matrix F
    % defined by F = kron(dftmtx(M),eye(N)) to the
    % matrix X
aF  =  @(x) reshape(fft(reshape(x,[s,size(x,2)]),[],2) ,[p,size(x,2)])/sqrt(M);
aFt =  @(x) conj(aF(conj(x)));

fX = aF(X); fY = aF(Y);
    d = cell(M,1);

for j = 1:M
    ls = (j-1)*N + (1:N);
    if strcmp(method,'BC')
        d{j} = fY(ls,:)/fX(ls,:);
    elseif strcmp(method,'BCtri')
        d{j} = piDMD(fX(ls,:),fY(ls,:),'diagonal',2);
    elseif strcmp(method,'BCtls')
        d{j} = tls(fX(ls,:)',fY(ls,:)')';
    end
end

    BD = blkdiag(d{:});
    A = @(v) aFt(BD*aF(v));

elseif strcmp(method,'symtridiagonal')

    T1e = vecnorm(X,2,2).^2; % Compute the entries of the first block
    T1 = spdiags(T1e,0,nx,nx); % Form the leading block
    T2e = dot(X(2:end,:),X(1:end-1,:),2); % Compute the entries of the second block
    T2 = spdiags([T2e T2e],-1:0,nx,nx-1); % Form the second and third blocks
    T3e = [0; dot(X(3:end,:),X(1:end-2,:),2)]; % Compute the entries of the final block
    T3 = spdiags(T1e(1:end-1) + T1e(2:end),0,nx-1,nx-1) ...
         + spdiags(T3e,1,nx-1,nx-1) + spdiags(T3e,1,nx-1,nx-1)'; % Form the final block
    T = [T1 T2; T2' T3]; % Form the block tridiagonal matrix
    d = [dot(X,Y,2); dot(X(1:end-1,:),Y(2:end,:),2) + dot(X(2:end,:),Y(1:end-1,:),2)]; % Compute the RHS vector
    c = real(T)\real(d); % Take real parts then solve linear system
    % Form the solution matrix
    A = spdiags(c(1:nx),0,nx,nx) + spdiags([0;c(nx+1:end)],1,nx,nx) + spdiags([c(nx+1:end); 0],-1,nx,nx);
else
    error('The selected method doesn''t exist.');

end


function [rpm] = MHz2rpm(MHz)
% Convert frequency from megahertz to revolutions per minute.
% Chad A. Greene 2012
rpm = MHz*60*1e+6;


function [uu,Up,Uc] = depixellise(pix,cal)

% DEPIXELLISE  Pixellic to metric conversion
%   DEPIXELLISE(PIX,CAL) returns the metric coordinates of an image pixel
%   PIX given camera intrinsic parameters CAL = [u0 v0 au av]'
%
%   [u,Up,Uc] = DEPIXELLISE(...) gives the jacobians wrt PIX and CAL.
%
%   See also INVPINHOLE.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

u0 = cal(1);
v0 = cal(2);
au = cal(3);
av = cal(4);

u = pix(1,:);
v = pix(2,:);

uu = [(u-u0)/au
    (v-v0)/av];

if nargout > 1
    Up = [...
        [ 1/au,    0]
        [    0, 1/av]];
    Uc = [...
        [        -1/au,            0, (-u+u0)/au^2,            0]
        [            0,        -1/av,            0, (-v+v0)/av^2]];
end

return

%% jacobians test

syms u0 v0 au av u v real
pix = [u;v];
cal = [u0;v0;au;av];

[u,Up,Uc] = depixellise(pix,cal);

simplify(Up - jacobian(u,pix))
simplify(Uc - jacobian(u,cal))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


% Mathematics Q1344369
% https://math.stackexchange.com/questions/1344369
% Compressive Sensing over the Complex Domain
% References:
%   1.  aa
% Remarks:
%   1.  sa
% TODO:
% 	1.  ds
% Release Notes
% - 1.0.000     14/04/2018
%   *   First release.

%% General Parameters

run('InitScript.m');

figureIdx           = 0;
figureCounterSpec   = '%04d';

generateFigures = ON;

%% Simulation Parameters

numRows = 64;
numCols = 256;

paramLambda = 4.0;

numIterations = 1000;

cSolversFun = {@(mA, vB, paramLambda, numIterations) SolveLsL1ComplexSubGrad(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealSubGrad(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexPgm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealPgm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexAdmm(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexIrls(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexCd(mA, vB, paramLambda, numIterations); ...
    @(mA, vB, paramLambda, numIterations) SolveLsL1ComplexRealCd(mA, vB, paramLambda, numIterations);};

cMethodString = {['Sub Gradient Method']; ['Sub Gradient Method - Real Domain']; ...
    ['Proximal Gradient Method']; ['Proximal Gradient Method - Real Domain']; ...
    ['ADMM Method']; ['Fixed Point Iteration (IRLS) Method']; ...
    ['Coordinate Descent (CD) Method']; ['Coordinate Descent (CD) Method - Real Domain']};

%% Generate Data

mA = randn([numRows, numCols]) + (1i * randn([numRows, numCols]));
vB = randn([numRows, 1]) + (1i * randn([numRows, 1]));

hCalcErrorNorm = @(mX, vXRef) sum((mX - vXRef) .* conj(mX - vXRef));
hCalcObjFunVal = @(mX) 0.5 * sum(((mA * mX) - vB) .* conj((mA * mX) - vB)) + paramLambda * sum(abs(mX), 1);

%% Solution by CVX

tic();
cvx_begin quiet
    variable vXCvx(numCols) complex
    minimize( (0.5 * sum_square_abs((mA * vXCvx) - vB)) + paramLambda * norm(vXCvx, 1) )
cvx_end
toc();

% disp([' ']);
% disp(['CVX Solution Summary']);
% disp(['The CVX Solver Status - ', cvx_status]);
% disp(['The Optimal Value Is Given By - ', num2str(cvx_optval)]);
% disp(['The Optimal Argument Is Given By - [ ', num2str(vXCvx.'), ' ]']);
% disp([' ']);

%% Method Analysis

numMethods = length(cSolversFun);

vRunTime = zeros([numMethods, 1]);
mErrorNorm = zeros([numIterations, numMethods]);
mObjFunVal = zeros([numIterations, numMethods]);

for ii = 1:numMethods
    hRunTime = tic();
    [vX, mX] = cSolversFun{ii}(mA, vB, paramLambda, numIterations);
    runTime = toc(hRunTime);

    vRunTime(ii)         = runTime;
    mErrorNorm(:, ii)    = hCalcErrorNorm(mX, vXCvx);
    mObjFunVal(:, ii)    = hCalcObjFunVal(mX);
end

%% Display Results

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot([1:numIterations], 10 * log10(mErrorNorm));
set(hLineSeries, 'LineWidth', lineWidthNormal);
set(get(hAxes, 'Title'), 'String', ['The Error Norm - $ {\left\| {x}^{k} - {x}_{CVX} \right\|}_{2}^{2} $'], ...
    'FontSize', fontSizeTitle, 'Interpreter', 'latex');
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Error Norm [dB]', ...
    'FontSize', fontSizeAxis);
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);
hLegend = ClickableLegend(cMethodString);
set(hLegend, 'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
set(hAxes, 'NextPlot', 'add');
hLineSeries = plot([1:numIterations], mObjFunVal);
set(hLineSeries, 'LineWidth', lineWidthNormal);
% set(hLineSeries(5), 'LineStyle', ':', 'LineWidth', lineWidthThin);
set(get(hAxes, 'Title'), 'String', ['The Objection Function Value - $ \frac{1}{2} {\left\| A {x}^{k} - b \right\|}_{2}^{2} + \lambda {\left\| {x}^{k} \right\|}_{1} $'], ...
    'FontSize', fontSizeTitle, 'Interpreter', 'latex');
set(get(hAxes, 'XLabel'), 'String', 'Iteration Number', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Objective Function Value', ...
    'FontSize', fontSizeAxis);
set(hAxes, 'LooseInset', [0.07, 0.07, 0.07, 0.07]);
hLegend = ClickableLegend(cMethodString);
set(hLegend, 'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

figureIdx = figureIdx + 1;

hFigure     = figure('Position', figPosLarge);
hAxes       = axes();
hBarObj = bar(1:length(vRunTime), vRunTime);
% set(hLineSeries, 'LineWidth', lineWidthNormal);
% set(hLineSeries(2:end), 'LineStyle', ':');
set(get(hAxes, 'Title'), 'String', ['L_1 Regularized Least Squares - Methods Run Time'], ...
    'FontSize', fontSizeTitle);
set(hAxes, 'XTickLabel', cMethodString, 'XTickLabelRotation', 45);
set(get(hAxes, 'XLabel'), 'String', 'Method', ...
    'FontSize', fontSizeAxis);
set(get(hAxes, 'YLabel'), 'String', 'Run Time [Sec]', ...
    'FontSize', fontSizeAxis);

if(generateFigures == ON)
    saveas(hFigure,['Figure', num2str(figureIdx, figureCounterSpec), '.png']);
end

%% Restore Defaults

% set(0, 'DefaultFigureWindowStyle', 'normal');
% set(0, 'DefaultAxesLooseInset', defaultLoosInset);


function V = mttkrp(X,U,n)
%MTTKRP Matricized tensor times Khatri-Rao product for sparse tensor.
%
%   V = MTTKRP(X,U,n) efficiently calculates the matrix product of the
%   n-mode matricization of X with the Khatri-Rao product of all
%   entries in U, a cell array of matrices, except the nth.  How to
%   most efficiently do this computation depends on the type of tensor
%   involved.
%
%   See also SPTENSOR, TENSOR/MTTKRP, SPTENSOR/TTV
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% In the sparse case, it is most efficient to do a series of TTV operations
% rather than forming the Khatri-Rao product.

N = ndims(X);

if (n == 1)
    R = size(U{2},2);
else
    R = size(U{1},2);
end

V = zeros(size(X,n),R);
for r = 1:R
    % Set up cell array with appropriate vectors for ttv multiplication
    Z = cell(N,1);
    for i = [1:n-1,n+1:N]
        Z{i} = U{i}(:,r);
    end
    % Perform ttv multiplication
    V(:,r) = double(ttv(X, Z, -n));
end


function [x ft] = EProjSimplex_new(v, k)

%
%% Problem
%
%  min  1/2 || x - v||^2
%  s.t. x>=0, 1'x=1
%

if nargin < 2
    k = 1;
end;

ft=1;
n = length(v);

v0 = v-mean(v) + k/n;
%vmax = max(v0);
vmin = min(v0);
if vmin < 0
    f = 1;
    lambda_m = 0;
    while abs(f) > 10^-10
        v1 = v0 - lambda_m;
        posidx = v1>0;
        npos = sum(posidx);
        g = -npos;
        f = sum(v1(posidx)) - k;
        lambda_m = lambda_m - f/g;
        ft=ft+1;
        if ft > 100
            x = max(v1,0);
            break;
        end;
    end;
    x = max(v1,0);
else
    x = v0;
end;


function I = integral(f, varargin)
%INTEGRAL   Line integral of a CHEBFUN3 over a parametric curve.
%   I = INTEGRAL(F, G), returns the integral of the CHEBFUN3 object F
%   along a parametric curve defined by the Inf x 3 quasimatrix G. Columns
%   of G represent parametrization of the 3D curve.
%
%   I = INTEGRAL(F), returns the triple definite integral of the CHEBFUN3
%   object F over its domain of definition.
%
% See also CHEBFUN3/INTEGRAL2, CHEBFUN3/INTEGRAL3, CHEBFUN3/SUM,
% CHEBFUN3/SUM2, and CHEBFUN3/SUM3.

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

% Empty check:
if ( isempty(f) )
    I = [];
    return
end

if ( nargin == 1 )                       % Another way to do sum3(f)
    % Triple definite integral:
    I = sum3(f);

else
    if ( (isa(varargin{1}, 'chebfun')) && size(varargin{1}, 2) == 3 )
        % Line integral over an Inf x 3 quasimatrix.
        % Get curve:
        curve = varargin{1};
        xCurve = curve(:, 1);
        yCurve = curve(:, 2);
        zCurve = curve(:, 3);
        diffC = diff(curve);
        ds_squared = diffC(:, 1).^2 + diffC(:, 2).^2 + diffC(:, 3).^2;
        I = sum(feval(f, xCurve, yCurve, zCurve) .* sqrt(ds_squared), ...
            curve.domain);
    else
        error('CHEBFUN:CHEBFUN3:integral:badInputs',...
        'Unrecognised input arguments.');
    end
end

end


function [ n1, n2 ] = year_to_dominical_common ( y )

%*****************************************************************************80
%
%% YEAR_TO_DOMINICAL_COMMON: dominical numbers, Common calendar.
%
%  Discussion:
%
%    The Julian calendar calculations are used through the year 1582,
%    and the Gregorian thereafter.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, integer N1, N2, the dominical numbers for the year.
%    If Y is a leap year, then N1 applies before March 1, and N2 after.
%    If Y is not a leap year, then N1 applies throughout the year,
%    and N2 is returned as N1.
%
  if ( y <= 1582 )
    [ n1, n2 ] = year_to_dominical_julian ( y );
  else
    [ n1, n2 ] = year_to_dominical_gregorian ( y );
  end

  return
end


function e = normcdfln(x)
% NORMCDFLN   log of normal cumulative density function.
% More accurate than log(normcdf(x)) when x is small.
% The following is a quick and dirty approximation to normcdfln:
% normcdfln(x) =approx -(log(1+exp(0.88-x))/1.5)^2

% Written by Tom Minka
% (c) Microsoft Corporation. All rights reserved.

% make e the same shape as x, and inherit any NaNs.
e = x;
t = -6.5;
i = find(x >= t);
if ~isempty(i)
  e(i) = log(normcdf(x(i)));
end
i = find(x < t);
if ~isempty(i)
  x = x(i);
  z = x.^(-2);
  if 0
    % log of asymptotic series for cdf
    % subs(x=-x,asympt(sqrt(2*Pi)*gauss_cdf(-x),x));
    c = [-1 3 -15 105 -945 10395 -135135 2027025 -34459425 654729075];
    y = 0;
    for i = length(c):-1:1
      y = z.*(y + c(i));
    end
    %y = z.*(c(1)+z.*(c(2)+z.*(c(3)+z.*(c(4)+z.*(c(5)+z.*(c(6)+z.*c(7)))))));
    y = log(1+y);
  else
    % asymptotic series for logcdf
    % subs(x=-x,asympt(log(gauss_cdf(-x)),x));
    c = [-1 5/2 -37/3 353/4 -4081/5 55205/6 -854197/7];
    y = z.*(c(1)+z.*(c(2)+z.*(c(3)+z.*(c(4)+z.*(c(5)+z.*(c(6)+z.*c(7)))))));
  end
  e(i) = y -0.5*log(2*pi) -0.5*x.^2 - log(-x);
end


%% Check rate of convergence of FEM for 3D elliptic interface problem
%     -div(A grad u)  = f,    x\in \Omega
%      where A is a piecewise constant on Omega^+ and Omega^-.
%
% Domain: Rectangular domain: [xmin,xmax] X [ymin,ymax] X [zmin,zmax]
% Mesh: Structured rectangular mesh.
% Method: Conforming Lagrange Type FEMs (Q1: Tri-Linear FEM).
%
% Last modified by Xu Zhang 08/06/2020.

%% Geometry and Boundary Conditions
clear
close all
%clc

domain = [-1,1,-1,1,-1,1];
bc = [1,1,1,1,1,1]; % Dirichelet BC

%% Finite Element Type
femtype = 'P1';
disp(['FEM Type =  Conforming ', femtype]);

%% Initial Partition
nx0 = 10;
ny0 = nx0;
nz0 = nx0;

%% Task
showErr = 0;
showMesh = 0;
computErr = 1;

%% PDE
test = 2;
switch test
    case 0
        pde = poissonNonPoly3D;
    case 1 % circular interface
        r = pi/4; bm = 1; bp = pi/(2*r^2);
        x0 = 0; y0 = 0; z0 = 0; rx = pi/4; ry = pi/4; rz = pi/4;
        pde = elli3DcircIntf(bm,bp,r,x0,y0,z0,rx,ry,rz);
    case 2 % orthotorus interface
        domain = [-1.2,1.2,-1.2,1.2,-1.2,1.2];
        bm = 1; bp = 1;
        rx = 1; ry = 0.075; rz = 3;
        pde = elli3DorthocircIntf(bm,bp,rx,ry,rz);
    case 3 % line interface
        bm = 1; bp = 100;
        rx = 1; ry = 0; rz = 1; cx = pi/10; cy = 0; cz = 0;
        cm = 1; cp = 1; a = 1;
        pde = elli3DlinIntf(bm,bp,cx,cy,cz,rx,ry,rz,a,cm,cp);
end

%% Max Iteration
maxIt = 16;
time = zeros(7,maxIt);

for i = 1:maxIt

    %% 1. Generate Mesh
    tic
    nx = nx0 + 10*(i-1); h = (domain(2) - domain(1))/nx;
    ny = ny0 + 10*(i-1);
    nz = nz0 + 10*(i-1);
    time(1,i) = nx;
    disp(' ')
    disp('*******************************************************************************')
    disp(['Partition =  ',int2str(nx),' X ',int2str(ny),' X ',int2str(nz)]);
    disp(' ')

    mesh = genMesh3D(domain, nx, ny, nz);
    disp(['number of element =  ', int2str(length(mesh.t))]);
%     mesh = genIntfMesh3D(mesh,pde.intf);
%     disp(['number of interface element =  ', int2str(-min(mesh.tLoc)),...
%         ', is ', num2str(100*-min(mesh.tLoc)/length(mesh.t)), '% of all elements']);
    if showMesh == 1
        tetramesh(mesh.t,mesh.p,ones(size(mesh.t)));
    end
    time(2,i) = toc;

    %% 2. Generate FEM DoF
    tic
    fem = genFEM3D(mesh,femtype,bc);
    disp(['number of DoF =  ', int2str(length(fem.p))]);
    time(3,i) = toc;

    %% 3. Assemble Matrix
    tic
    disp(' '); disp('Start Assembling Matrix');
    matrix = genMatEll3D(pde,mesh,fem);
    time(4,i) = toc;

    %% 3. Solve the linear system Au = f
    tic
    % L = ichol(matrix.A,struct('michol','on'));
    L = ichol(matrix.A);
    [u,flag,relres,iter,resvec] = pcg(matrix.A,matrix.f,1e-8,300,L,L');
    time(5,i) = toc;
    tu = pde.exactu(fem.p(:,1),fem.p(:,2),fem.p(:,3));
    uh = tu; uh(fem.mapper) = u;

    %% 4. Postprocess: Calculating Errors
    tic
    errND = max(abs(uh - tu)); % Error on nodes
    err.nd = errND; err.inf = 0; err.l2 = 0; err.h1 = 0;
    if computErr == 1
        disp(' ');  disp('Start computing error in Inf norm');
        errInf = getErrInf3D(uh,pde.exactu,fem,[0,0,0]);
        eNorm = 'L2'; disp(['Start computing error in ',eNorm,'  norm']);
        [errL2,errL2K] = getErr3D(uh, pde.exactu, fem, eNorm);
        eNorm = 'H1x'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1x,errH1xK] = getErr3D(uh, pde.Dxu, fem, eNorm);
        eNorm = 'H1y'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1y,errH1yK] = getErr3D(uh, pde.Dyu, fem, eNorm);
        eNorm = 'H1z'; disp(['Start computing error in ',eNorm,' norm']);
        [errH1z,errH1zK] = getErr3D(uh, pde.Dzu, fem, eNorm);
        err.inf = max([errND,errInf]);
        err.l2 = errL2;
        err.h1 = sqrt(errH1x^2+errH1y^2+errH1z^2);
    end
    time(6,i) = toc;
    time(7,i) = 1e6*sum(time(2:6,i))/length(mesh.t);

    %% 5: Output

    disp(' ')
    disp('Errors')
    disp('Node        Inf norm    L2 norm     H1 norm')
    formatSpec = '%6.4e  %6.4e  %6.4e  %6.4e\n';
    fprintf(formatSpec, err.nd, err.inf, err.l2, err.h1)

    if i > 1
        format short
        rNd = log(err0.nd/err.nd)./log(h0/h);
        rInf = log(err0.inf/err.inf)./log(h0/h);
        rL2 = log(err0.l2/err.l2)./log(h0/h);
        rH1 = log(err0.h1/err.h1)./log(h0/h);
        disp(' ')
        disp('Convergence Rate')
        disp('Node        Inf norm    L2 norm     H1 norm')
        formatSpec = '%6.4f      %6.4f      %6.4f      %6.4f\n';
        fprintf(formatSpec, rNd, rInf, rL2, rH1)
    end
    err0 = err; h0 = h;

    disp(' '); disp('CPU Time')
    disp('   N     Mesh     FEM      Matrix   Solve    Error    Time/1M cell')
    formatSpec = '%4i  %7.2f  %7.2f  %7.2f  %7.2f  %7.2f   %7.2f\n';
    fprintf(formatSpec, time(:,1:i))

    %% 6. Plot Solution and Error
    if showErr == 1
    end
end


%Extremely similar to the false position method. The main difference is
%that the secant method does not actually have a defined interval where
%the root lies on. It converges faster than the false position method,
%but it is not always guaranteed to converge.

%INPUTS:
%Function handle f
%x1 = a
%x2 = b
%maximum tolerated error

%OUTPUTS:
%An approximated value for the root of f.

%Written by MatteoRaso

function y = secant(f, a, b, error)
  x = [a, b];
  n = 2;
  while abs(f(x(n))) > error
    x(n + 1) = -f(x(n)) * (x(n) - x(n - 1)) / (f(x(n)) - f(x(n - 1))) + x(n);
    n = n + 1;
    disp(f(x(n)))
  endwhile
  A = ["The root is approximately ", num2str(x(n))];
  disp(A)
  y = x(n);
endfunction


function [tfr,t,f] = tfrsp(x,t,N,h,trace);
%TFRSP	Spectrogram time-frequency distribution.
%	[TFR,T,F]=TFRSP(X,T,N,H,TRACE) computes the Spectrogram
%	distribution of a discrete-time signal X.
%
%	X     : signal.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	H     : analysis window, H being normalized so as to
%	        be  of unit energy.      (default : Hamming(N/4)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency representation. When called without
%	        output arguments, TFRSP runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4);
%	 h=tftb_window(17,'Kaiser'); tfrsp(sig,1:128,64,h,1);
%
%        [tfr,t,freq]=tfrsp(sig,1:128,64,h,1); plot(fftshift(freq),fftshift(tfr(:,100)))
%
%	See also all the time-frequency representations listed in
%	 the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least 1 parameter required');
end;
[xrow,xcol] = size(x);
if (nargin < 1),
 error('At least 1 parameter is required');
elseif (nargin <= 2),
 N=xrow;
end;

hlength=floor(N/4);
hlength=hlength+1-rem(hlength,2);

if (nargin == 1),
 t=1:xrow; h = tftb_window(hlength); trace=0;
elseif (nargin == 2) | (nargin == 3),
 h = tftb_window(hlength); trace=0;
elseif (nargin == 4),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;
[trow,tcol] = size(t);
if (xcol==0) | (xcol>2),
 error('X must have one or two columns');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

[hrow,hcol]=size(h); Lh=(hrow-1)/2;
if (hcol~=1)|(rem(hrow,2)==0),
 error('H must be a smoothing window with odd length');
end;

tfr= zeros (N,tcol) ;
if trace, disp('Spectrogram'); end;
for icol=1:tcol,
 ti= t(icol); tau=-min([round(N/2)-1,Lh,ti-1]):min([round(N/2)-1,Lh,xrow-ti]);
 indices= rem(N+tau,N)+1;
 if trace, disprog(icol,tcol,10); end;
 tfr(indices,icol)=x(ti+tau).*conj(h(Lh+1+tau))/norm(h(Lh+1+tau));
end;
tfr=abs(fft(tfr)).^2;
if trace, fprintf('\n'); end;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrsp',h);
elseif (nargout==3),
 if rem(N,2)==0,
  f=[0:N/2-1 -N/2:-1]'/N;
 else
  f=[0:(N-1)/2 -(N-1)/2:-1]'/N;
 end;
end;


function [x_lambda,rho,eta,data,X] = maxent(A,b,lambda,w,x0)
%MAXENT Maximum entropy regularization.
%
% [x_lambda,rho,eta] = maxent(A,b,lambda,w,x0)
%
% Maximum entropy regularization:
%    min { || A x - b ||^2 + lambda^2*x'*log(diag(w)*x) } ,
% where -x'*log(diag(w)*x) is the entropy of the solution x.
% If no weights w are specified, unit weights are used.
%
% If lambda is a vector, then x_lambda is a matrix such that
%    x_lambda = [x_lambda(1), x_lambda(2), ... ] .
%
% This routine uses a nonlinear conjugate gradient algorithm with "soft"
% line search and a step-length control that insures a positive solution.
% If the starting vector x0 is not specified, then the default is
%    x0 = norm(b)/norm(A,1)*ones(n,1) .

% Per Christian Hansen, IMM and Tommy Elfving, Dept. of Mathematics,
% Linkoping University, 06/10/92.

% Reference: R. Fletcher, "Practical Methods for Optimization",
% Second Edition, Wiley, Chichester, 1987.

% Set defaults.
flat = 1e-3;     % Measures a flat minimum.
flatrange = 10;  % How many iterations before a minimum is considered flat.
maxit = 150;     % Maximum number of CG iterations.
minstep = 1e-12; % Determines the accuracy of x_lambda.
sigma = 0.5;     % Threshold used in descent test.
tau0 = 1e-3;     % Initial threshold used in secant root finder.

% Initialization.
[m,n] = size(A); x_lambda = zeros(n,length(lambda)); F = zeros(maxit,1);
if (min(lambda) <= 0)
  error('Regularization parameter lambda must be positive')
end
if (nargin ==3), w  = ones(n,1); end
if (nargin < 5), x0 = ones(n,1); end

% Treat each lambda separately.
for j=1:length(lambda);

  % Prepare for nonlinear CG iteration.
  l2 = lambda(j)^2;
  x  = x0; Ax = A*x;
  g  = 2*A'*(Ax - b) + l2*(1 + log(w.*x));
  p  = -g;
  r  = Ax - b;

  % Start the nonlinear CG iteration here.
  delta_x = x; dF = 1; it = 0; phi0 = p'*g;
  while (norm(delta_x) > minstep*norm(x) & dF > flat & it < maxit & phi0 < 0)
    it = it + 1;

    % Compute some CG quantities.
    Ap = A*p; gamma = Ap'*Ap; v = A'*Ap;

    % Determine the steplength alpha by "soft" line search in which
    % the minimum of phi(alpha) = p'*g(x + alpha*p) is determined to
    % a certain "soft" tolerance.
    % First compute initial parameters for the root finder.
    alpha_left = 0; phi_left = phi0;
    if (min(p) >= 0)
      alpha_right = -phi0/(2*gamma);
      h = 1 + alpha_right*p./x;
    else
      % Step-length control to insure a positive x + alpha*p.
      I = find(p < 0);
      alpha_right = min(-x(I)./p(I));
      h = 1 + alpha_right*p./x; delta = eps;
      while (min(h) <= 0)
        alpha_right = alpha_right*(1 - delta);
        h = 1 + alpha_right*p./x;
        delta = delta*2;
      end
    end
    z = log(h);
    phi_right = phi0 + 2*alpha_right*gamma + l2*p'*z;
    alpha = alpha_right; phi = phi_right;

    if (phi_right <= 0)

      % Special treatment of the case when phi(alpha_right) = 0.
      z = log(1 + alpha*p./x);
      g_new = g + l2*z + 2*alpha*v; t = g_new'*g_new;
      beta = (t - g'*g_new)/(phi - phi0);

    else

      % The regular case: improve the steplength alpha iteratively
      % until the new step is a descent step.
      t = 1; u = 1; tau = tau0;
      while (u > -sigma*t)

        % Use the secant method to improve the root of phi(alpha) = 0
        % to within an accuracy determined by tau.
        while (abs(phi/phi0) > tau)
          alpha = (alpha_left*phi_right - alpha_right*phi_left)/...
                  (phi_right - phi_left);
          z = log(1 + alpha*p./x);
          phi = phi0 + 2*alpha*gamma + l2*p'*z;
          if (phi > 0)
            alpha_right = alpha; phi_right = phi;
          else
            alpha_left  = alpha; phi_left  = phi;
          end
        end

        % To check the descent step, compute u = p'*g_new and
        % t = norm(g_new)^2, where g_new is the gradient at x + alpha*p.
        g_new = g + l2*z + 2*alpha*v; t = g_new'*g_new;
        beta = (t - g'*g_new)/(phi - phi0);
        u = -t + beta*phi;
        tau = tau/10;

      end  % End of improvement iteration.

    end  % End of regular case.

    % Update the iteration vectors.
    g = g_new; delta_x = alpha*p;
    x = x + delta_x;
    p = -g + beta*p;
    r = r + alpha*Ap;
    phi0 = p'*g;

    % Compute some norms and check for flat minimum.
    rho(j,1) = norm(r); eta(j,1) = x'*log(w.*x);
    F(it) = rho(j,1)^2 + l2*eta(j,1);
    if (it <= flatrange)
      dF = 1;
    else
      dF = abs(F(it) - F(it-flatrange))/abs(F(it));
    end

    data(it,:) = [F(it),norm(delta_x),norm(g)];
    X(:,it) = x;

  end  % End of iteration for x_lambda(j).

  x_lambda(:,j) = x;

end


function [vMat,fMat] = spheretribydepth(depth)
% SPHERETRIBYDEPTH is a high-performance vectorized function for building
% a triangulation of a unit sphere based on recursive partitioning of each
% of Icosahedron faces into 4 triangles with vertices in the middles of
% original face
%
% Input:
%   depth: double[1,1] - depth of partitioning, use 1 for the first level of
%       Icosahedron partitioning, and greater value for a greater level
%       of partitioning
%
% Output:
%   vMat: double[nVerts,3] - (x,y,z) coordinates of triangulation
%       vertices
%   fMat: double[nFaces,3] - indices of face verties in vertMat
%
% Example:
%   [vMat, fMat] = spheretribydepth(4);
%   patch('Vertices',vMat,'Faces',fMat,'FaceColor','g','EdgeColor','k');
%
% $Author: Peter Gagarinov, PhD  <pgagarinov@gmail.com> $
% $Copyright: Peter Gagarinov, PhD,
%            Moscow State University,
%            Faculty of Computational Mathematics and Computer Science,
%            System Analysis Department 2011-2016 $
%
if ~(numel(depth)&&isnumeric(depth)&&depth>=0&&fix(depth)==depth)
    error('spheretri:wrongInput',...
        'depth is expected to be a not negative integer scalar');
end
[vMat,fMat]=icosahedron();
[vMat,fMat]=shrinkfacetri(vMat,fMat,0,depth,@normvert);
end
function x=normvert(x)
x=x./repmat(realsqrt(sum((x.*x),2)),1,3);
end


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


%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 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 [ 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