#include "iostream.h"
#include "math.h"

void comput_grad(double (*pf)(double *x),  int n,  double *point,  double *grad);                        //計算梯度
double line_search1(double (*pf)(double *x),  int n,  double *start,  double *direction);        //0.618法線搜索
double line_search(double (*pf)(double *x),   int n,  double *start,  double *direction);        //解析法線搜索
double DFP(double (*pf)(double *x),   int n,   double *min_point);                //無約束變尺度法



//梯度計算模塊
//參數：指向目標函數的指針，變量個數，求梯度的點，結果
void comput_grad(double (*pf)(double *x),
                                  int n,
                                  double *point,
                                  double *grad)
{
        double h=1E-3;
        int i;
        double *temp;
        temp = new double[n];
        for(i=1;i<=n;i++)
        {
                temp[i-1]=point[i-1];
        }
        for(i=1;i<=n;i++)
        {
                temp[i-1]+=0.5*h;
                grad[i-1]=4*pf(temp)/(3*h);
                temp[i-1]-=h;
                grad[i-1]-=4*pf(temp)/(3*h);
                temp[i-1]+=(3*h/2);
                grad[i-1]-=(pf(temp)/(6*h));
                temp[i-1]-=(2*h);
                grad[i-1]+=(pf(temp)/(6*h));
                temp[i-1]=point[i-1];
        }
        delete[] temp;
}

//一維搜索模塊
//參數：指向目標函數的指針，變量個數，出發點，搜索方向
//返回：最優步長
double line_search(
                                   double (*pf)(double *x),
                                   int n,
                                   double *start,
                                   double *direction)
{
        int i;
        double step=0.001;
        double a=0,value_a,diver_a;
        double b,value_b,diver_b;
        double t,value_t,diver_t;
        double s,z,w;
        double *grad,*temp_point;

        grad=new double[n];
        temp_point=new double[n];
        comput_grad(pf,n,start,grad);
        diver_a=0;
        for(i=1;i<=n;i++)
                diver_a=diver_a+grad[i-1]*direction[i-1];
        do
        {
                b=a+step;
                for(i=1;i<=n;i++)
                        temp_point[i-1]=start[i-1]+b*direction[i-1];
                comput_grad(pf,n,temp_point,grad);
                diver_b=0;
                for(i=1;i<=n;i++)
                        diver_b=diver_b+grad[i-1]*direction[i-1];
                if( fabs(diver_b)<1E-10 )
                {
                        delete[] grad;
                        delete[] temp_point;
                        return(b);
                }
                if( diver_b<-1E-15 )
                {
                        a=b;
                        diver_a=diver_b;
                        step=2*step;
                }
        }while( diver_b<=1E-15 );

        for(i=1;i<=n;i++)
                temp_point[i-1]=start[i-1]+a*direction[i-1];
        value_a=(*pf)(temp_point);
        for(i=1;i<=n;i++)
                temp_point[i-1]=start[i-1]+b*direction[i-1];
        value_b=(*pf)(temp_point);

        do
        {
                s=3*(value_b-value_a)/(b-a);
                z=s-diver_a-diver_b;
                w=sqrt( fabs(z*z-diver_a*diver_b) );        //////////////////!!!!!!!!!!!!!!!!!!!!!!
                t=a+(w-z-diver_a)*(b-a)/(diver_b-diver_a+2*w);
                value_b=(*pf)(temp_point);
                for(i=1;i<=n;i++)
                        temp_point[i-1]=start[i-1]+t*direction[i-1];
                value_t=(*pf)(temp_point);
                comput_grad(pf,n,temp_point,grad);
                diver_t=0;
                for(i=1;i<=n;i++)
                        diver_t=diver_t+grad[i-1]*direction[i-1];
                if(diver_t>1E-6)
                {
                        b=t;
                        value_b=value_t;
                        diver_b=diver_t;
                }
                else if(diver_t<-1E-6)
                {
                        a=t;
                        value_a=value_t;
                        diver_a=diver_t;
                }
                else break;
        }while( (fabs(diver_t)>=1E-6) && (fabs(b-a)>1E-6) );

        delete[] grad;
        delete[] temp_point;
        return(t);

}

