function [x,fval] = fminsearch(funfcn,x,options,varargin)
%FMINSEARCH Multidimensional unconstrained nonlinear minimization (Nelder-Mead).
%
%   BAYESOPTIM.FMINSEARCH is a simplified version of MATLAB's FMINSEARCH
%   that allows minimizing a function starting from multiple initial
%   points. It also combines calls to the objective function.
%
%   BAYESOPTIM.FMINSEARCH is most beneficial when the objective function
%   is vectorized and the time complexity with respect to the number of
%   points is sublinear; this can happen due to function overhead or/and
%   repeated computations (as in GP regression).
%
%   BAYESOPTIM.FMINSEARCH is only for internal use and may change in the
%   future without any notice.
%
%   BAYESOPTIM.FMINSEARCH has the following limitations with respect to
%   FMINSEARCH:
%
%   - In X_HAT = BAYESOPTIM.FMINSEARCH(FUN,X0) the objective function FUN should
%     accept input X as a matrix where every row is a different point to evaluate 
%     and the number of columns is the number of variables being optimized. 
%   - In X_HAT = BAYESOPTIM.FMINSEARCH(FUN,X0) the start points X0 is a matrix
%     where every row is a different initialization point. The output X_HAT has the
%     same number of rows as X0.
%   - In X_HAT = FMINSEARCH(FUN,X0,OPTIONS) the options PlotFcns
%     and OutputFcn are ignored.
%   - FMINSEARCH only returns up to 2 output arguments.
%   - Only 'iter'/'none' verbosity is supported.
%   - No support for anonymous functions.
%   - Does not check function evaluation for complex values or NaNs.
%
%   See also FMINSEARCH, BAYESOPT, OPTIMSET

%   Copyright 1984-2021 The MathWorks, Inc.


defaultopt = struct('Display','notify','MaxIter','200*numberOfVariables',...
    'MaxFunEvals','200*numberOfVariables','TolX',1e-4,'TolFun',1e-4, ...
    'FunValCheck','off','OutputFcn',[],'PlotFcns',[]);

% If just 'defaults' passed in, return the default options in X
if nargin == 1 && nargout <= 1 && strcmpi(funfcn,'defaults')
    x = defaultopt;
    return
end

if nargin < 3, options = []; end

% Detect problem structure input
if nargin == 1
    if isa(funfcn,'struct') 
        [funfcn,x,options] = separateOptimStruct(funfcn);
    else % Single input and non-structure
        error('MATLAB:fminsearch:InputArg',...
            getString(message('MATLAB:optimfun:fminsearch:InputArg')));
    end
end

if nargin == 0
    error('MATLAB:fminsearch:NotEnoughInputs',...
        getString(message('MATLAB:optimfun:fminsearch:NotEnoughInputs')));
end

% Check for non-double inputs
if ~isa(x,'double')
  error('MATLAB:fminsearch:NonDoubleInput',...
    getString(message('MATLAB:optimfun:fminsearch:NonDoubleInput')));
end

[s,n] = size(x);
% n -> number Of Variables 
% s -> number Of Starts

% Check that options is a struct
if ~isempty(options) && ~isa(options,'struct')
    error('MATLAB:fminsearch:ArgNotStruct',...
        getString(message('MATLAB:optimfun:commonMessages:ArgNotStruct', 3)));
end

printtype = optimget(options,'Display',defaultopt,'fast');
tolx = optimget(options,'TolX',defaultopt,'fast');
tolf = optimget(options,'TolFun',defaultopt,'fast');
maxfun = optimget(options,'MaxFunEvals',defaultopt,'fast');
maxiter = optimget(options,'MaxIter',defaultopt,'fast');

% In case the defaults were gathered from calling: optimset('fminsearch'):
if ischar(maxfun) || isstring(maxfun)
    if strcmpi(maxfun,'200*numberofvariables')
        maxfun = 200*n;
    else
        error('MATLAB:fminsearch:OptMaxFunEvalsNotInteger',...
            getString(message('MATLAB:optimfun:fminsearch:OptMaxFunEvalsNotInteger')));
    end
end
if ischar(maxiter) || isstring(maxiter)
    if strcmpi(maxiter,'200*numberofvariables')
        maxiter = 200*n;
    else
        error('MATLAB:fminsearch:OptMaxIterNotInteger',...
            getString(message('MATLAB:optimfun:fminsearch:OptMaxIterNotInteger')));
    end
end

switch printtype
    % BAYESOPTIM.FMINSEARCH supports only 'iter' verbosity for internal debugging
    % purpose
    case {'iter','iter-detailed'}
        prnt = 3;
    otherwise
        prnt = 0;
end

% Initialize parameters
rho = 1; chi = 2; psi = 0.5; sigma = 0.5;
onesn = ones(1,n);
two2np1 = 2:n+1;
one2n = 1:n;
func_evals = 0;
itercount = 0;

% Set up a simplex near the initial guess.  (credit L.Pfeffer at Stanford)
usual_delta = 0.05;         % 5 percent deltas for non-zero terms
zero_term_delta = 0.00025;  % Even smaller delta for zero elements of x
v = zeros(n,n+1,s); 
v(:) = x(repelem(1:s,n+1),:)';
h = n+1:n+1:(n+1)*n*s;
v(h) = v(h) .* (1+usual_delta);
v(h(v(h)==0)) = zero_term_delta;

