#!/net/python/bin/python
import numpy as num

def minimize(f,p0,Nmax=1000,ftol = 1E-5,scale=1.0,quiet=True):
    n = len(p0)
    N = n+1 #number of points in the simplex

    p = [num.array(p0)] + [p0+unitvector(j,n) for j in range(n)]
    f_p = [f(pi) for pi in p]

    #sort p and f_p
    eval = [(f_p[j],p[j].tolist()) for j in range(n+1)]
    eval.sort()
    f_p = [e[0] for e in eval]
    p = [num.array(e[1]) for e in eval]

    #perform minimization
    i = 0
    min_vec = p[0]

    while True:
        i+=1
        if i > Nmax:
            print "Maximum iterations reached! (%i)" % Nmax
            break

        old_min = p[0]

        #perform simplex step
        p_bar = sum(p[:-1]) * 1.0/n
        p_star = p_bar + scale*(p_bar-p[-1])
        f_star = f(p_star)

        if f_star <= f_p[0]:
            #this is better than the best point, so
            # try extrapolating two times farther
            p_star2 = p_bar + 2*scale*(p_bar - p[-1])
            f_star2 = f(p_star2)

            if f_star < f_star2:
                j = num.searchsorted(f_p,f_star)
                p = p[:j] + [p_star] + p[j:-1]
                f_p = f_p[:j] + [f_star] + f_p[j:-1]
                
            else:
                j = num.searchsorted(f_p,f_star2)
                p = p[:j] + [p_star2] + p[j:-1]
                f_p = f_p[:j] + [f_star2] + f_p[j:-1]
                
        elif f_star >= f_p[1]:
            #f_star is worse than the second highest point,
            # so we will try a contraction
            p_dag = 0.5*scale*(p_bar+p[-1])
            f_dag = f(p_dag)
            
            if f_dag < f_p[0]: #f_dag < f(p_min)
                j = num.searchsorted(f_p,f_dag)
                p = p[:j] + [p_dag] + p[j:-1]
                f_p = f_p[:j] + [f_dag] + f_p[j:-1]
                
            else: #none of these options is better! - reduce the dimension
                p_l = p[0]
                p = [0.5*(p_l+pi) for pi in p]
                #sort p and f_p
                eval = [(f_p[j],p[j].tolist()) for j in range(n+1)]
                eval.sort()
                f_p = [e[0] for e in eval]
                p = [num.array(e[1]) for e in eval]
        else:
           #we will use f_star
            j = num.searchsorted(f_p,f_star)
            p = p[:j] + [p_star] + p[j:-1]
            f_p = f_p[:j] + [f_star] + f_p[j:-1]
            
        min_vec = p[0]

        if ftol > f_p[-1]-f_p[0]:
            if not quiet:
                print "converged to f(p) = %.5g" % f_p[0]
                print "at (x1,x2) = ", p[0]
                print "   after %i iterations!" % i
                print "   ftol = %.2g" % ftol
                print "   scale factor = %.1f" % scale
            break
    
    return min_vec

def unitvector(n,N):
    """
    returns an N-dimensional unit vector in direction n
    """
    assert n<N and n>=0
    v = num.zeros(N)
    v[n] = 1.0
    return v

def replace_highest(p,f_p,p_star,f_star):
    """
    returns p,f_p with p_star and f_star inserted, and p[-1] and f[-1] removed

     - f_p is a list of evaluations f(p) sorted in ascending order
     - p is a list of the corresponding vectors
     - p_star is a point to add to the list
     - f_star is the evaluation f(p_star)
    """
    j = num.searchsorted(f_p,f_star)
    p = p[:j] + [p_star] + p[j:-1]
    f_p = f_p[:j] + [f_star] + f_p[j:-1]
    return p,f_p