//無約束變尺度法DFP函數聲明
//
//參數：pf指向目標函數的指針，n變量個數，min_point接受初始點、存放結果
//返回：極小點處函數值
//
double DFP(
                   double (*pf)(double *x),
                   int n,
                   double *min_point
                   )
{
        int i,j;
        int k=0;
        double e=1E-5;
        double g_norm;
                
        double *g0=new double[n];                //梯度
        double *g1=new double[n];
        double *dg=new double[n];

        double *p=new double[n];                //搜索方向 =-g
        double t;                                                //一維搜索步長

        double *x0=new double[n];
        double *x1=new double[n];
        double *dx=new double[n];

        double **H=new double*[n];
        for (i=0; i<n; i++)                H[i] = new double[n];

        double **tempH=new double*[n];
        for (i=0; i<n; i++)                tempH[i] = new double[n];

        double *gH=new double[n];
        double *Hg=new double[n];
        double num1;
        double num2;



        for(i=0;i<n;i++)
                for(j=0;j<n;j++)
                {
                        if(i==j)        H[i][j]=1.0;        // H0=I
                        else        H[i][j]=0.0;
                        tempH[i][j]=0.0;
                }


        for(i=0;i<n;i++)
                x0[i]=min_point[i];        

        comput_grad(pf,n,x0,g0);

        g_norm=0.0;
        for(i=0;i<n;i++)        g_norm=g_norm+g0[i]*g0[i];        
        g_norm=sqrt(g_norm);
        if (g_norm<e) 
        {
                for(i=0;i<n;i++)        min_point[i]=x0[i];

                delete[] g0;                
                delete[] g1;
                delete[] dg;
                delete[] p;
                delete[] x0;
                delete[] x1;
                delete[] dx;
                for (i=0; i<n; i++)                delete[] H[i];
                delete []H;
                for (i=0; i<n; i++)                delete[] tempH[i];
                delete []tempH;
                delete[] gH;
                delete[] Hg;

                return pf(min_point);
        }

        for(i=0;i<n;i++)        p[i]=-g0[i];        

        do
        {
                t=line_search(pf,n,x0,p);                                
                for(i=0;i<n;i++)        x1[i]=x0[i]+t*p[i];
                comput_grad(pf,n,x1,g1);
                g_norm=0.0;
                for(i=0;i<n;i++)        g_norm=g_norm+g1[i]*g1[i];
                g_norm=sqrt(g_norm);
                //cout<<k<<"    "<<x0[0]<<"       "<<x0[1]<<"       "<<g_norm<<"\n";
                if (g_norm<e) 
                {
                        for(i=0;i<n;i++)        min_point[i]=x1[i];

                        delete[] g0;                
                        delete[] g1;
                        delete[] dg;
                        delete[] p;
                        delete[] x0;
                        delete[] x1;
                        delete[] dx;
                        for (i=0; i<n; i++)                delete[] H[i];
                        delete []H;
                        for (i=0; i<n; i++)                delete[] tempH[i];
                        delete []tempH;
                        delete[] gH;
                        delete[] Hg;
                        
                        return pf(min_point);
                }
                for(i=0;i<n;i++)
                {
                        dx[i]=x1[i]-x0[i];
                        dg[i]=g1[i]-g0[i];
                }

                //////////////////求Hk+1的矩陣運算

                //g*H,H*g
                for(i=0;i<n;i++)
                {
                        gH[i]=0.0;
                        Hg[i]=0.0;
                }
                for(i=0;i<n;i++)
                {
                        for(j=0;j<n;j++)
                        {
                                gH[i]=gH[i]+dg[j]*H[j][i];
                                //Hg[i]=Hg[i]+H[i][j]*dg[j];
                                Hg[i]=gH[i];
                        }                        
                }

                //num1,num2
                num1=0.0;
                num2=0.0;
                for(i=0;i<n;i++)
                {
                        num1=num1+dx[i]*dg[i];
                        num2=num2+gH[i]*dg[i];
                }

                //tempH[i][j]
                for(i=0;i<n;i++)
                        for(j=0;j<n;j++)
                                tempH[i][j]=0.0;
                for(i=0;i<n;i++)
                {
                        for(j=0;j<n;j++)
                        {
                                tempH[i][j]=tempH[i][j]+H[i][j];
                                tempH[i][j]=tempH[i][j]+dx[i]*dx[j]/num1;
                                tempH[i][j]=tempH[i][j]-Hg[i]*gH[j]/num2;
                        }
                }

                for(i=0;i<n;i++)
                {
                        for(j=0;j<n;j++)
                        {
                                H[i][j]=tempH[i][j];
                        }
                }
                /////////////////////////////

                //P 
                for(i=0;i<n;i++)        p[i]=0.0;
                for(i=0;i<n;i++)
                {
                        for(j=0;j<n;j++)
                        {
                                p[i]=p[i]-H[i][j]*g1[j];
                        }                        
                }

                for(i=0;i<n;i++)
                {
                        g0[i]=g1[i];
                        x0[i]=x1[i];
                }
                k=k+1;
        }while(g_norm>e);

        for(i=0;i<n;i++)        min_point[i]=x1[i];

        delete[] g0;                
        delete[] g1;
        delete[] dg;
        delete[] p;
        delete[] x0;
        delete[] x1;
        delete[] dx;
        for (i=0; i<n; i++)                delete[] H[i];
        delete []H;
        for (i=0; i<n; i++)                delete[] tempH[i];
        delete []tempH;
        delete[] gH;
        delete[] Hg;

        return pf(min_point);
}

