function [xOrigShape,costFun,LAMBDA,JAC,EXITFLAG,OUTPUT,msgData] = levenbergMarquardt(funfcn,xOrigShape,lb,ub,...
    verbosity,options,defaultopt,costFun,JAC,caller,gamma,detailedExitMsg,optimgetFlag,finDiffFlags,sizes,varargin)
%

%levenbergMarquardt Levenberg-Marquardt solver of non-linear least squares problems.
%   levenbergMarquardt solves problems of the form:
%   min sum {FUN(x).^2}
%     lb <= x <= ub
%   using the Levenberg-Marquardt algorithm.

%   Copyright 2007-2022 The MathWorks, Inc.

import matlab.internal.math.nowarn.mldivide

% ------------Initialization----------------
x = xOrigShape(:);
nVar = sizes.nVar;
xShape = sizes.xShape;
OUTPUT = [];
iter = 0;

LAMBDA.upper = zeros(nVar,1);
LAMBDA.lower = zeros(nVar,1);

% Check for complex-valued bounds
if ~isreal(lb) || ~isreal(ub)
    error('optimlib:checkbounds:ComplexBounds',getString(message('optimlib:checkbounds:ComplexBounds')));
end

% Check for NaN-valued bounds
if anynan(lb) || anynan(ub)
    error('optimlib:checkbounds:NanBounds',getString(message('optimlib:checkbounds:NanBounds')));
end

% Handle the output
outputfcn = optimget(options,'OutputFcn',defaultopt,optimgetFlag);
if isempty(outputfcn)
    haveoutputfcn = false;
else
    haveoutputfcn = true;
    % Parse OutputFcn which is needed to support cell array syntax for OutputFcn.
    outputfcn = createCellArrayOfFunctions(outputfcn,'OutputFcn');
end

% Handle the output
plotfcns = optimget(options,'PlotFcns',defaultopt,optimgetFlag);
if isempty(plotfcns)
    haveplotfcn = false;
else
    haveplotfcn = true;
    % Parse PlotFcns which is needed to support cell array syntax for PlotFcns.
    plotfcns = createCellArrayOfFunctions(plotfcns,'PlotFcns');
end

% Options checking
gradflag =  strcmp(optimget(options,'Jacobian',defaultopt,optimgetFlag),'on');
tolX = optimget(options,'TolX',defaultopt,optimgetFlag);
tolFun = optimget(options,'TolFunValue',defaultopt,optimgetFlag);

% For parallel finite difference (if needed) we need to send the function
% handles now to the workers. This avoids sending the function handles in
% every iteration of the solver. The output from 'setOptimFcnHandleOnWorkers'
% is a onCleanup object that will perform cleanup task on the workers.
UseParallel = options.UseParallel;
ProblemdefOptions = optimget(options, 'ProblemdefOptions',defaultopt,optimgetFlag);
FromSolve = false;
if ~isempty(ProblemdefOptions) && isfield(ProblemdefOptions, 'FromSolve')
    FromSolve = ProblemdefOptions.FromSolve;
end

if UseParallel
    cleanupObj = setOptimFcnHandleOnWorkers(UseParallel,funfcn,{''},FromSolve); %#ok<NASGU>
end

maxFunEvals = optimget(options,'MaxFunEvals',defaultopt,optimgetFlag);
maxIter = optimget(options,'MaxIter',defaultopt,optimgetFlag);
% If MaxFunEvals is default string, set to default setting for LM
% Also accept string 100*numberOfVariables (previous default)
if strcmpi(maxFunEvals,'200*numberofvariables')
    maxFunEvals = 200*nVar;
elseif strcmpi(maxFunEvals,'100*numberofvariables')
    maxFunEvals = 100*nVar;
end

% Convert values to full to avoid having a sparse sum of squares
costFun = full(costFun); 
nfun = length(costFun);
numFunEvals = 1;
sumSq = costFun'*costFun;  % Sum of squares
zeroPad = zeros(nVar,1);   % Padding for LM step computation
successfulStep = true; % Flag indicating that JAC needs to be computed this loop iteration
% Initialize the step variable for the OutputFcn and PlotFcn
step = [];

% Initialize the boolean indicating whether all function values are well
% defined during estimation of Jacobian.
evalOK = true;

