#include<time.h> 
#include<stdlib.h> 
#include<stdio.h> 
#include<iomanip.h> 
#include<string.h>
#include<math.h> 
#include<iostream.h> 
#include<conio.h>
int set[100];                        //用來表示有效集合中是否為空,其值為有效約束的個數 
#define null 0 

/************************************矩陣的四則運算**********************************************/
double *multiply(double *q0,double *q1,int n,int m0,int m1) //m0 為矩陣 q0 的行數,m1為矩陣q1的列數 //n 為的q0的列數q1行數. //該函數返回一個指向新開辟空間的整型指針 
{ 
	int i,k,t,j; 
    double *q,total; 
    q=new double[m0*m1]; 
    total=0.0; 
    j=0; 
    for(i=0;i<m0;i++) 
		for(t=0;t<m1;t++)
		{ 
			for(k=0;k<n;k++) 
            total+=q0[i*n+k]*q1[k*m1+t]; 
            q[j]=total; 
            j++; 
            total=0.0; 
		} 
        return q;
} 

double *Div(double *matrix,double num,int total)    //matrix為被除矩陣,num為除數,total為矩陣元素數
{ 
       double *q; 
       q=new double[total]; 
       for(int t=0;t<total;t++) 
          q[t]=matrix[t]/num; 
       return q;
} 


double *sub(double *q1,double *q2,int n)
{ 
      double *q; 
      q=new double[n]; 
      for(int i=0;i<n;i++)
	  { 
		  q[i]=q1[i]-q2[i];
	  } 
      return q;
} 


double *add(double *q1,double *q2,int total)
{ 
       double *q; 
       q=new double[total]; 
       for(int i=0;i<total;i++) 
       q[i]=q1[i]+q2[i]; 
       return q;
} 


double *NumMul(double *Matrix,double Num,int n)
{  
	double *q; 
    q=new double[n]; 
    for(int t=0;t<n;t++) 
    q[t]=Num*Matrix[t]; 
    return q;
} 

/************************************矩陣的四則運算結束**********************************************/

double Find_ak(int *I,double *A,double *B,double *Xk,double *dk,int n,int m,int k)      //求ak的值,k為迭代次數
{ 
       int flag;                                     //標志是否加入到有效集中                      
       double *p,*At,ak,a0;                          //p臨時存放數據,At為對應的子矩陣,ak用于比較
	                                                 //a0存放(B-At*Xk)/At*dk的臨時結果
	   int i;
       ak=1.0; 
       cout<<"求步長時的有效約束集："; 
       for(i=0;i<m;i++) 
            cout<<I[k*m+i]<<" "; 
       cout<<endl; 
       for(int t=0;t<m;t++)
	   { 
		   if(I[k*m+t]==-1)
		   { 
              At=new double[n]; 
              for(i=0;i<n;i++) 
              At[i]=A[i*m+t]; 
              //for(i=0;i<n;i++) 
              //cout<<At[i]<<" at "; 
              //cout<<endl; 
              //for(i=0;i<n;i++) 
              //cout<<dk[i]<<" dk "; 
              //cout<<endl; 
              p=multiply(At,dk,n,1,1); 
              //cout<<p[0]<<endl; 
              a0=p[0];
              //cout<<a0<<endl; 
              if(a0>=-0.00000001&&a0<=0.00000001) 
              continue; 
              delete []p; 
              p=multiply(At,Xk,n,1,1); 
              delete []At; 
              a0=(B[t]-p[0])/a0; 
              delete []p; 
              if(ak>a0&&a0>0)
			  { 
			      ak=a0; 
                  flag=t;
			  }
		   }
	   } 
	   cout<<"求得步長為: ";
        cout<<ak<<endl; 
        if(ak<1) 
        {
			I[k*m+flag]=flag;
			set[k]++;                                   //有效集加一個
			for(i=0;i<m;i++)                                 //X0不變
			{
                I[m*(k+1)+i]=I[m*k+i];
			}
		}
		else
		{
            for(i=0;i<m;i++)                                 
			{
                I[m*(k+1)+i]=I[m*k+i];
			}
		}
        return ak;
} 

/**********************************求解方程組部分用jacobi迭代法**********************************/