/////////////
double fun(double *x)
{
        return 100*( x[1]-x[0]*x[0] )*( x[1]-x[0]*x[0] ) + (1-x[0])*(1-x[0]);
}

void main()
{
        int n=2;
        double min_point[2]={-5,10};
        double min_value=DFP(fun,n,min_point);
        cout<<min_point[0]<<" and "<<min_point[1]<<"   "<<min_value;

}

//0.618法線搜索
//
//參數：指向目標函數的指針，變量個數，出發點，搜索方向
//返回：最優步長
//
double line_search1(
                                   double (*pf)(double *x),
                                   int n,
                                   double *start,
                                   double *direction)
{
        int i;
        int k;

        double l,a,b,c,u,lamda,t,e;

        double *xa=new double[n];
        double *xb=new double[n];
        double *xc=new double[n];
        double *xl=new double[n];
        double *xu=new double[n];

        
        //確定初始搜索區間
        l=0.001;
        a=0;

        k=0;
        do
        {
                k++;
                c=a+l;
                for(i=0;i<n;i++)
                {
                        xc[i]=start[i]+c*direction[i];
                        xa[i]=start[i]+a*direction[i];
                }
                l=l/3;
        }while( pf(xc) >= pf(xa) );                        // ???

        k=0;
        do
        {
                k++;
                l=2*l;
                b=c+l;
                for(i=0;i<n;i++)
                {
                        xc[i]=start[i]+c*direction[i];
                        xb[i]=start[i]+b*direction[i];
                }
                a=c;
                c=b;
        }while( pf(xb) <= pf(xc) );


        a=0;
        b=0.1;

        //尋優
        t=0.618;
        e=0.000001;

        lamda=b-t*(b-a);
        u=a+t*(b-a);
        
        for(i=0;i<n;i++)
        {
                xl[i]=start[i]+lamda*direction[i];
                xu[i]=start[i]+u*direction[i];
        }

        k=0;
        do
        {
                k++;
                if( pf(xl)<pf(xu) )
                {
                        b=u;
                        u=lamda;
                        lamda=b-t*(b-a);
                }
                else
                {
                        a=lamda;
                        lamda=u;
                        u=t*(b-a);
                }
        }while( b-a>=e );

        lamda=(a+b)/2;

        delete[] xa;
        delete[] xb;
        delete[] xc;
        delete[] xl;
        delete[] xu;
        
        return lamda ;

}