function [x,fval,exitflag,output] = fminsearch(funfcn,x,options,varargin)
%MATLAB Code Generation Library Function

% Limitations:
%   1. A problem struct input is not supported for code generation.
%   2. The Display option in the options struct is not honored.  No status
%   information is printed during execution.  This includes when terminating
%   early.  Check the third output argument instead.
%   3. The message and algorithm fields of the output struct are not supported.
%   4. The OutputFcn and PlotFcns options are ignored for code generation.

%   Copyright 2013-2018 The MathWorks, Inc.
%#codegen

narginchk(1,Inf);

% Disallow problem struct input
eml_invariant(~isstruct(funfcn), ...
                      'Coder:toolbox:OptimNoProblemStruct', upper(mfilename), ...
                      'IfNotConst', 'Fail')

defaultopt = struct('Display',     'notify', ...
                    'MaxIter',     MAXVAL, ...
                    'MaxFunEvals', MAXVAL, ...
                    'TolX',        1.0000e-04, ...
                    'TolFun',      1.0000e-04, ...
                    'FunValCheck', 'off', ...
                    'OutputFcn',   [], ...
                    'PlotFcns',    []);

% Undocumented syntax used by OPTIMSET to query default options structs
if nargin == 1 && eml_is_const(funfcn) && strcmp(funfcn, 'defaults')
    x = defaultopt;
    return;
end

eml_invariant(nargin >= 2, ...
                      'MATLAB:optimfun:fminsearch:NotEnoughInputs', ...
                      'IfNotConst', 'Fail');
eml_invariant(isa(x,'double'), ...
                      'MATLAB:optimfun:fminsearch:NonDoubleInput', ...
                      'IfNotConst', 'Fail');
eml_invariant(isreal(x), ...
                      'Coder:toolbox:OptimfunRealX', ...
                      upper(mfilename), ...
                      'IfNotConst', 'Fail');

% Get the default options if the user did not specify them
if nargin < 3
    options = defaultopt;
else
    % A valid options argument is required for code generation
    eml_invariant(isstruct(options) || (eml_is_const(isempty(options)) && isempty(options)), ...
                          'Coder:toolbox:OptimfunOptArg', ...
                          'IfNotConst', 'Fail');
end

% Get options values passed by the user
tolx          = double(optimget(options,'TolX',defaultopt,'fast'));
tolf          = double(optimget(options,'TolFun',defaultopt,'fast'));
maxfun_t      = optimget(options,'MaxFunEvals',defaultopt,'fast');
maxiter_t     = optimget(options,'MaxIter',defaultopt,'fast');
funValCheck   = strcmp(optimget(options,'FunValCheck',defaultopt,'fast'),'on');
outputfcn_t   = optimget(options,'OutputFcn',defaultopt,'fast');

% NOTE: Calling an OutputFcn does not work in codegen because of limitations on
% function handles in structs and not tracking constant struct fields.
haveOutputFcn = isempty(coder.target()) && ~isempty(outputfcn_t);
if haveOutputFcn
    coder.internal.prefer_const(outputfcn_t);
    if ischar(outputfcn_t)
        outputfcn = str2func(outputfcn_t);
    else
        outputfcn = outputfcn_t;
    end
else
    outputfcn = @noOpOutputFcn;
end

% Adjust if a special string was passed for MaxFunEvals or MaxIter
nvars = numel(x);
maxfun        = getMaxVal(maxfun_t, nvars, 'MATLAB:optimfun:fminsearch:OptMaxFunEvalsNotInteger');
maxiter       = getMaxVal(maxiter_t, nvars, 'MATLAB:optimfun:fminsearch:OptMaxIterNotInteger');

% Get and check the function to be called
[fcn, initialVal] = getCheckFcn(funfcn, funValCheck, x, varargin{:});

% Initialize parameters
n = nvars;
rho = 1;
chi = 2;
psi = 0.5;
sigma = 0.5;