double *root(short matrixNum,double *matrixA,double *b)         //jacobi迭代解線性方程組
{
   int   i,j,m,k;
   double   TempM=0,TempMm=0,TempN=0,TempNn=0;
   double *X;
   double   L[100][100];
   double   M[100][100];
   double   N[100][1];
   for(i=0;i<matrixNum;i++)
   {
        for(j=0;j<matrixNum;j++)
		{
             M[i][j]=matrixA[i*matrixNum+j];
		}
   }
   for(i=0;i<matrixNum;i++)
   {
	   N[i][0]=b[i];
   }
   /*   求矩陣M的L陣和D陣,L陣存放在L中,D陣存放在M陣的對角元上*/
  for(i=1;i<matrixNum;i++)
  {
	  for(j=0;j<i;j++)
	  {
		  for(m=0;m<j;m++)
		  {
			  TempM=M[i][m]*L[j][m]+TempMm;
              TempMm=TempM;
              TempM=0;
		  }
        M[i][j]=M[i][j]-TempMm;
        TempMm=0;
        L[i][j]=M[i][j]/M[j][j];
	  }
  for(k=0;k<i;k++)
  {
     TempN=M[i][k]*L[i][k]+TempNn;
     TempNn=TempN;
     TempN=0;
  }
     M[i][i]=M[i][i]-TempNn;
     TempNn=0;
  }
/*求解*/
    for(i=0;i<matrixNum;i++)
	{
		for(m=0;m<i;m++)
		{
			TempM=L[i][m]*N[m][0]+TempMm;
			TempMm=TempM;
			TempM=0;
		}
       N[i][0]=N[i][0]-TempMm;
       TempMm=0;
	}
	for(i=0;i<matrixNum;i++)
	{ 
		N[i][0]=N[i][0]/M[i][i];
	}
	for(i=matrixNum-1;i>=0;i--)
	{
		for(m=i+1;m<matrixNum;m++)
		{
			TempM=L[m][i]*N[m][0]+TempMm;
			TempMm=TempM;
			TempM=0;
		}
       N[i][0]=N[i][0]-TempMm;
       TempMm=0;
	}
/*     結果     */
    X=new double[matrixNum];
    for(i=0;i<matrixNum;i++)
	{
		X[i]=N[i][0];
	}  
     return X;     
}
/********************************求解方程組結束**************************************************/
/********************************下面是組合矩陣為定理求方程解做準備******************************/
double *ConstructMatrixL(double *G,double *Ak,int n,int m)   //組合矩陣左邊
{ 
    //n為G的維數,m為A的列數,p的維數為n+m         
	//{G  A
	// AT 0};
	double *p;
	int size=n+m;
	int i,j,k;
	k=0;
	p=new double[size*size];
	for(i=0;i<n;i++)                           //裝入G
		for(j=0;j<n;j++)
		{	
			k=i*size+j;
		    p[k]=G[i*n+j];
		}
    for(i=0;i<n;i++)                           //裝入Ak
		for(j=n;j<n+m;j++)
		{	
			k=i*size+j;
			if(k%(n+m)==1)
				k=k+n;
		    p[k]=Ak[i*m+j-n];
		}
	for(i=n;i<n+m;i++)                         //裝入Ak轉置
		for(j=0;j<n;j++)
		{	
			k=i*size+j;
		    p[k]=Ak[j*m+(i-n)];
		}	                                       
    for(i=n;i<n+m;i++)                           //其余元素賦值0
		for(j=n;j<n+m;j++)
		{	
			k=i*size+j;
			if(k%(n+m)==1)
				k=k+n;
		    p[k]=0;
		}                                           
	return p;
	delete []p;
}

