/*******************矩陣的擬上三角化************************/
#include "stdio.h"
#include "math.h"
#define n 4
void print(char arr[2],double A[n][n])
{
	printf("%s is:\n",arr);
	for(int i=0;i<n;i++)
		for(int j=0;j<n;j++)
		{	
			
			printf("%12.7f,",A[i][j]);
			if(j==n-1) printf("\n");
		}
}
//打印函數
void print1(char ar[2],double *arr)
{
	printf("%s is:",ar);
	for(int j=0;j<n;j++)				
	{	
		if(j==0) printf("\t[");
		printf("%14.9f",*(arr+j));
		if(j!=n-1) printf(",");
		else printf("\t]");
	}
	printf("\n");
}
void chenfa(double arr1[n][n],double arr2[n][n])
{
	double temp;
	double re[n][n];
	for(int i=0;i<n;i++)
		for(int j=0;j<n;j++)
		{
			temp=0;
			for(int k=0;k<n;k++)
				temp=temp+arr1[i][k]*arr2[k][j];
			re[i][j]=temp;
		}
	print("T",re);
}
int sign(double x)
{
	if(x>0) return 1;
	else return -1;
}
double neiji(double arr1[n],double arr2[n])
{
	double value=0;
	for(int i=0;i<n;i++)
		value=value+arr1[i]*arr2[i];
	return value;
}
void Sanjiaohua(double A[n][n])
{
	int r,i,j;
	double dr,cr,hr,tr;
	double I[n][n],H[n][n];
	double u[n],w[n],p[n],q[n];
	int pd;
	double temp;
	for(i=0;i<n;i++)//Q is I
		for(j=0;j<n;j++)
		{
			if(i==j) I[i][j]=1;
			else I[i][j]=0;
		}
	for(r=1;r<=n-2;r++)
	{
		for(i=r+2;i<=n;i++)
		{
			if(A[i-1][r-1]==0) 
			{
				if(i==n) pd=1;
				else continue;
			}
			else {pd=0;break;}
		}
		if(pd==1) continue;
		else
		{
			for(temp=0,i=r+1;i<=n;i++)
				temp=temp+A[i-1][r-1]*A[i-1][r-1];
			dr=sqrt(temp);
			cr=-sign(A[r][r-1])*dr;
			hr=cr*cr-cr*A[r][r-1];
			for(i=1;i<=n;i++)//ur
			{
				if(i<r+1) u[i-1]=0;
				else if(i==r+1) u[i-1]=A[r][r-1]-cr;
				else u[i-1]=A[i-1][r-1];
			}
			for(i=0;i<n;i++)//pr
			{
				temp=0;
				for(j=0;j<n;j++)	
					temp=temp+A[j][i]*u[j];
				p[i]=temp/hr;
			}
			for(i=0;i<n;i++)//qr
			{
				temp=0;
				for(j=0;j<n;j++)	
					temp=temp+A[i][j]*u[j];
				q[i]=temp/hr;
			}
			tr=neiji(p,u)/hr;//tr
			for(i=0;i<n;i++)//wr
			{
				w[i]=q[i]-tr*u[i];			
			}
			for(i=0;i<n;i++)//A(r+1)
			{
				for(j=0;j<n;j++)	
					A[i][j]=A[i][j]-w[i]*u[j]-u[i]*p[j];
			}
			//printf("dr is %10.5f,cr is %10.5f,hr is %10.5f\n",dr,cr,hr);
			//print1("u",u);
			//print1("w",w);	
			//print1("p",p);
			//print("A",A);
			for(i=0;i<n;i++)
				for(j=0;j<n;j++)
				{
					H[i][j]=I[i][j]-2*u[i]*u[j]/(neiji(u,u));
				}
			print("H",H);
		}//else
	}//for(r)
	print("A",A);
}
int main()
{
	double A[n][n]={1,2,1,2,2,1,2,-1,1,2,0,3,2,-1,3,1};
	//double A[n][n]={2,1,3,4,1,-1,2,1,-1,2,1,2,1,0,-1,3};
	print("A",A);
	Sanjiaohua(A);
	return 0;
}