% Compute the initial simplex.  Columns of v are points in the simplex
v = coder.nullcopy(eml_expand(coder.internal.scalarEg(x), [n,n+1]));
fv = coder.nullcopy(eml_expand(coder.internal.scalarEg(initialVal),[1,n+1]));

% Store f(x0)
fv(1) = initialVal;
fun_evals = 1;
itercount = 0;

% Call user output function to initialize it
actionID = 0;
if haveOutputFcn
    [optimValues, stop] = callOutputFcn(outputfcn,x,'init',itercount,...
                                        fun_evals,actionName(actionID),fv(1),varargin{:});
    if stop
        % Compute outputs and return if the user function requested a stop
        [x,fval,exitflag,output] = getOutputs(x, optimValues);
        return;
    end
end

% Call user output function for first iteration
if haveOutputFcn
    [optimValues, stop] = callOutputFcn(outputfcn,x,'iter',itercount,...
                                        fun_evals,actionName(actionID),fv(1),varargin{:});
    if stop
        % Compute outputs and return if the user function requested a stop
        [x,fval,exitflag,output] = getOutputs(x, optimValues);
        return;
    end
end

delta = 0.05;
delta_zero = 0.00025;
% Fill columns of v with x
for k = 1:n+1
    for j = 1:n
        v(j + n*(k-1)) = x(j);
    end
end

% Allocate intermediate values
xr = coder.nullcopy(x);
xe = coder.nullcopy(x);
nrows = coder.internal.indexInt(n);

% Update the diagonal of v(:,2:end) and the function values in fv
for k = 1:nrows
    colIdx = nrows*k;
    if x(k) ~= 0
        v(k + colIdx) = (1 + delta)*x(k);
    else
        v(k + colIdx) = delta_zero;
    end
    for j = 1:nrows
        xr(j) = v(j + colIdx);
    end
    fv(k+1) = checkfun(fcn,xr,funValCheck,varargin{:});
end


% We maintain the veritces, v, and function values, fv, such that:
%
%   fv(k) = fcn(v(:,k)),    for 1 <= k <= n+1.
%
% Moreover, idx is a vector of indices into the vertices v such that:
%
%  fv(idx(1)) <= fv(idx(k)) <= fv(idx(n+1)),    for 2 <= k <= n.
%
% This avoids sorting and permits performing indirect indexing.
idx = coder.internal.sortIdx(fv,'a');
doShrink = false;
itercount = IPP(itercount);
fun_evals = fun_evals + n;

% Call user output function with initial simplex
actionID = 1;
if haveOutputFcn
    colIdx = nrows*(idx(1) - 1);
    for k = 1:nrows
        x(k) = v(k + colIdx);
    end
    [optimValues, stop] = callOutputFcn(outputfcn,x,'iter',itercount,...
                                        fun_evals,actionName(actionID),fv(idx(1)),varargin{:});
    if stop
        % Compute outputs and return if the user function requested a stop
        [x,fval,exitflag,output] = getOutputs(x, optimValues);
        return;
    end
end

xbar = coder.nullcopy(zeros(n,1,'like',v));
lastCol = nrows*(idx(end) - 1);
firstCol = nrows*(idx(1) - 1);

