//外點懲罰函數法 
//問題: minf(x)=x1*x4*(x1+x2+x3)+x3
//      s.t.  g1(x)=x1^2+x2^2+x3^2+x4^2-40=0
//            g2(x)=25-x1*x2*x3*x4
//            1<=xj<=5   j=1,2,3,4

//n-設計變量的維數；
//nc-約束條件的個數；
//ne-等式約束條件的個數；
//tt-一維搜索的初始步長；
//ff-差分法求梯度時的步長；
//ac-外點懲罰函數法和變尺度法的收斂精度；
//ad-一維搜索時的收斂精度；
//bw-懲罰因子遞增系數；
//r-初始懲罰因子；
//xk[n]-設計變量的初始點。
  
//#include"stdio.h"
#include "iostream.h"
#include"stdlib.h"
#include"math.h"
#define n 2
#define nc 5
#define tt 0.005
#define ff 0.00001
#define ac 0.000001
#define ad 0.0000001
#define bw 0.5
double r,ia;
double fun(double *x,double *c);
double *iterate(double *x,double a,double *s);
double construct(double *x);
double penalty(double *x,double a,double *s);
void finding(double a[3],double f[3],double *xk,double *s);
double lagrange(double *xk,double *ft,double *s);
double *gradient(double *xk);
double *bfgs(double *xk);
void main()
{//int j;
   int sum=0;
   double c[nc];
   double *xk1,f1,f2,q;
   double xk[n]={5,5};      
  xk1=(double *)malloc(n*sizeof(double)); 
  r=37.5;
//cout<<"請輸入初始點:";
//for(int i=0;i<n;i++)
   //cin>>xk[i];        // 輸入初始點
//for( i=0;i<n;i++)
    //cout<<xk[i]<<"\t";
   f1=fun(xk,c);
for(int k=0;;k++)
 {
    ia=0;
    xk1=bfgs(xk);
	//for(j=0;j<n;j++)
	//printf("\nxk1[%d]=%f",j,xk1[j]);
	f2=fun(xk1,c);
    //printf("\nf2=%f\n",f2);   
    double xx=0; 
  if(nc!=0)
  { q=fabs(c[0]);
      for(int i=0;i<nc;i++)
           if(fabs(c[i])<q)
                q=fabs(c[i]); 
  }
    if(q<=ac) break;
    for(int i=0;i<n;i++)
             xx+=pow((xk1[i]-xk[i]),2);
    xx=sqrt(xx); 
    if((fabs(xx)<=ac)&&(fabs(f2-f1)<=ac)) break;
  r=bw*r;
    for(i=0;i<n;i++)
            xk[i]=xk1[i];
            f1=f2;
	sum++;
 }
   if(f2>f1)
   {  f2=f1;  xk1=xk;}
   cout<<"\n\t循環次數:sum="<<sum<<"\n";
   cout<<"\n\t懲罰因子:"<<"r="<<r<<"\n";
   for(k=0;k<n;k++)
        cout<<"\n\t"<<"x*["<<k<<"]="<<xk1[k];
  //free(xk1);
   cout<<"\n\t"<<"f="<<f2<<"\n";
   for(k=0;k<nc;k++)
      cout<<"\n\t"<<"c["<<k<<"]="<<c[k];
   cout<<"\n";
}

double fun(double *x,double *c)  
{ 
   double f;
   f=60-10*x[0]-4*x[1]+pow(x[0],2)+pow(x[1],2)-x[0]*x[1];

   c[0]=-x[0];
   c[1]=-x[1];
   c[2]=x[0]-6;
   c[3]=x[1]-8;
   c[4]=x[0]+x[1]-11;
   /*f=pow((x[1]),3)*((x[0]-3)*(x[0]-3)-9)/27/sqrt(3);
 c[0]=x[1]-x[0]/sqrt(3);
 c[1]=-x[0]+x[1]/sqrt(3);
 c[2]=x[0]+x[1]/sqrt(3)-6;
 c[3]=-x[0];
 c[4]=-x[1];*/
   return(f);
 }
double *iterate(double *x,double a,double *s)
 {
      double *x1;
      x1=(double *)malloc(n*sizeof(double));
      for(int i=0;i<n;i++)
           x1[i]=x[i]+a*s[i];
      return(x1);
 }

double construct(double *x)
   {
     double y,c[nc];
     y=fun(x,c);
     if(nc==0)   return(y);
     for(int i=0;i<nc;i++)
		 y-=r/c[i];
     return(y);
     }
 
 double penalty(double *x,double a,double *s) 
  {
         double y;
         double *x1;
         x1=iterate(x,a,s);
         y=construct(x1);
         return(y);
  }
 void finding(double a[3],double f[3],double *xk,double *s)
  {
     double t=tt;
     double a1,f1;
     a[0]=0;
     f[0]=penalty(xk,a[0],s);
     for(int i=0;;i++)
       {
         a[1]=a[0]+t;     f[1]=penalty(xk,a[1],s);
         if(f[1]<f[0]) break;
         if(fabs(f[1]-f[0])>=ad)
              {   t=-t;  a[0]=a[1];  f[0]=f[1];}
         else {   if(ia==1) return;
                  t=t/2;    ia=1;     
		      }
 }
 for(i=0;;i++)
 {     a[2]=a[1]+t;    f[2]=penalty(xk,a[2],s);
       if(f[2]>f[1])  break;
       t=2*t;
       a[0]=a[1];    f[0]=f[1];
       a[1]=a[2];    f[1]=f[2];
 }
 if(a[0]>a[2])
 {   a1=a[0];    f1=f[0];
     a[0]=a[2];    f[0]=f[2];
     a[2]=a1;      f[2]=f1;  
   } //調試中問題所在 大括弧
 return;
 }
 
double lagrange(double *xk,double *ft,double *s)
   {
     double a[3],f[3];
     double b,c,d,aa;
     finding(a,f,xk,s);
     for(int i=0;;i++)
      {
       if(ia==1) { aa=a[1];  *ft=f[1];  break;  }
     d=(pow(a[0],2)-pow(a[2],2))*(a[0]-a[1])-(pow(a[0],2)-pow(a[1],2))*(a[0]-a[2]);
     if(fabs(d)==0)   break;
     c=((f[0]-f[2])*(a[0]-a[1])-(f[0]-f[1])*(a[0]-a[2]))/d;
     if(fabs(c)==0)   break; 
     b=((f[0]-f[1])-c*(pow(a[0],2)-pow(a[1],2)))/(a[0]-a[1]);
     aa=-b/(2*c);
     *ft=penalty(xk,aa,s);
     if(fabs(aa-a[1])<=ad)
        { if(*ft>f[1])  aa=a[1];
          break;   
	    }
     if(aa>a[1])
        {
           if(*ft>f[1])    {  a[2]=aa;  f[2]=*ft;  }
           else if(*ft<f[1])   
                 {  a[0]=a[1];a[1]=aa;   
		            f[0]=f[1];   f[1]=*ft;  
		         }
           else if(*ft=f[1])
                 {  a[2]=aa;   a[0]=a[1]; 
                    f[2]=*ft;  f[0]=f[1];       
                    a[1]=(a[0]+a[2])/2;  
					f[1]=penalty(xk,a[1],s);  
		         }
           }
      else
         {  if(*ft>f[1])     {    a[0]=aa;    f[0]=*ft;   }
            else if(*ft<f[1])
                  {    a[2]=a[1];     a[1]=aa;
                       f[2]=f[1];     f[1]=*ft;
                      }
           else if(*ft=f[1])
                {     a[0]=aa;    a[2]=a[1];
                      f[0]=*ft;    f[2]=f[1];
                      a[1]=(a[0]+a[2])/2;    f[1]=penalty(xk,a[1],s);
                        }
                }
            }
     if(*ft>f[1])
         {   aa=a[1];     *ft=f[1];     }
     return(aa);
   }
 double *gradient(double *xk)
 {
    double *g,f1,f2,q;
    g=(double *)malloc(n*sizeof(double));
    f1=construct(xk);
    for(int i=0;i<n;i++)
       {  q=ff;
          xk[i]=xk[i]+q;      f2=construct(xk);
          g[i]=(f2-f1)/q;  xk[i]=xk[i]-q;
       }
     return(g);
 }
double *bfgs(double *xk)
  {
    double u[n],v[n],h[n][n],dx[n],dg[n],s[n];
    double aa,ib=0;
    double *ft,*xk1,*g1,*g2,*xx,*x0=xk;
    ft=(double *)malloc(sizeof(double));
    xk1=(double *)malloc(n*sizeof(double)); 
    int i,j,k;
    
    for(i=0;i<n;i++)
      { s[i]=0;
        for(j=0;j<n;j++)
       {  h[i][j]=0;
          if(j==i)  
          h[i][j]=1; 
		}
      }
    g1=gradient(xk);
   //for(j=0;j<n;j++)
	//printf("\ng1[%d]=%f\n",j,g1[j]);
    double fi=construct(xk);
    //printf("fi=%f",fi);
    x0=xk;
  for(k=0;k<n;k++)
    { int ib;
      if(ia==1 )  {  xx=xk; break; }
      ib=0; 
      for(i=0;i<n;i++)
        s[i]=0;
      for(i=0;i<n;i++)
        for(j=0;j<n;j++)
         s[i]+=-h[i][j]*g1[j];
     aa=lagrange(xk,ft,s);
    //printf("aa=%f ",aa);
     xk1=iterate(xk,aa,s);
     g2=gradient(xk1);
     for(i=0;i<n;i++)
       if((fabs(g2[i])>=ac)&&(fabs(g2[i]-g1[i])>=ac)||(fabs(*ft-fi)>=ac))
           ib=ib+1;
       if(ib==0)
         {  xx=xk1;  break;   }
       fi=*ft;
       if(k==(n-1))
        { xk=xk1;
        for(i=0;i<n;i++)
        for(int j=0;j<n;j++)
         {  h[i][j]=0;
           if(j==i) h[i][j]=1;
       }
        g1=g2;
        k=-1;
      }
       else  
     {
       for(i=0;i<n;i++)
	   { dg[i]=g2[i]-g1[i];
         dx[i]=xk1[i]-xk[i];    }
         for(i=0;i<n;i++)
        {
       u[i]=0;    v[i]=0;
       for(int j=0;j<n;j++)
      {  u[i]=u[i]+dg[j]*h[i][j];  
         v[i]=v[i]+dg[j]*h[i][j]; 
      }
      }
   double a1=0,a2=0;
   for(int j=0;j<n;j++)
    { a1+=dx[j]*dg[j];    
      a2+=v[j]*dg[j];    
    }
   if(fabs(a1)!=0)
    {a2=1+a2/a1;
   for(i=0;i<n;i++)
   for(j=0;j<n;j++)
    h[i][j]+=(a2*dx[i]*dx[j]-v[i]*dx[j]-dx[i]*u[j])/a1;
   }
   xk=xk1;g1=g2;
	   }
 }
 if(*ft>fi)
  {   *ft=fi;   xx=xk;    }
      xk=x0;
	  return(xx);
   }