% Compute function value for simplex
fv = zeros(n+1,s);
fv(:) = funfcn(v(:,:)',varargin{:});
func_evals = func_evals+1;

% Print out initial f(x) as 0th iteration
if prnt == 3
    header = ' Iteration   Func-count     min f(x)';
    disp(' ')
    disp(header)
    for i = 1:s
        fprintf(' %5.0f        %5.0f     %12.6g\n', itercount, func_evals, fv(i));
    end
end

% sort so v(1,:) has the lowest function value
[fv,j] = sort(fv);
for i = 1:s
    v(:,:,i) = v(:,j(:,i),i);
end

itercount = itercount + 1;

if prnt == 3
    for i = 1:s
        fprintf(' %5.0f        %5.0f     %12.6g\n', itercount, func_evals, fv(i))
    end
end

% Main algorithm: iterate until 
% (a) the maximum coordinate difference between the current best point and the 
% other points in the simplex is less than or equal to TolX. Specifically,
% until max(||v2-v1||,||v3-v1||,...,||v(n+1)-v1||) <= TolX,
% where ||.|| is the infinity-norm, and v1 holds the 
% vertex with the current lowest value; AND
% (b) the corresponding difference in function values is less than or equal
% to TolFun. (Cannot use OR instead of AND.)
% The iteration stops if the maximum number of iterations or function evaluations 
% are exceeded
FV = zeros(s,4);
while func_evals < maxfun && itercount < maxiter
    done =  max(abs(fv(1,:)-fv(two2np1,:))) <= max(tolf,10*eps(fv(1,:))) & ...
        squeeze(max(abs(v(:,two2np1,:)-v(:,onesn,:)),[],[1 2]) <= max(tolx,10*eps(max(v(:,1,:),[],1))))';
    
    if all(done) % optimization for ALL the threads (i.e. initial points) must meet the output conditions
        break
    end
    
    % xbar = average of the n (NOT n+1) best points
    xbar = sum(v(:,one2n,:), 2)/n;
    
    xr = (1 + rho)*xbar - rho*v(:,end,:);         % (1) reflexion point (always needed)
    xe = (1 + rho*chi)*xbar - rho*chi*v(:,end,:); % (2) future expansion
    xcc = (1-psi)*xbar + psi*v(:,end,:);          % (3) future inside contraction
    xc = (1 + psi*rho)*xbar - psi*rho*v(:,end,:); % (4) future outside contraction
           
    FV(~done,:) = reshape(funfcn([xr(:,~done) xe(:,~done) xcc(:,~done) xc(:,~done)]',varargin{:}),sum(~done),4);
    func_evals = func_evals+1;
    
    fxr = FV(:,1)';
    fxe = FV(:,2)';
    fxcc = FV(:,3)';
    fxc = FV(:,4)';
    fv1 = fv(1,:);
    fvn = fv(n,:);
    fve = fv(end,:);
    
    do_expand = fxr<fv1 & fxe<fxr;
    do_contract_or_shrink = fv1<=fxr & fxr>=fvn;
    do_reflect = ~(do_expand | do_contract_or_shrink);
    do_contract_in  = do_contract_or_shrink & fxr>=fve & fxcc<=fve;
    do_contract_out = do_contract_or_shrink & fxr<fve  & fxc<=fxr; 
    do_contract = do_contract_out | do_contract_in;
    do_shrink = do_contract_or_shrink & ~do_contract;
    
    do_expand = do_expand & ~done;
    do_reflect = do_reflect & ~done;
    do_contract_out = do_contract_out & ~done;
    do_contract_in = do_contract_in & ~done;
    do_shrink = do_shrink & ~done;
    
    
    if any(do_expand)
        v(:,end,do_expand) = xe(:,:,do_expand);
        fv(end,do_expand) = fxe(do_expand);
    end
    
    if any(do_reflect)
        v(:,end,do_reflect) = xr(:,:,do_reflect);
        fv(end,do_reflect) = fxr(do_reflect);
    end
    
    if any(do_contract_out)
        v(:,end,do_contract_out) = xc(:,:,do_contract_out);
        fv(end,do_contract_out) = fxc(do_contract_out);
    end
    
    if any(do_contract_in)
        v(:,end,do_contract_in) = xcc(:,:,do_contract_in);
        fv(end,do_contract_in) = fxcc(do_contract_in);
    end
    
    if any(do_shrink)
        vn = v(:,1,do_shrink)+sigma.*(v(:,two2np1,do_shrink)-v(:,1,do_shrink));
        v(:,two2np1,do_shrink) = vn;
        fv(two2np1,do_shrink) = reshape(funfcn(vn(:,:)',varargin{:}),n,sum(do_shrink));
        func_evals = func_evals+1;
    end

    [fv,j] = sort(fv);
    for i = 1:s
        v(:,:,i) = v(:,j(:,i),i);
    end

    itercount = itercount + 1;
    
    if prnt == 3
        for i = 1:s
            fprintf(' %5.0f        %5.0f     %12.6g\n', itercount, func_evals, fv(i))
        end
    end
end   % while

x = reshape(v(:,1,:),n,s)';
fval =  fv(1,:)';

if prnt == 3
    if func_evals >= maxfun
        msg = getString(message('MATLAB:optimfun:fminsearch:ExitingMaxFunctionEvals', sprintf('%f',fval)));
    elseif itercount >= maxiter
        msg = getString(message('MATLAB:optimfun:fminsearch:ExitingMaxIterations', sprintf('%f',fval)));
    else
        msg = getString(message('MATLAB:optimfun:fminsearch:OptimizationTerminatedXSatisfiesCriteria', ...
            sprintf('%e',tolx), sprintf('%e',tolf)));
    end
    disp(' ')
    disp(msg)
end

end

