% *********************************************************************
%              陳為棟－基本BFGS算法求解
% *********************************************************************
% 該程序找全局最小值
% minf=100*(x1^2-x2)^2+(1-x1)^2,在點（1，1）處有最小值0
% 取值范圍：-2.048<=x1,x2<=2.048
% syms x1 x2     %求梯度
% f=100*(x1^2-x2)^2+(1-x1)^2;
% fx=diff(f,x1);
% fy=diff(f,x2);
% fi=[fx,fy];
% fx
% fy

clc
%求梯度
syms x1 x2 t   
f=21.5+x1*sin(4.0*pi*x1)+x2*sin(20.0*pi*x2); %適應值函數，可以變化
fx=diff(f,x1);  %對x求梯度
fy=diff(f,x2);  %對y求梯度
%初始化賦值
dim=2;    %維數
e=0.1;    % H的終止準則限


%算法第一步
x0=[0;5]; %給初值
fz0=subs(f,{x1,x2},{x0(1,1),x0(2,1)});%計算函數值
g0(1,1)=subs(fx,{x1,x2},{x0(1,1),x0(2,1)});%計算梯度值
g0(2,1)=subs(fy,{x1,x2},{x0(1,1),x0(2,1)}); %計算梯度值

%算法第二步
k=0;  %迭代次數
H0=[1,0;0,1];  %迭代矩陣H初值為單位矩陣
p0=-g0;
       
%算法第三步,一維搜索
m=1; %標志量
while (m)
    x=x0+t*p0;
    ft=subs(f,{x1,x2},{x(1,1),x(2,1)});
    
    %調用進退法算法，確定范圍
    d0=0;%初始點
    h=1;%初始步長
    s=2; %加倍系數
    y0=subs(ft,t,d0);
    n=0;   
    p=1;%標志量    
    while (p)
        d1=d0+h;
        y1=subs(ft,t,d1);
        if y1<y0
            h=s*h;
            d=d0;
            d0=d1;
            y0=y1;
            n=n+1;
        elseif n==0;
                 h=-h;
                 d=d1;
        else
                p=0;
                break;
        end 
    end    
    a=min(d,d1);%得到搜索區間
    b=max(d,d1);
    %0.618算法確定t的值
    r=0.618;
    h1=a+(1-r)*(b-a);%兩個邊界點
    u1=a+r*(b-a);
    y1=subs(ft,t,h1);%邊界點的值
    y2=subs(ft,t,u1);
    while (abs(h1-u1)>0.01)
        if y1<y2
            a=u1;
            u1=h1;
            y2=y1;
            h1=a+(1-r)*(b-a);
            y1=subs(ft,t,h1);
        else
            a=h1;
            h1=u1;
            y1=y2;
            u1=a+r*(b-a);
            y2=subs(ft,t,u1);
        end
    end
    t=(h1+u1)/2; %得到最優值
    %一維搜索后使新的迭代點不超過x的取值范圍,限幅處理
    x=x0+t*p0;   
    if x(1,1)>12.1  
        x(1,1)=12.1;
    end
    if x(1,1)<-3.0
        x(1,1)=-3.0;
    end
    if x(2,1)>5.8
        x(2,1)=5.8;
    end
    if x(2,1)<4.1
        x(2,1)=4.1;
    end
    %計算f(k+1)和g(k+1)的值
    fz=subs(f,{x1,x2},{x(1,1),x(2,1)});%計算新的值
    g(1,1)=subs(fx,{x1,x2},{x(1,1),x(2,1)});% 計算新的梯度
    g(2,1)=subs(fy,{x1,x2},{x(1,1),x(2,1)});%計算新的梯度
    %算法第四步
    while(norm(g(:,1),2)>=e) %判斷H是否滿足終止條件,計算2－范數
        if fz>=fz0   %不滿足，進入算法第五步
              x0=x0;    %迭代后函數值沒有下降，重新迭代
              f0=f0;
              g0=g0;
              H0=[1,0;0,1]; %迭代矩陣H初值為單位矩陣
              p0=-g0;
              m=1;
              k=0;
              continue; %轉算法第三步
        elseif k==dim  %進入算法第六步
                  x0=x;
                  fz0=fz;
                  g0=g;
                  H0=[1,0;0,1]; %迭代矩陣H初值為單位矩陣
                  p0=-g0;
                  m=1;
                  k=0;
                  continue; %轉算法第三步
        else                %進入算法第七步
              y=g-g0;
              s=x-x0;
              H=H0+[(1+y'*H0*y/s'*y)*s*s'-H0*y*s'-s*y'*H0]/s'*y;
              p=-H*g;
              H0=H;
              p0=p;
              fz0=fz;
              x0=x;
              k=k+1;
              continue; %轉算法第三步
        end
    end
    m=0;
end
xpso=x %輸出x,f的值，BFGS算法結束,最優值帶回PSO程序
fpso=fz