% Grab function handles
objEvalType = funfcn{1};
objfun = funfcn{3};
jacobfun = funfcn{4};

% Compute initial Jacobian via finite-differences for derivative check or
% if JAC is empty.
if ~gradflag
    
    JAC = zeros(nfun,nVar); % pre-allocate derivative array
    
    [JAC,~,~,numEvals,evalOK] = computeFinDiffGradAndJac(x,objfun,[],costFun, ...
        [],[],JAC,[],[],lb,ub,[],options,finDiffFlags,sizes,varargin{:});
    
    numFunEvals = numFunEvals + numEvals;
    if ~evalOK
        error('optimlib:levenbergMarquardt:DerivUndefAtX0', ...
            getString(message('optimlib:commonMsgs:FinDiffJacUndefAtX0',caller)));
    end
end

% Check if finite differences evaluated user functions at an undefined
% point. If the Jacobian contains invalid values, set undefJac to true,
% notifying the stopping test function, testConvergence, to halt the
% algorithm. In this case, we know that the Jacobian is ok since otherwise
% an error would have been thrown above.
undefJac = ~evalOK;

anyFiniteBounds = finDiffFlags.anyFiniteBounds;

if anyFiniteBounds
    projectionOp = @(x,dx) projectBox(x,dx,lb,ub);
    zerovec = zeros(nVar,1);
else
    projectionOp = @(x,dx) dx;
end

gradF = JAC'*costFun;
firstOrderOpt = computeFirstOrderOpt(projectionOp,x,gradF);
sqrtEps = sqrt(eps);
% Initial 1st order optimality with safeguard to prevent value of 0, used in stopping tests
relFactor = max(firstOrderOpt,1.0);
% tolOpt: tolerance used when checking the 1st-order optimality
tolOpt = 1e-4 * tolFun;

jacIsSparse = issparse(JAC);  % Flag indicating that JAC is a sparse matrix
scaleStep = strcmpi(optimget(options,'ScaleProblem',defaultopt,optimgetFlag),'jacobian');
if scaleStep
    diagJacTJac = sum(JAC.^2,1)'; % Compute diagonal of J'*J, used for scaling in LM step
end

if haveoutputfcn || haveplotfcn
    directionalDeriv = [];  % Initialize the directional-derivative
    % Initialize the output function.
    [optimValues, stop] = callOutputAndPlotFcns(outputfcn,plotfcns,caller,xOrigShape,...
        'init',iter,numFunEvals,costFun,sumSq,[],gradF,[],gamma,projectionOp,varargin{:});
    if stop
        [costFun,JAC,EXITFLAG,OUTPUT,msgData] = cleanUpInterrupt(optimValues,caller,verbosity);
        return;
    end
    % Call for iteration 0
    [optimValues, stop] = callOutputAndPlotFcns(outputfcn,plotfcns,caller,xOrigShape,...
        'iter',iter,numFunEvals,costFun,sumSq,[],gradF,[],gamma,projectionOp,varargin{:});
    if stop
        [costFun,JAC,EXITFLAG,OUTPUT,msgData] = cleanUpInterrupt(optimValues,caller,verbosity);
        return;
    end
end

% Display first iteration information
if verbosity > 1
    % 1st iteration does not show last column norm(step) because it's undefined
    formatStrFirstIter = [blanks(1),'%5.0f',blanks(7),'%5.0f',blanks(4),'%13.6g',blanks(4),'%12.3g',blanks(1),'%12.6g\n'];
    formatstr          = [blanks(1),'%5.0f',blanks(7),'%5.0f',blanks(4),'%13.6g',blanks(4),'%12.3g',blanks(1),'%12.6g',blanks(3),'%12.6g\n'];
    
    if isequal(caller,'fsolve')
        colHeader = [blanks(3),'||f(x)||^2'];
    else % lsqnonlin, lsqcurvefit
        colHeader = [blanks(6),'Resnorm'];
    end
    fprintf( ...
        ['\n',blanks(40),'First-order',blanks(21),'Norm of \n', ...
        blanks(1),'Iteration',blanks(2),'Func-count',colHeader,blanks(6),'optimality',blanks(7),'Lambda',blanks(11),'step\n']);
    fprintf(formatStrFirstIter,iter,numFunEvals,sumSq,firstOrderOpt,gamma);
