import numpy as np
import math

def grad(func,point,eps):
        ret = np.zeros(len(point))
        for i in range(0,len(point)):
                tmp = np.array(point)
                tmp[i] += eps/2.0
                ret[i] = func(tmp)
                tmp[i] -= eps
                ret[i] -= func(tmp)
                ret[i] /= eps
        return ret/np.linalg.norm(ret)

def sign(number):
        if number<0.0000000001:
                return -1
        elif number>0.0000000001:
                return 1
        else:
                return 0


def metrika(main_func,one_func,transform_func,point,h,eps):
        dimn = len(point)
        x0 = np.array(point)
        g0 = grad(main_func,point,eps*0.1)
        d = -1 * g0
        H = np.eye(dimn)
        while True:
                z=np.zeros(dimn)
                tmp_func = lambda x: main_func(x0+x*d)
                z = one_func(tmp_func,h,eps)
                if abs(z)<eps:
                        break
                x1 = x0 + z*d
                v = z * d
                x0 = x1
                g1 = grad(main_func,x1,eps*0.1)
                u = g1-g0
                g0 = g1
                H = H + transform_func(v,u,H)
                d = np.array((-H * np.matrix(g1).transpose()).transpose())[0]
                d = d/np.linalg.norm(d)
                if np.linalg.norm(v)+np.linalg.norm(g1)<eps:
                        break
        return x1

def transform_green(v,u,H):
        H = np.matrix(H)
        v = np.matrix(v).transpose()
        u = np.matrix(u).transpose()
        v_vt = v * v.transpose()
        vt_u = v.transpose() * u
        A = v_vt/vt_u
        H_u = H*u
        ut_H = u.transpose()*H
        ut_H_u = ut_H*u
        H_u_ut_H = H_u*ut_H
        A -= H_u_ut_H/ut_H_u
        return A

def one_mp2(func,h,eps):
        x1,x2,x3 = -h,0,h
        y1,y2,y3 = func(x1),func(x2),func(x3)
        while True:
                z1 = x1 - x3
                z2 = x2 - x3
                p = ((y1-y3)*z2-(y2-y3)*z1)/(z1*z2*(z1-z2))
                q = ((y1-y3)*z2*z2-(y2-y3)*z1*z1)/(z1*z2*(z2-z1))
                zm = -q/(2*p)
                x1,x2 = x2,x3
                y1,y2 = y2,y3
                x3 = x3 + zm
                y3 = func(x3)
                if abs(zm) < eps:
                        break
        return x3+zm

def shtraf(g,point):
        a=0.5
        temp_point=point
        while 1:
                temp_x=metrika(g,one_mp2,transform_green,temp_point,0.1,0.01)
                if abs(temp_point-temp_x)<=[0.0001,0.0001]:
                        a=f(temp_x)
                        print(temp_x,a)
                        break

                if f(temp_x)<f(temp_point):
                        temp_point=temp_x
                else:
                        a=f(temp_x)
                        print(temp_x,a)
                        break

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


def g(x):
        return (x[1]-1)**2+x[2]+0.5*(((x[1]+x[2]**2)/5)**2)*(1+sign(x[1])*(1+sign(x[2]))

shtraf(g,[3,3])