% Main loop
while fun_evals < maxfun && itercount < maxiter && ~convergedq(v, fv, idx, tolx, tolf)
    % Compute the reflection point

    % xbar is the average of n vertices with smallest magnitude function value and
    % its shape is that of a column of v

    % Copy in first column
    for k = 1:nrows
        xbar(k) = v(k + firstCol);
    end

    % Add in additional columns
    for k = 2:nrows
        colIdx = nrows * (idx(k) - 1);
        for j = 1:nrows
            xbar(j) = xbar(j) + v(j + colIdx);
        end
    end

    % Scale the values and compute reflection point
    for k = 1:nrows
        xbar(k) = xbar(k)/n;
        xr(k) = (1+rho)*xbar(k) - rho*v(k + lastCol);
    end

    fxr = checkfun(fcn,xr,funValCheck,varargin{:});
    fun_evals = IPP(fun_evals);

    % Determine the operation to perform based on the value of fcn at the reflection
    % point
    if fxr < fv(idx(1))
        % Calculate an expansion point
        for k = 1:nrows
            xe(k) = (1+rho*chi)*xbar(k) - rho*chi*v(k + lastCol);
        end
        fxe = checkfun(fcn,xe,funValCheck,varargin{:});
        fun_evals = IPP(fun_evals);
        if fxe < fxr
            % Choose the expansion point
            for k = 1:nrows
                v(k + lastCol) = xe(k);
            end
            fv(idx(end)) = fxe;
            actionID = 2;
        else
            % Choose the reflection point
            for k = 1:nrows
                v(k + lastCol) = xr(k);
            end
            fv(idx(end)) = fxr;
            actionID = 3;
        end
    else                            % fv(1) <= fr
        if fxr < fv(idx(n))
            for k = 1:nrows
                v(k + lastCol) = xr(k);
            end
            fv(idx(end)) = fxr;
            actionID = 3;
        else                            % fxr >= fv(n)
            if fxr < fv(idx(end))
                % Do an outside contraction
                for k = 1:nrows
                    x(k) = (1 + psi*rho)*xbar(k) - psi*rho*v(k + lastCol);
                end
                fxc = checkfun(fcn,x,funValCheck,varargin{:});
                fun_evals = IPP(fun_evals);

                if fxc <= fxr
                    for k = 1:nrows
                        v(k + lastCol) = x(k);
                    end
                    fv(idx(end)) = fxc;
                    actionID = 4;
                else
                    % Perform a shrink
                    doShrink = true;
                    actionID = 5;
                end
            else
                % Perform an inside contraction
                for k = 1:nrows
                    x(k) = (1-psi)*xbar(k) + psi*v(k + lastCol);
                end
                fxc = checkfun(fcn,x,funValCheck,varargin{:});
                fun_evals = IPP(fun_evals);

                if fxc < fv(idx(end))
                    for k = 1:nrows
                        v(k + lastCol) = x(k);
                    end
                    fv(idx(end)) = fxc;
                    actionID = 6;
                else
                    % Perform a shrink
                    doShrink = true;
                    actionID = 5;
                end

            end
            if doShrink
                for k = 2:nrows+1
                    colIdx = nrows*(idx(k) - 1);
                    for j = 1:nrows
                        v(j + colIdx) = v(j + firstCol) + sigma * (v(j + colIdx) - v(j + firstCol));
                        x(j) = v(j + colIdx);
                    end
                    fv(idx(k)) = checkfun(fcn,x,funValCheck,varargin{:});
                end
                fun_evals = fun_evals + n;
            end
        end
    end

    % Update the sorted order since it may have changed
    if doShrink
        % We updated all elements in fv so it must be sorted

        % Take care to maintain same sorting stability as MATLAB.  At this point MATLAB
        % would have fv sorted to the order fv(idx).  We sort fv to this and
        % pass it to eml_sort_idx.  Then update idx based upon this value after
        % calling eml_sort_idx.
        fvt = coder.nullcopy(fv);
        for k = 1:numel(fv)
            fvt(k) = fv(idx(k));
        end

        % Back up idx
        idxb = idx;
        idx = coder.internal.sortIdx(fvt,'a');

        % Adjust idx to point to fv rather than fvt
        for k = 1:numel(idx)
            idx(k) = idxb(idx(k));
        end
        doShrink = false;
    else
        % We only inserted an element in idx(end).  So this method bubbles idx(end) to
        % the right sorted position maintaining that it is located after any
        % elements in fv which are equal to fv(idx(end)).
        idx = sort_idx_single_insert(fv, idx);
    end
    itercount = IPP(itercount);
    lastCol = nrows*(idx(end) - 1);
    firstCol = nrows*(idx(1) - 1);

    % Call user output function for this iteration
    if haveOutputFcn
        for k = 1:nrows
            x(k) = v(k + firstCol);
        end
        [optimValues, stop] = callOutputFcn(outputfcn,x,'iter',itercount,...
                                            fun_evals,actionName(actionID),fv(idx(1)),varargin{:});
        if stop
            [x,fval,exitflag,output] = getOutputs(x, optimValues);
            return;
        end
    end

