% Nelder-Mead downhill simplex method
% 
% Algorithm details: see lecture notes or Wikipedia
% Use octave 
%
function [xmin,fmin] = nelder_mead_simplex(f,x)
% Nelder-Mead downhill simplex method
% input: f    - function to minimize
%        x    - x(n+1,n) matrix
%               legs (vertices) of the starting simplex
%               n parameters requires n+1 legs,
%               each leg is a n-dimensional vector 
% output: xmin - found minimum   
%         fmin - f(xmin)
    
    n=size(x,2); % number of parameters, dimension
    
    % test: Himmelblau function
    %f=@(x) (x(1).^2+x(2)-11).^2+(x(1)+x(2).^2-7).^2

    if(size(x,1)~=n+1)
        disp(['Error: initial simplex must be ' num2str(n+1) ' X '  ...
              num2str(n)])        
        x=nan;
        return
    end
    fxi=zeros(n+1,1);
    for i=1:n+1
        fxi(i)=f(x(i,:));  % f(x) at initial leg points
    end
    % standard choice of internal parameters
    alpha = 1.0;     % reflection coefficient   
    gamma = 2.0;     % expansion coeff.
    rho   = -0.5;  % contraction coeff.
    sigma = 0.5;   % reduction coeff.
    
    acc = 1e-10 ;   % accuracy goal
    % sort values to increasing order,
    % f(x_1)<=f(x_2)<=...<=f(x_n+1)
    % use builtin "sort" function, first column leg index, second
    % function values 
    [fxi,ind] = sort(fxi);
    x = x(ind,:);
    diff = abs(fxi(n+1)-fxi(1)); % height difference of legs  
    outfile = fopen('out', 'w');
    fprintf(outfile,'%f %f %f\n',[x fxi]');
    fprintf(outfile,'%f %f %f\n',x(1,:)',fxi(1)');
    fprintf(outfile,'\n\n');
    while(diff>acc)        
        % centroid (center of gravity of n points), leaving out worst point x_n+1
        x0 = sum(x(1:n,:),1)/n;
        % REFLECTION
        xr = x0 + alpha*(x0-x(n+1,:));
        fxr = f(xr);
        if(fxi(1)<=fxr && fxr<fxi(n))
            % replace worst point with xr
            x(n+1,:)=xr;
            fxi(n+1)=fxr;
        elseif(fxr<fxi(1))
                % best so far, expand simplex
                xe =  x0 + gamma*(x0-x(n+1,:));
                fxe= f(xe);
                if(fxe<fxr)
                    % even better, replace worst point with xe                 
                    x(n+1,:)=xe;
                    fxi(n+1)=fxe;
                else
                    % expansion makes it worse, replace worst point with xr
                    x(n+1,:)=xr;
                    fxi(n+1)=fxr;
                end
        else
            % CONTRACTION            
            xc = x0 + rho*(x0-x(n+1,:));
            fxc=f(xc);
            if(fxc<fxi(n+1))
                % replace worst point with xc
                x(n+1,:)=xc;
                fxi(n+1)=fxc;
            else
                % REDUCTION
                % all points toward best
                for i=2:n+1
                    x(i,:) = x(1,:)+sigma*(x(i,:)-x(1,:));                                    
                    fxi(i)=f(x(i,:));  
                end
            end
        end
        [fxi,ind] = sort(fxi);
        x = x(ind,:);
        fprintf(outfile,'%f %f %f\n',[x fxi]');
        fprintf(outfile,'%f %f %f\n',x(1,:)',fxi(1)');
        fprintf(outfile,'\n\n');
        diff = abs(fxi(n+1)-fxi(1)); % convergence
        end
    fclose(outfile);
    xmin = x(1,:);
    fmin = f(xmin);
end
 

