# coding: utf-8

# ## Лабораторная работа №5.

# ### Вирцева Наталья, ИУ9-81, Вариант 4.

# In[163]:

import pylab
import numpy as np
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
from matplotlib import cm

from scipy.optimize import minimize, minimize_scalar
from scipy.optimize import fmin

get_ipython().magic(u'pylab inline')
#import seaborn as sns


# функция Розенброка:

# In[164]:

a, b, f0 = 250, 2, 50
#russian open source summit

def rozenbrock(x1, x2):
    return a*(x1**2 - x2)**2 + b*(x1 - 1)**2 + f0

def grad_r(x1, x2):
    dx1 = 4*x1*a*(x1**2 - x2) + 2*b*(x1 - 1) #
    dx2 = -2*a*(x1**2 - x2)  #
    return np.array([dx1, dx2])


n, m = 0.05, 0.05
x = numpy.linspace(0.37,0.43, 100, endpoint=False)
y = numpy.linspace(0.1,0.2,100, endpoint=False)
x, y = numpy.meshgrid(x, y)
z = rozenbrock(x, y)

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()
#---------------------------------------
n, m = 0.11, 0.11
x = numpy.linspace(-n,n, 100, endpoint=False)
y = numpy.linspace(-m,m,100, endpoint=False)
x, y = numpy.meshgrid(x, y)
z = rozenbrock(x, y)

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()
#---------------------------------------------
n, m = 2.1, 15.1
x = numpy.linspace(-n,n, 100, endpoint=False)
y = numpy.linspace(-m,m,100, endpoint=False)
x, y = numpy.meshgrid(x, y)
z = rozenbrock(x, y)

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()
#--------------------------------------------
n, m = 100.1, 10000.1
x = numpy.linspace(-n,n, 100, endpoint=False)
y = numpy.linspace(-m,m,100, endpoint=False)
x, y = numpy.meshgrid(x, y)
z = rozenbrock(x, y)

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)

#ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap=cm.coolwarm, linewidth=0, antialiased=False)
pylab.show()


# #### функции ограничений:

# In[165]:

def g1(x):
    return x[0]**2 + x[1]**2 - 2

def g2(x):
    return -x[0]

def g3(x):
    return -x[1]

def g_plus(x, func):
    res = func(x)
    #print x, res
    if res > 0:
        return res
    return 0

n, m = 0.5, 0.5 #1, 1
x = numpy.linspace(-n,n, 100, endpoint=False)
y = numpy.linspace(-m,m,100, endpoint=False)
x, y = numpy.meshgrid(x, y)
z = rozenbrock(x, y)

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)

x = numpy.linspace(-n,n, 100, endpoint=False)
y = numpy.linspace(-m,m,100, endpoint=False)
x, y = numpy.meshgrid(x, y)

z = g1([x, y])

#fig = pylab.figure()
#axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()

z = g2([x, y])

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()

z = g3([x, y])