end

% Check to see if the initial point is optimal
[done,EXITFLAG,msgData,iter] = testConvergence(detailedExitMsg, ...
    verbosity,caller,gradF,tolOpt,relFactor,tolFun,iter,undefJac,x,[],[],[],[],[], ...
    numFunEvals,maxFunEvals,maxIter,projectionOp,firstOrderOpt);

% Create the fault tolerance structure
faultTolStruct = createFaultTolStruct(false);
newFaultTolStruct = faultTolStruct;

while ~done
    % Compute scaling matrix if needed
    if ~scaleStep   % Unscaled Jacobian
        if ~jacIsSparse
            scaleMat = sqrt(gamma)*eye(nVar);
        else
            scaleMat = sqrt(gamma)*speye(nVar);
        end
    else
        if ~jacIsSparse
            scaleMat = diag(sqrt(gamma*diagJacTJac));
        else
            scaleMat = sparse(1:nVar,1:nVar,sqrt(gamma*diagJacTJac),nVar,nVar);
        end
    end

    % Compute LM step
    if successfulStep
        % Augmented Jacobian
        AugJac = [JAC; scaleMat];
        AugRes = [-costFun; zeroPad]; % Augmented residual
    else
        % If previous step failed, replace only the part of the matrix that has changed
        AugJac(nfun+1:end,:) = scaleMat;
    end

    %Previously, warning messages were disabled before and re-enabled after
    %the LLS solve to ignore rank-deficient AugJac. A new builtin provides
    %a warning-less mldivide which is computationally equivalent and much
    %faster. The following import command is called above outside the loop:
    % import matlab.internal.math.nowarn.mldivide

    if anyFiniteBounds
        % Determine Active Indices
        if scaleStep
            tolActive = min(norm(projectBox(x,-gradF/(1+gamma)./max(eps,diagJacTJac),lb,ub),Inf), min((ub-lb)/2));
        else
            tolActive = min(norm(projectBox(x,-gradF/(1+gamma),lb,ub),Inf), min((ub-lb)/2));
        end
        indActive = (x - lb <= tolActive & gradF > 0) | (ub - x <= tolActive & gradF < 0);

        % Zero Jacobian Columns for Active Indices
        AugJac(1:nfun,indActive) = 0;

        step = AugJac \ AugRes;              % Compute LM step

        % Step direction for the active coordinates is negative gradient.
        if scaleStep
            step(indActive) = -gradF(indActive) / (1 + gamma) ./ max(eps,diagJacTJac(indActive));
        else
            step(indActive) = -gradF(indActive) / (1 + gamma);
        end
        trialX = projectBox(zerovec,x + step,lb,ub);  % Update X with computed step
    else
        % Unconstrained LM step
        step = AugJac \ AugRes;
        trialX = x + step;
    end

    % Evaluate objective functions and Jacobian, if given by user
    
    % If the previous step wasn't successful, we don't need to evaluate the
    % Jacobian until we're sure that the latest step is a good one. Only
    % evaluate the cost function in that case.
    xOrigShape(:) = trialX;
    if ~successfulStep || ~gradflag
        trialCostFun = feval(objfun,xOrigShape,varargin{:});
    else
        switch objEvalType
           %case 'fun' computed above
            case 'fungrad'
                [trialCostFun,JAC] = feval(objfun,xOrigShape,varargin{:});
            case 'fun_then_grad'
                trialCostFun = feval(objfun,xOrigShape,varargin{:});
                JAC = feval(jacobfun,reshape(trialX,xShape),varargin{:});
        end
    end
    trialCostFun = full(trialCostFun(:));          % Convert to full to prevent error with iterative display
    numFunEvals = numFunEvals + 1;
    trialSumSq = trialCostFun'*trialCostFun;             % Updated sum of squares
    
    % Update fault tolerance structure
    faultTolStruct = updateFaultTolStruct(faultTolStruct, trialSumSq, ...
        verbosity > 1);
            
    if faultTolStruct.currTrialWellDefined && trialSumSq < sumSq   
        % If we've reduced the sum squared error and the function vector is
        % defined at the new trial point.
        costFun = trialCostFun;         % Save a copy of the last successful cost function
        x = trialX;                  % Update X
        if successfulStep
            gamma = 0.1*gamma;    % Reduce LM parameter, only if previous step was good        
        end
        % If Jacobian given by user, and previous step(s) were
        % unsuccessful, we have yet to compute the Jacobian, so compute
        % it here.
        if gradflag && ~successfulStep   
            switch objEvalType
               % If funfcn == 'fun', JAC will be updated when it is computed by finite-differences                    
                case 'fungrad'
                    [~,JAC] = feval(objfun,xOrigShape,varargin{:});
                case 'fun_then_grad'
                    JAC = feval(jacobfun,xOrigShape,varargin{:});
            end
        elseif ~gradflag
            
            [JAC,~,~,numEvals,evalOK] = computeFinDiffGradAndJac(x,objfun,[],costFun, ...
                [],[],JAC,[],[],lb,ub,[],options,finDiffFlags,sizes,varargin{:});
            
            numFunEvals = numFunEvals + numEvals;
            
            % Check if finite differences evaluated user functions at an
            % undefined point If the Jacobian contains invalid values, set
            % undefGrads to true, notifying the stopping test function,
            % testConvergence, to halt the algorithm.
            undefJac = ~evalOK;
        end
        
        if scaleStep  % Recompute scaling factor from latest Jacobian information
            diagJacTJac = sum(JAC.^2,1)'; % Compute diagonal of J'*J, used for scaling in LM step
        end
        successfulStep = true;        % Successful step, new Jacobian needs to be computed 
        iter = iter + 1;
                
        gradF = JAC'*costFun;  
        firstOrderOpt = computeFirstOrderOpt(projectionOp,x,gradF);
        
        % Print iterative display
        if verbosity > 1
            if faultTolStruct.undefObj
                fprintf(getString(message('optimlib:commonMsgs:ObjInfNaNComplex', ...
                    faultTolStruct.undefValue)));
            end
            fprintf(formatstr,iter,numFunEvals,trialSumSq,firstOrderOpt,gamma,norm(step));
        end
        
        if haveoutputfcn || haveplotfcn                 % Call Output or PlotFcn 
            % Compute directional derivative for OutputFcn
            directionalDeriv = gradF'*step/norm(step);
            [optimValues, stop] = callOutputAndPlotFcns(outputfcn,plotfcns,caller,reshape(x,xShape),...
                'iter',iter,numFunEvals,costFun,trialSumSq,directionalDeriv,gradF,step,gamma,projectionOp,varargin{:});
            if stop
                [costFun,JAC,EXITFLAG,OUTPUT,msgData] = cleanUpInterrupt(optimValues,caller,verbosity);
                return;
            end
        end
        
        % Check Termination Criteria
        [done,EXITFLAG,msgData,iter] = testConvergence(detailedExitMsg, ...
            verbosity,caller,gradF,tolOpt,relFactor,tolFun,iter,undefJac,x,trialSumSq,tolX,sqrtEps, ...
            step,sumSq,numFunEvals,maxFunEvals,maxIter,projectionOp,firstOrderOpt);

        sumSq = trialSumSq;         % Update sum of squares after convergence test
        
        % As the iteration has been completed, the fault tolerance
        % structure needs to be reset.
        faultTolStruct = newFaultTolStruct;
    else                                
        gamma = 10*gamma;              % Increase LM parameter
        successfulStep = false;          % Unsuccessful step, no need to re-compute the Jacobian

        % The step is too small, cannot proceed
        % or too many function evals
        if norm(step) < tolX*(sqrtEps + norm(trialX))
            EXITFLAG = 4;
            msgData = {{'optimlib:levenbergMarquardt:Exit4basic',caller}, ...
                {'optimlib:levenbergMarquardt:Exit4detailed',norm(step)/(sqrtEps+norm(trialX)),tolX}, ...
                verbosity > 0,detailedExitMsg};
            done = true;
            iter = iter + 1;
            if verbosity > 1
                % Print only occurs when iteration has begun but step size causes the iteration to fail.
                % Without, the output struct will not match the displayed final iterations, func-count, 
                % and norm(step) of the final failed iteration.
                fprintf(' %5.0f       %5.0f                                                %12.6g\n',...
                    iter,numFunEvals,norm(step));
            end
        elseif numFunEvals >= maxFunEvals
            EXITFLAG = 0;
            msgData = {{'optimlib:commonMsgs:Exit0basic',caller,maxFunEvals}, ...
                        {},verbosity > 0,false};
            done = true;
        end        
    end % if newF < oldF
end  % while ~done

if scaleStep
    tolActive = min(norm(projectionOp(x,-gradF/(1+gamma)./max(eps,diagJacTJac)),Inf), min((ub-lb)/2));
else
    tolActive = min(norm(projectionOp(x,-gradF/(1+gamma)),Inf), min((ub-lb)/2));
end
indActiveLower = (x - lb <= tolActive & gradF > 0);
indActiveUpper = (ub - x <= tolActive & gradF < 0);
LAMBDA.lower(indActiveLower) = 2 * gradF(indActiveLower);
LAMBDA.upper(indActiveUpper) = -2 * gradF(indActiveUpper);

xOrigShape(:) = x;
OUTPUT.iterations    = iter;
OUTPUT.funcCount     = numFunEvals;
OUTPUT.stepsize      = norm(step);
OUTPUT.cgiterations  = [];
OUTPUT.firstorderopt = firstOrderOpt;
OUTPUT.algorithm     = 'levenberg-marquardt';

if haveoutputfcn || haveplotfcn
    callOutputAndPlotFcns(outputfcn,plotfcns,caller,xOrigShape,'done',iter,numFunEvals, ...
        costFun,sumSq,directionalDeriv,gradF,step,gamma,projectionOp,varargin{:});
    % Optimization done, so ignore "stop"
