function [X,type,centroid] = nelder_mead_step(f,Xcur)
% Take one step of the Nelder-Mead procedure
%
% [X,type] = nelder_mead_step(f,Xcur)

% this is a really simplistic implementation
% you can improve it considerably...
X = Xcur;
npts = size(X,2);
n = size(X,1);
assert(npts == n+1); % we need a simplex!
fs = zeros(npts,1);
for i=1:npts
    fs(i) = f(X(:,i));
end
[sfs,p] = sort(fs); % sort in increasing order

Xmid = X(:,p(1:npts-1));
assert(rank(Xmid) == npts-1); % we need a non-singular simplex!

xworst = X(:,p(end));
centroid = mean(Xmid,2); 

xbar = @(alpha) centroid + alpha*(xworst - centroid);

fm1 = f(xbar(-1));
if sfs(1) <= fm1 && fm1 <= sfs(npts-1)
    % just go there!
    xnew = xbar(-1);
    step = -1;
elseif fm1 < sfs(1)
    % we found a much better point, 
    % try and improve
    fm2 = f(xbar(-2));
    if fm2 < fm1
        % we made things larger!
        xnew = xbar(-2);
        step = -2;
    else
        xnew = xbar(-1);
        step = -1;
    end
else 
    % fm1 > second worst point
    step = 0;
    if sfs(end-1) <= fm1 && fm1 < sfs(end)
        % try to stay outside as we got better
        fmhalf = f(xbar(-1/2));
        if fmhalf <= fm1 
            step = -1/2;
            xnew = xbar(-1/2);
        end
    else
        fhalf = f(xbar(1/2));
        if fhalf < sfs(end)
            xnew = xbar(1/2);
            step = 1/2;
        end
    end
end

if step ~= 0
    X = [Xmid xnew];
else
    % shrink towards the best point
    xbest = X(:,p(1));
    X = Xcur;
    for i=1:npts
        if i~=p(1) 
            X(:,i) = 1/2*(X(:,i) + xbest);
        end
    end
end
        
        
    