end

% Record the final outputs
colIdx = nrows*(idx(1) - 1);
for k = 1:nrows
    x(k) = v(k + colIdx);
end
fval = fv(idx(1));

if nargout > 2
    % User requested exitflag so determine the cause.  If an OutputFcn terminated
    % the solver then we would not have reached this point
    if fun_evals >= maxfun
        exitflag = 0;
    elseif itercount >= maxiter
        exitflag = 0;
    else
        exitflag = 1;
    end
end

if nargout == 4
    % User requested output struct
    output.iterations = itercount;
    output.funcCount = fun_evals;
end

% Call user output function with final results
if haveOutputFcn
    callOutputFcn(outputfcn,x,'done',itercount,fun_evals,actionName(actionID),fval,varargin{:});
end

%--------------------------------------------------------------------------

function idx = sort_idx_single_insert(fv, idx)
% Given the index vector idx into the vector of function values, fv, update idx
% so that fv(idx) is sorted.  This assumes that the only element of fv which may
% be out of place is fv(idx(end)) which is true in all cases in the
% computational loop unless a shrink was performed.

% We know that fv and idx have at least 2 elements
for k = numel(idx):-1:2
    if fv(idx(k)) < fv(idx(k-1))
        % The current value is less than the next value so swap the corresponding indices
        cidx = idx(k);
        idx(k) = idx(k-1);
        idx(k-1) = cidx;
    end
end

%--------------------------------------------------------------------------

function p = convergedq(v, fv, idx, tolx, tolf)
% Return true if convergence criteria are met provided tolx and tolf.  Namely if
% ||*|| is the infinity norm convergence has occurred if:
%
%   1. max ||fv(1) - fv(k)|| <= tolf,    where 2 <= k <= n+1
%   2. max ||v(1) - v(k)||   <= tolx,    where 2 <= k <= n+1
coder.inline('always');
coder.internal.prefer_const(tolx, tolf);

ONE = ones(coder.internal.indexIntClass());

% Compute 1.
maxfv = zeros('like',fv); %abs(fv(idx(1)) - fv(idx(2)));
for k = 2:numel(fv)
    cfv = abs(fv(idx(ONE)) - fv(idx(k)));
    if cfv > maxfv
        maxfv = cfv;
    end
end
fvub = 10*eps(fv(idx(ONE)));

% Short-circuit if the function value has not converged
if maxfv > eml_max(tolf,10*eps(fvub))
    p = false;
    return;
end

% Compute 2.
nstride = coder.internal.indexInt(size(v,1));
ncols   = coder.internal.indexInt(size(v,2));
maxv  = zeros('like',v);
firstCol = (idx(ONE) - ONE) * nstride;
for j = coder.internal.indexInt(2):ncols
    colIdx = (idx(j) - ONE) * nstride;
    for k = ones(coder.internal.indexIntClass()):nstride
        cv = abs(v(k + firstCol) - v(k + colIdx));
        if cv > maxv
            maxv = cv;
        end
    end
end

vub = v(ONE + firstCol);              % Store v(1,idx(1))

% Compute max(v(:,idx(1)))
for k = coder.internal.indexInt(2):nstride
    cv = v(k + firstCol);
    if cv > vub
        vub = cv;
    end
end

p = maxv <= eml_max(tolx,10*eps(vub));

%--------------------------------------------------------------------------

function m = getMaxVal(maxIn,nvars,msg)
% Translates a string for maxIn to the proper iteration bound and error-checks
% that string.
coder.inline('always');
coder.internal.prefer_const(msg, nvars);
coder.internal.prefer_const(maxIn);
if ischar(maxIn)
    % Ensure this is the default value and compute the max
    eml_invariant(strcmpi(maxIn, MAXVAL), msg);
    m = 200*nvars;
else
    eml_invariant(coder.internal.isBuiltInNumeric(maxIn) && ...
                          eml_is_const(isscalar(maxIn)) && ...
                          isscalar(maxIn) && ...
                          maxIn == floor(maxIn), ...
                          msg)
    m = double(maxIn);