fig = pylab.figure()
axes = fig.gca(projection='3d')
axes.plot_surface(x, y, z,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
pylab.show()


# #### Метод штрафных функций:

# In[166]:

def f(x):
    return rozenbrock(x[0], x[1])

def grad_f(x):
    return grad_r(x[0], x[1])

g = [g1, g2, g3]


# In[167]:



def gap_functions(x, r = 1., c = 0.618, epsela = 0.01, k = 0):
    print x
    if k > 50:
        return 'too much iterations', x, f(x)
    def P(x):
        sum1 = 0.#sum([func(x)**2 for func in g])
        sum2 = sum([g_plus(x, func)**2 for func in g])
        return r/2.*(sum1 + sum2)
    def F(x):
        #print x
        return f(x) + P(x)
    
    #-----------------
    """n, m = 0.05, 0.05
    xx = numpy.linspace(x[0]-n,x[0]+n, 100, endpoint=False)
    yy = numpy.linspace(x[1]-m,x[1]+m,100, endpoint=False)
    xx, yy = numpy.meshgrid(xx, yy)
    zz = []
    for i in range(len(xx)):
        string = []
        for j in range(len(xx[0])):
            string.append(F([xx[i][j], yy[i][j]]))
        zz.append(string)

    fig = pylab.figure()
    axes = fig.gca(projection='3d')
    axes.plot_surface(xx, yy, zz,  alpha=0.8, cmap=cm.coolwarm,  linewidth=0, antialiased=True)
    pylab.show()
    """
    #---------------------
    #x_z = fmin(F(x),) #(F(x), x)
    x_z = minimize(lambda x: F(x), (x[0], x[1]), method='SLSQP').x
    print 'xz=', x_z
    if P(x_z) < epsela:
        return x_z, f(x_z), k
    rk, xk = c*r, x_z
    return gap_functions(x_z, c*r, epsela, k + 1)
    
    #alpha = -minimize_scalar(lambda l: func(x + l * grad_x), bounds = (-100, 100), method = 'Golden').x


# In[168]:

#print gap_functions(random.random(2)*100)#[-100.3,1000.6])
get_ipython().magic(u'time gap_functions(random.random(2)*100)')


# #### Метод барьерных функций:

# In[169]:

def barrier_functions(x, r = 1., c = 1.618, k = 0, epsela = 0.01):
    #c =1.618
    if k > 50: return 'too much iterations', x, k
    def P(x):
        return -r*sum([1/(func(x) + epsela) for func in g])
    def F(x):
        return f(x) + P(x)
    #print 'start'
    x_z = minimize(lambda x: F(x), (x[0], x[1]), method='SLSQP').x
    #print 'finish'
    if math.fabs(P(x_z)) <= epsela:
        return x_z, f(x_z), k
    return barrier_functions(x_z, r/c, c, k+1)
    


# In[170]:

#print barrier_functions([-100., -100.])
get_ipython().magic(u'time barrier_functions([-100., -100.])')


# #### Метод функций Лагранжа:

# In[171]:

def Lagranj_functions(x, r = 1., c = 1.618, lamda = [1.,1.,1.],                       mu = [1.,1.,1.], epsela = 0.001, k = 0):
    if k > 50: return 'too much iterations', x, f(x), k
    def P(x):
        sum2 = 0. #sum([func(x)**2 for func in g])
        sum3 = sum([(max(0., mu[i] + r*g[i](x))**2 - mu[i]**2)                     for i in range(len(g))])
        return r/2.*sum2 + 1/(2*r)*sum3
    def L(x):
        sum1 = 0. #sum([g[i](x)*lamda[i] for i in range(len(g))])
        return f(x) + sum1 + P(x)
    
    x_min = minimize(lambda x: L(x), (x[0], x[1]), method='SLSQP').x
    if P(x_min) <= epsela:
        return x_min, f(x_min), k
    #new_lamda = [lamda[i] +r*g[i](x_min) for i in range(len(g))]
    new_mu = [max(mu[i] + r*g[i], 0) for i in range(len(g))]
    return Lagranj_functions(x_min, c*r, c, lamda + r*g(x_min), new_mu,                             epsela, k + 1)
    


# In[172]:

get_ipython().magic(u'time Lagranj_functions(random.random(2)*100)')


# #### Метод проекции градиента:

# In[176]:

def func(x):
    return rozenbrock(x[0], x[1])

def Ak(x):
    ak = [[2*x[0], 2*x[1]],
          [-1,     0     ],
          [ 0,     -1    ]]
    return np.matrix(ak)

#E = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
E = np.identity(2)#array([[1, 0], [0, 1]])

active_g = [g1, g2, g3]


def equation( x, delta_x):
    b = 2*(x[0]*delta_x[0] + x[1]*delta_x[1])
    a = delta_x[0]**2 + delta_x[1]**2
    c = x[0]**2 + x[1]**2
    D = b**2 - 4*a*c
    if D < 0:
        return 
    sqrtD = math.sqrt(D)
    return (-b + sqrtD)/(2*a), (-b - sqrtD)/(2*a) 

def find_solutoin_g(x, delta_x):
    result = []
    #for g1
    result += [equation(x, delta_x)]
    result += [(-x[0]/delta_x[0])] #for g2
    result += [(-x[1]/delta_x[1])] #for g3
    return result


def probable_directions(x, k = 0, epsela1 = -0.01, epsela2 = 0.01, M = 5):
    #step 4
    if k == 5:
        return x
    def count_g(x):
        g_val = [func(x) for func in g] #g = [g1, g2, g3]
        #step 5
        less = [(func(x) >= epsela1 and func(x) <= 0) for func in g]
        if True in less:
            grad = grad_f(x)
            if np.linalg.norm(grad) != 0.:#[0., 0.]:
                return count_delta_x(x)
            elif k > 0:
                return count_lambda(x)
            else:
                print 'check if x_0 in proper space', x, k
                return count_lambda(x)
                #count_g()
        #step 6
        tau = np.array([(-func(x)) for func in g])
        #print 'tau:', tau, '; x:', x, k
        #mtr = np.matrix(np.dot(Ak(x), Ak(x).T))
        #print mtr, mtr.I
        print Ak(x).I
        print 'Ak', Ak(x), 'mul', np.dot(Ak(x), Ak(x).T)
        #np.dot(np.dot(Ak(x).T, np.dot(Ak(x), Ak(x).T)),tau.T)
        xv = x + np.dot(np.dot(Ak(x).T, np.dot(Ak(x), Ak(x).T).I),                        tau.T)
        xv = np.array(xv)[0]
        print 'answer x =', x0
        print 'answer f(x) =', func(x0)
        return count_delta_x(xv)
    #step 7
    def count_delta_x(x):
       
        x = [x[0] + 0.001, x[1]+ 0.001]
        
        print 'counting x=', x, 'Ak=', Ak(x)
        newmult = np.dot(Ak(x), Ak(x).T) + 0.001
        print 'AkAkt=', newmult
        mulAks = np.dot(np.dot(Ak(x).T, newmult.I), Ak(x))
        delta_x = -np.dot(E - mulAks, grad_f(x))#grad_f(x).T)
        #step 8
        if np.linalg.norm(delta_x) <= epsela2:
            return count_lambda(x)
        else:
            return count_point(x, np.array(delta_x)[0])
        
    #step 9
    def count_lambda(x):
        global g_active
        newmult = np.dot(Ak(x), Ak(x).T) + 0.001
        temp2 = newmult.I
        temp1 = np.dot(Ak(x), grad_f(x))
        lamda_k = -np.dot(temp2, temp1.T)#grad_f(x).T))
        if np.linalg.norm(lamda_k) <= 0:
            return 'not full - may be solution, may be not', x, f(x)
        #+ need 
        print 'need to check enough minimum condotions', x, func(x), k
        lamda_ar = np.array(lamda_k)[0]
        index = np.where(lamda_ar == max(lamda_ar))
        #index = lamda_ar.index(max(lamda_ar))
        #g_active.remove(g_active[index])
        #if conditions are not - not enough - Ak should also be modified
        return count_delta_x(x)
    
    #step 10
    def count_point(x, delta_x):
        #x_next = x + alpha*delta_x
        #step 11
        print 'delta_x:', delta_x
        res = minimize_scalar(lambda l: func(x + l*delta_x), bounds=(0, 100),method='bounded')
        print res.message
        alpha = res.x
        alpha_min = min(find_solutoin_g(x, delta_x))
        if alpha_min > 0:
            alpha = min(alpha_min, alpha)
        x_next = x + alpha*delta_x
        return probable_directions(x_next, k+1)
    
    #step 3
    if k >= M:
        return count_lambda(x)
    else:
        return count_g(x)


# In[177]:

x = probable_directions(np.array([0.8,  0.7]))
print x


# In[175]:

x0 = [ 0.85748723,  0.73500404]


# In[ ]: