import numpy as np
from sympy import *


def graphic(fd,Fd):
    a=0
    bujiru=0
    for i in range(len(Fd)):
        for j in range(i+1,len(Fd),1):
            if len(solve([Fd[i],Fd[j]],[d1,d2]))==0:
                continue
            else:
                d_new = np.array([[float(solve([Fd[i], Fd[j]], [d1, d2])[d1]), float(solve([Fd[i], Fd[j]], [d1, d2])[d2])]])
                for Fdi in Fd:
                    if Fdi.subs([(d1,d_new[0][0]),(d2,d_new[0][1])])<0:
                        bujiru=1
                if bujiru==0:
                    if a == 0:
                        d = d_new
                    else:
                        d = np.concatenate((d, d_new), axis=0)
                    a = a + 1
                bujiru=0
    y=[[]for i in range(len(d))]
    for m in range(len(d)):
        y[m]=fd.subs([(d1,d[m][0]),(d2,d[m][1])])
    d_min=d[np.argmin(np.array(y))]
    return d_min

def search(di,step_max):
    new_x1 = init[0] + t * di[0]
    new_x2 = init[1] + t * di[1]
    f_new = f.subs([(x1, new_x1), (x2, new_x2)])  # 新函数
    grad_new = diff(f_new, t)  # 对t偏导
    t_val = nsolve(grad_new, t, 0)
    if t_val>step_max:
        print(f'使用一维搜索计算λ={t_val}>λmax\n'
              f'最终取λ={step_max}')
        return step_max
    else:
        print(f'使用一维搜索计算λ={t_val}<λmax\n'
              f'最终取λ={t_val}')
        return t_val


def main():
    global a,init,d
    g1=diff(f,x1)
    g2=diff(f,x2)
    a=np.array(a)
    x_num=np.shape(a)[1]-1
    init=np.array(init)
    print(f'目标函数的梯度∇f(x)=[{g1}\n'
          f'                     {g2}]')
    num=0
    while 1:
        num=num+1
        print(f"___________________第{num}次迭代__________________")
        g1_new=g1.subs(x1,init[0])
        g2_new=g2.subs(x2,init[1])
        print(f'1.在点x^({num})={init},∇f(x)=[{g1_new},{g2_new}]')
        A1_num=0
        A2_num=0
        for i in range(len(a)):
            if sum(a[i][:-1]*init)+a[i][-1]==0:
                A1_num=A1_num+1
            else:
                A2_num=A2_num+1
        A1=np.ones([A1_num,x_num])
        A2=np.ones([A2_num,x_num])
        b1=np.ones(A1_num)
        b2=np.ones(A2_num)
        A1_num=0
        A2_num=0
        for i in range(len(a)):
            if sum(a[i][:-1]*init)+a[i][-1]==0:
                A1[A1_num]=a[i][:-1]
                b1[A1_num]=a[i][-1]
                A1_num=A1_num+1
            else:
                A2[A2_num]=a[i][:-1]
                b2[A2_num]=a[i][-1]
                A2_num=A2_num+1
        b1=-b1
        b2=-b2
        print(f'起作用约束(紧约束)系数矩阵,约束右端\nb1={b1}\nA1={A1}\n'
              f'不起作用约束(紧约束)系数矩阵，约束右端\nb2={b2}\nA2={A2}')
        Fd=[[]for i in range(A1_num+4)]
        fdi=0*d1
        for n in range(A1_num):
            for m in range(x_num):
                fdi=fdi+A1[n][m]*d[m]
            Fd[n]=fdi
            fdi=0*d1
        Fd[n+1]=d1+1
        Fd[n+2]=-d1+1
        Fd[n+3]=d2+1
        Fd[n+4]=-d2+1
        fd=g1_new*d[0]+g2_new*d[1]
        di=graphic(fd,Fd)
        print(f'2.求在点x^({num})=init处下降可行方向d\n'
              f'即解min∇f(x^{num})^Td={fd}\n'
              f's.t.  A1*d>=0\n'
              f'      |dj|<=1(j=1,2))\n'
              f'通过图解法求得d^{num}={di}')
        dd=np.dot(np.array(A2),np.array(di).T)
        bb=np.array(b2)-np.dot(np.array(A2),np.array(init).T)
        for i in range(len(dd)):
            if (dd[i]<0) & ((dd[i]/bb[i])<=(dd[0]/bb[0])):
                step_max = dd[i]/bb[i]
        print(f'3.求步长：\n'
              f'∵d=A2*d{num}={dd},b=b2-A2x{num}={bb}\n'
              f'∴λmax=min(d/b|d<0)={step_max}')
        t = search(di,step_max)
        init_new = init+t*di
        print(f'目标点位{init_new}')
        if (init_new==init).all():
            print(f'\n_________________________结果______________________\n'
                  f'最终点位{init_new},最终函数值{f.subs([(x1,init_new[0]),(x2,init_new[1])])}')
            break
        else:
            init=init_new





if __name__ == '__main__':
    x1, x2 ,d1,d2,t= symbols('x1,x2,d1,d2,t')
    f=x1**2+x2**2-2*x1-4*x2+6
    d=[d1,d2]
    a=[[-2,1,1],[-1,-1,2],[1,0,0],[0,1,0]]
    init=[0,0]
    main()