end

%--------------------------------------------------------------------------

function t = MAXVAL
% The default string for MaxFunEvals and MaxIter
coder.inline('always');
t = '200*numberofvariables';

%--------------------------------------------------------------------------

function [f,initialVal] = getCheckFcn(fcn, funValCheck, x, varargin)
% Take the user's input for funfcn and return a proper function handle after
% error-checking it
if coder.internal.isCharOrScalarString(fcn)
    eml_invariant(eml_is_const(fcn), ...
                          'Coder:toolbox:OptimfunConstStr', ...
                          'IfNotConst', 'Fail');
    f = str2func(fcn);
elseif isa(fcn, 'function_handle')
    f = fcn;
else
    eml_invariant(false, 'Coder:toolbox:OptimfunFcnOrStr', upper(mfilename));
end
initialVal = f(x,varargin{:});
eml_invariant(eml_is_const(isscalar(initialVal)) && isscalar(initialVal), ...
                      'Coder:toolbox:OptimFunScalarObjFcn', ...
                      upper(mfilename), ...
                      'IfNotConst', 'Fail');
if funValCheck
    checkFunValue(initialVal);
end

%--------------------------------------------------------------------------

function y = checkfun(f,x,funValCheck,varargin)
% Wrapper function to call user's function and validate that results are real
% and non NaN
coder.inline('always');
coder.internal.prefer_const(f,funValCheck);

v = f(x,varargin{:});
y = v(1);
if funValCheck
    checkFunValue(y);
end

%--------------------------------------------------------------------------

function checkFunValue(y)
if isnan(y)
    coder.internal.error('Coder:toolbox:OptimfunCheckFunNaNVal', ...
                         coder.internal.flt2str(y), ...
                         upper(mfilename));
elseif ~isreal(y)
    coder.internal.error('Coder:toolbox:OptimfunCheckFunComplexVal', ...
                         coder.internal.flt2str(y), ...
                         upper(mfilename));
end

%--------------------------------------------------------------------------

function x = IPP(x)
% Increment i.e. x++
coder.inline('always');
x = x+cast(1,'like',x);

%--------------------------------------------------------------------------

function [optimValues, stop] = callOutputFcn(outputfcn,x,state,iter,numf,how,f,varargin)
% Sub function which calls the user output function and reports if termination
% was requested
optimValues.iteration = iter;
optimValues.funccount = numf;
optimValues.fval = f;
optimValues.procedure = how;

stop = false;

if ~isempty(outputfcn)
    switch state
      case {'iter', 'init'}
        stop = outputfcn(x,optimValues,state,varargin{:}) || stop;
      case 'done'
        % No need to report the stop flag
        outputfcn(x,optimValues,state,varargin{:});
        stop = false;
      otherwise
        eml_invariant(false, 'MATLAB:optimfun:fminsearch:InvalidState');
    end
end


%--------------------------------------------------------------------------

function [x,fval,exitflag,output] = getOutputs(x, optimValues)
% If a user OutputFcn interrupts the optimization, this is called to create the
% outputs for fminsearch
coder.inline('always');
fval = optimValues.fval;
exitflag = -1;
output.iterations = optimValues.iteration;
output.funcCount  = optimValues.funccount;

%--------------------------------------------------------------------------

function str = actionName(id)
% Maintains a lookup table of action names to be passed to output functions.
% This prevents variable-sizing needed when storing the action string.
coder.inline('always');
coder.internal.prefer_const(id);
if id == 1
    str = 'initial simplex';
elseif id == 2
    str = 'expand';
elseif id == 3
    str = 'reflect';
elseif id == 4
    str = 'contract outside';
elseif id == 5
    str = 'shrink';
elseif id == 6
    str = 'contract inside';
else
    str = '';
end

%--------------------------------------------------------------------------

function stop = noOpOutputFcn(x,optimValues,state,varargin) %#ok
coder.inline('always');
stop = false;
