import numpy as np
from numpy import *
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib import animation
from scipy.optimize import minimize

global a, b

func_eval = 0

a = 4; b = 2
# given point:
x0 = [12, 8]

# find closest point on line:
def y(x):
	global a, b
	return a*x+b

# minimizing distance
def dist(x):
	global x0
	d_sq = (x0[0]-x)**2 + (x0[1]-y(x))**2
	return d_sq

# subjet to
def c(x):
	global a, b
	c = y(x)-a*x-b
	return c

def f(x):
	global func_eval
	func_eval += 1
	u = 1.
	f = dist(x) + u/2.*c(x)**2
	return f

res = minimize(f,-100,method='powell',options={'xtol':1e-8})
print res
# def NM_simplex(f,[xs,ys]):
# pick 2 more points
D = 1
xs = -100
p = zeros([3,D])
p_ord = zeros([3,D])
ff = zeros([3,1])
p[0] = [xs]
p[1] = [xs+0.05]
p[2] = [xs+0.2]
ff = f(p)
order = argsort(ff,0)
p_ord[0] = p[order[0]]
p_ord[1] = p[order[1]]
p_ord[2] = p[order[2]]
p = p_ord

it = 0
pp = []; zz = [];
while 1:
    ff = f(p)
    midpoint = (p[0] + p[1])/2
    p_new = 2*midpoint-p_ord[2]
    f_new = f(p_new)
    if f_new < ff[0]:
#       Good move, new best
        p_new2 = 3*midpoint-2*p_ord[2]
        f_new2 = f(p_new2)
        if f_new2 < f_new:
#           better move, grow
            p[2] = p[1]
            p[1] = p[0]
            p[0] = p_new2 
        else:
#           not that good
            p[2] = p[1]
            p[1] = p[0]
            p[0] = p_new
    elif f_new < ff[1]:
#       better than 2nd best
        p[2] = p[1]
        p[1] = p_new
    else:
#       still worst
        p_new = 3.*midpoint/2.-p[2]/2.
        f_new = f(p_new)
        if f_new < ff[1]:
#           shrink, not worst
            p[2] = p[1]
            p[1] = p_new
        else:
            p_new = (midpoint+p[2])/2.
            f_new = f(p_new)
            if f_new < ff[1]:
#               shrink more, not worst
                p[2] = p[1]
                p[1] = p_new
            else:
#               still worst, shrink all vertices
                p[0] = (p_ord[0]+p_ord[0])/2.
                p[1] = (p_ord[1]+p_ord[0])/2.
                p[2] = (p_ord[2]+p_ord[0])/2.

    pp = np.append(pp,p[0],axis=-1)
    zz = np.append(zz,f(p))

    # print p[0]
    it += 1
    diff = abs(f(p[0])-zz[len(zz)-1])
    # print diff
    if (diff < 1e-8) and (c(p[0])**2 < 1e-6):
        print "Done!"
        print "Minimum @ " + str(p[0]) + ", " + str(y(p[0]))
        print "Iterations: " + str(it)
        print "Function Evaluations: " + str(func_eval)
        break

# x = arange(-3,3,0.1)
# y = arange(-3,3,0.1)
# xx, yy = meshgrid(x, y)
# z = f(xx, yy)