end

function firstOrderOpt = computeFirstOrderOpt(projectionOp,XOUT,gradF)
    a = norm(projectionOp(XOUT,-gradF),Inf);
    b = norm(gradF,Inf);
    % When a==b, a^2 / b = a via cancellation (no active bounds)
    % When a==b==0, a^2 / b = 0 via removable singularity (x at local unconstrained optimizer)
    if abs(a-b) <= eps * max(a,b) || b == 0
        firstOrderOpt = a;
    else
        firstOrderOpt = a^2 / b;
    end
    
%--------------------------------------------------------------------------
function [optimValues, stop] = callOutputAndPlotFcns(outputfcn,plotfcns,caller,...
    xOutputfcn,state,iter,numFunEvals,costFun,newF,gdnew,gradF,step,gamma,project,varargin)
% CALLOUTPUTANDPLOTFCNS assigns values to the struct OptimValues and then calls the
% outputfcn/plotfcns.
%
% state - can have the values 'init','iter', or 'done'. 

% For the 'done' state we do not check the value of 'stop' because the
% optimization is already done.
optimValues.iteration = iter;
optimValues.funccount = numFunEvals;
optimValues.stepsize = norm(step);
optimValues.directionalderivative = gdnew;
optimValues.gradient = gradF;
% xOutputfcn is flattened here because it has been resized to the original x0
% shape so it can be passed to the output and plot functions in the
% original shape; however, computeFirstOrderOpt requires intput to be flat.
optimValues.firstorderopt = computeFirstOrderOpt(project,xOutputfcn(:),gradF);
optimValues.searchdirection = step;
optimValues.lambda = gamma;
if isequal(caller,'fsolve') 
   optimValues.fval = costFun; 
else % lsqnonlin, lsqcurvefit 
   optimValues.residual = costFun; 
   optimValues.resnorm = newF; 
end 

stop = false;
% Call output function
if ~isempty(outputfcn)
    switch state
        case {'iter','init'}
            stop = callAllOptimOutputFcns(outputfcn,xOutputfcn,optimValues,state,varargin{:}) || stop;
        case 'done'
            callAllOptimOutputFcns(outputfcn,xOutputfcn,optimValues,state,varargin{:});
    end
end
% Call plot functions
if ~isempty(plotfcns)
    switch state
        case {'iter','init'}
            stop = callAllOptimPlotFcns(plotfcns,xOutputfcn,optimValues,state,varargin{:}) || stop;
        case 'done'
            callAllOptimPlotFcns(plotfcns,xOutputfcn,optimValues,state,varargin{:});
    end
end
%--------------------------------------------------------------------------
function [costFun,JAC,EXITFLAG,OUTPUT,msgData] = cleanUpInterrupt(optimValues, ...
                                                            caller,verbosity)