double *ConstructMatrixR(double *r,double *Bk,int n,int m)   //組合矩陣右邊,n為r維數,m為Bk維數
{
    double *p;
	double *temp;
	p=new double[n+m];
	temp=new double[n];
	int i;
	for(i=0;i<n;i++)
		temp[i]=-r[i];
	for(i=0;i<n;i++)
		p[i]=temp[i];
	for(i=0;i<m;i++)
		p[i+n]=Bk[i];
    return p;
	delete []p;
	delete []temp;
}
/**********************************矩陣合并結束**************************************************/
/**********************************求逆矩陣部分**************************************************/
double calDeterminant(double *s,int n)                   //按行展開,求行列式值
{
	int z,j,k;
	double r,total=0; 
	double *b;
	b=new double[(n-1)*(n-1)];
	if(n==1)
	{
		total=s[0];
	}
    if(n>2) 
	{
		for(z=0;z<n;z++) 
		{
		for(j=0;j<n-1;j++) 
			for(k=0;k<n-1;k++) 
				if(k>=z) 
					b[j*(n-1)+k]=s[(j+1)*n+k+1]; 
				else 
					b[j*(n-1)+k]=s[(j+1)*n+k]; 
				if(z%2==0) 
					r=s[z]*calDeterminant(b,n-1);  //遞歸調用 
				else  
					r=(-1)*s[z]*calDeterminant(b,n-1); 
				total=total+r; 
		} 
} 
    else if(n==2) 
		total=s[0]*s[3]-s[1]*s[2]; 
     return total; 
	 delete []b;
}

double *algebra(double *s,int n)                  //algebra()函數用于求原矩陣各元素對應的余子式，存放在數組b中，定義為double型
 {
	int z,j,k,l,m,g;
	double *a;
	double *b;
	a=new double[(n-1)*(n-1)];
    b=new double[n*n];
    for(z=0;z<n;z++) 
    {
		l=z; 
        for(j=0;j<n;j++) 
		{ 
			m=j; 
			for (k=0;k<n-1;k++) 
				for(g=0;g<n-1;g++) 
				{ 
					if(g>=m&&k<l) 
						a[k*(n-1)+g]=s[k*n+g+1]; 
					else if(g<m&&k>=l)  
						a[k*(n-1)+g]=s[(k+1)*n+g]; 
					else if(g>=m&&k>=l) 
						a[k*(n-1)+g]=s[(k+1)*n+g+1]; 
					else 
						a[k*(n-1)+g]=s[k*n+g]; 
				} 
				b[z*n+j]=calDeterminant(a,n-1); 
		} 
	} 
	return b;
} 
double *tranverse(double *a,int n)
{
   double *b; 
   b=new double[n*n];
   int z,j; 
   double r;
   double temp; 
    r=calDeterminant(a,n);                            //調用calDeterminant()函數計算原矩陣的行列式值      
    if (r==0) 
		cout<<"Because |A|==0,the original matrix have no tranverse!";  //判斷條件:若|A|==0，則原矩陣無逆矩陣，反之則存在逆矩陣 
    else 
    {
		b=algebra(a,n);                                //調用algebra() 函數，得到原矩陣各元素對應的"余子式",存放在數組b中 
        for(z=0;z<n;z++)                               //求代數余子式，此時b中存放的為原矩陣各元素對應的"代數余子式" 
           for(j=0;j<n;j++) 
              if((z+j)%2!=0 && b[z*n+j]!=0) 
				  b[z*n+j]=-b[z*n+j]; 
        for(z=0;z<n;z++)                               //對b轉置,此時b中存放的為原矩陣的伴隨矩陣 
			for(j=z+2;j<n;j++) 
			{
				temp=b[z*n+j]; 
                b[z*n+j]=b[j*n+z]; 
                b[j*n+z]=temp; 
			}
      for(z=0;z<n;z++)                                 //求逆矩陣，此時b中存放的是原矩陣的逆矩陣 
          for(j=0;j<n;j++) 
            b[z*n+j]=b[z*n+j]/r; 
	} 
	 return b;
	 delete []b;
}
/************************************************************************************************/
/***************************************用有效集法求解QP問題*************************************/ 
double *QP(double *G,double *r,double *A,double *B,double *X0,int n,int m)   //n為G的維數,m為約束條件的個數
{ 
	
	int i,j,total,y,sign,flag;         //sign用于表示是否找到了最優解
	int k=0;                           //控制迭代次數　　
    double x,a,c,*gk,*matrixR,*matrixL,*ak,*bk,*p,*d,*nami,*a0,*a1,*q,*temp; //gk用于存放等價的r
	int *I;                        //I存放一指針數組k(k為迭代次數),其每一個元素都指向一個一維數組m,用于存放每個元素的有效集
	for(i=0;i<100;i++)             //開始令為空,下標靠k控制                
	{
         set[i]=0;
	}
	p=new double[n*n];
	//第一步：確定初始點，求其有效集