% Call plot function driver to finalize the plot function figure window. If
% no plot functions have been specified or the plot function figure no
% longer exists, this call just returns.
callAllOptimPlotFcns('cleanuponstopsignal');

% costFun can be either 'fval' (fsolve) or 'residual'
if isequal(caller,'fsolve') 
    costFun = optimValues.fval;
else 
    costFun = optimValues.residual;
end
EXITFLAG = -1; 
OUTPUT.iterations    = optimValues.iteration;
OUTPUT.funcCount     = optimValues.funccount;
OUTPUT.stepsize      = optimValues.stepsize;
OUTPUT.cgiterations  = [];
OUTPUT.firstorderopt = [];
OUTPUT.algorithm     = 'levenberg-marquardt';
msgData = {{'optimlib:commonMsgs:ExitNeg1basic',caller},{},verbosity > 0,false};

JAC = []; % May be in an inconsistent state

%--------------------------------------------------------------------------
function [done,EXITFLAG,msgData,iter] = testConvergence(...
    detailedExitMsg,verbosity,caller,gradF,tolOpt,relFactor,tolFun,iter,undefJac, ...
    XOUT,newF,tolX,sqrtEps,step,oldF,numFunEvals,maxFunEvals,maxIter,projectionOp,firstOrderOpt)
% testConvergence tests all of the termination criteria for the
% Levenberg-Marquardt algorithm. This function is called at each successful
% step of the algorithm. In addition, testConvergence is called before the
% main loop of levenbergMarquardt to only check the initial point for
% optimality. In this case, only 10 inputs are required. 
% 
% Note that the first line of inputs in the API are those required by both
% calls to testConvergence. The second line of inputs are only required by
% the call to testConvergence in the main while loop.

% Initialize these quantities in case no criteria are met.
done = false;
EXITFLAG = [];
msgData = {};

optimalityMeas = norm(projectionOp(XOUT,-gradF),Inf)^2;

if undefJac
    EXITFLAG = 2;
    msgData = {{'optimlib:levenbergMarquardt:Exit26basic',caller},{},verbosity > 0,false};
    done = true;
elseif optimalityMeas <= tolOpt * relFactor * norm(gradF,Inf)
    EXITFLAG = 1;
    if iter == 0
        basicMsgID = 'optimlib:levenbergMarquardt:Exit100basic';
        msgID = 'optimlib:levenbergMarquardt:Exit100detailed';
    else
        basicMsgID = 'optimlib:levenbergMarquardt:Exit1basic';
        msgID = 'optimlib:levenbergMarquardt:Exit1detailed';
    end
    % Call createExitMsg with createExitMsgExitflag = 100 if x0 is
    % optimal, otherwise createExitMsgExitflag = 1
    msgData = {{basicMsgID}, ...
            {msgID,firstOrderOpt/relFactor,tolOpt}, ...
            verbosity > 0,detailedExitMsg};
    done = true;
elseif numFunEvals >= maxFunEvals
    EXITFLAG = 0;        
    msgData = {{'optimlib:commonMsgs:Exit0basic',caller,maxFunEvals},{},verbosity > 0,false}; 
    done = true;
elseif iter >= maxIter
    EXITFLAG = 0;        
    msgData = {{'optimlib:commonMsgs:Exit10basic',caller,maxIter},{},verbosity > 0,false};
    done = true;
elseif iter > 0
    if norm(step) < tolX*(sqrtEps + norm(XOUT))
        EXITFLAG = 4;        
        msgData = {{'optimlib:levenbergMarquardt:Exit4basic',caller}, ...
            {'optimlib:levenbergMarquardt:Exit4detailed', ...
            norm(step)/(sqrtEps+norm(XOUT)),tolX}, ...
            verbosity > 0,detailedExitMsg};
        done = true;
        %Unlike the exitflag = 4 exit above, testConvergence is only called after a step is
        %accepted and the iterative display prints; therefore no need to print final iteration
        %as we do in the failed step exitflag=4 case above.
    elseif abs(newF - oldF) <= tolFun*oldF
        EXITFLAG = 3;        
        msgData = {{'optimlib:snls:Exit3basic',caller}, ...
                {'optimlib:snls:Exit3detailed',tolFun}, ...
                verbosity > 0,detailedExitMsg};
        done = true;
    end
end
