/* lin_eq_Gauss.c
ガウスの消去法
*/

#include"sqmatrix.h"
// sqmatrix.h を同じディレクトリに download せよ

#define trace 0
// 途中経過をトレースするときは 1 に、しないときは 0 に

// 前進消去
void step1(MATRIX *A, VECTOR *b)
{
	int i,j,k,p;
	double m,max,tmp,c;
	char r;

	for(k = 0; k < SIZE - 1; k++){
		// 掃き出し
		for(i = k+1; i < SIZE; i++){
			c = A->ent[i][k] / A->ent[k][k];
			for(j = k; j < SIZE; j++){
				A->ent[i][j] -= c * A->ent[k][j];
			}
			b->ent[i] -= c * b->ent[k];
		}
		if(trace){
			printf("%d 行目を用いた掃き出し\n\n",k);
			printf("A :\n"); matprint(*A); printf("\n");
			printf("b :\n"); vecprint(*b); printf("\n");
			printf("Hit Return Key\n");
			r = getchar();
		}
	}
}

// 前進消去
void step1partialpivot(MATRIX *A, VECTOR *b)
{
	int i,j,k,p;
	double m,max,tmp,c;
	char r;

	for(k = 0; k < SIZE - 1; k++){

		// 部分ピボット選択
		max = 0.0;
		p = -1;
		for(i = k; i < SIZE; i++){
			m = fabs(A->ent[i][k]);
			if(m > max){
				p = i;
				max = m;
			}
		}
		// A->ent[k][k],...,A->ent[SIZE-1][k] の中で
		// 絶対値最大の要素が A->ent[p][k] 

		// p != k のときは k 行目と p 行目を入れ替える
		if(p != k){
			for(j = k; j < SIZE; j++){
				tmp = A->ent[k][j];
				A->ent[k][j] = A->ent[p][j];
				A->ent[p][j] = tmp;
			}
			tmp = b->ent[k];
			b->ent[k] = b->ent[p];
			b->ent[p] = tmp;
			if(trace){
				printf("%d 行目と %d 行目を交換\n\n",k,p);
				printf("A :\n"); matprint(*A); printf("\n");
				printf("b :\n"); vecprint(*b); printf("\n");
				printf("Hit Return Key\n");
				r = getchar();
			}
		}

		// 掃き出し
		for(i = k+1; i < SIZE; i++){
			c = A->ent[i][k] / A->ent[k][k];
			for(j = k; j < SIZE; j++){
				A->ent[i][j] -= c * A->ent[k][j];
			}
			b->ent[i] -= c * b->ent[k];
		}
		if(trace){
			printf("%d 行目を用いた掃き出し\n\n",k);
			printf("A :\n"); matprint(*A); printf("\n");
			printf("b :\n"); vecprint(*b); printf("\n");
			printf("Hit Return Key\n");
			r = getchar();
		}
	}
}

// 後退代入
VECTOR step2(MATRIX A, VECTOR b)
{
	int i,j;
	double w;
	VECTOR x;

	for(i = SIZE-1; i >= 0; i--){
		w = 0.0;
		for(j = i + 1; j < SIZE; j++){
			w += A.ent[i][j] * x.ent[j];
		}
		x.ent[i] = (b.ent[i] - w) / A.ent[i][i];;
	}
	return x;
}

main()
{
	MATRIX A,A0,L,U,C;
	VECTOR b,b0,x,y,z;
	int i,j,n,k;
	double e,f;
	char r;

	RANDOMIZE();
	SIZE = 16; // 行列のサイズを設定

	/***** 課題 (3) の行列・ベクトル *****/
/*
	for(i=0; i<SIZE; i++)
		for(j=0; j<SIZE; j++)
			A.ent[i][j] = 1.0/(i+j+1);
	for(i=0; i<SIZE; i++)
		b.ent[i] = i+1;
*/
	// 乱数行列・乱数ベクトルで実験するとき
	A = randommat(10.0); b = randomvec(10.0);

	// 整数係数の乱数行列・乱数ベクトルで実験するとき
	//A = randomintmat(10); b = randomintvec(10);

	A0 = A;
	b0 = b;
	printf("A :\n"); matprint(A); printf("\n");
	printf("b :\n"); vecprint(b); printf("\n");

	printf("----- ピボット選択無し -----\n\n");
	step1(&A,&b);		// ピボット選択無し

	printf("\nA' :\n"); matprint(A); printf("\nb' :\n"); vecprint(b); printf("\n");

	x = step2(A,b);
	z = matvecmul(A0,x);		
	printf("x :\n"); vecprint(x); printf("\n");
	printf("検算 Ax :\n"); vecprint(z); printf("\n");

	e = 0.0;
	for(i = 0; i < SIZE; i++){
		f = fabs((z.ent[i] - b0.ent[i])/b0.ent[i]);
		if(f > e) e = f;
	}
	printf("最大相対誤差 = %12.10lf\n\n",e);
	if(trace){
		printf("Hit Return Key\n");
		r = getchar();
	}

	A = A0;
	b = b0;
	printf("----- 部分ピボット選択あり -----\n\n");
	step1partialpivot(&A,&b);	// 部分ピボット選択あり

	printf("\nA' :\n"); matprint(A); printf("\nb' :\n"); vecprint(b); printf("\n");

	x = step2(A,b);
	z = matvecmul(A0,x);		
	printf("x :\n"); vecprint(x); printf("\n");
	printf("検算 Ax :\n"); vecprint(z); printf("\n");

	e = 0.0;
	for(i = 0; i < SIZE; i++){
		f = fabs((z.ent[i] - b0.ent[i])/b0.ent[i]);
		if(f > e) e = f;
	}
	printf("最大相対誤差 = %12.10lf\n",e);


}

/*　実行例

A :
   1.0000000   0.5000000   0.3333333   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000
   0.5000000   0.3333333   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091
   0.3333333   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333
   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231
   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286
   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286   0.0666667
   0.1428571   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286   0.0666667   0.0625000
   0.1250000   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286   0.0666667   0.0625000   0.0588235
   0.1111111   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286   0.0666667   0.0625000   0.0588235   0.0555556
   0.1000000   0.0909091   0.0833333   0.0769231   0.0714286   0.0666667   0.0625000   0.0588235   0.0555556   0.0526316

b :
   1.0000000
   2.0000000
   3.0000000
   4.0000000
   5.0000000
   6.0000000
   7.0000000
   8.0000000
   9.0000000
  10.0000000

----- ピボット選択無し -----

A' :
   1.0000000   0.5000000   0.3333333   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000
   0.0000000   0.0833333   0.0833333   0.0750000   0.0666667   0.0595238   0.0535714   0.0486111   0.0444444   0.0409091
   0.0000000  -0.0000000   0.0055556   0.0083333   0.0095238   0.0099206   0.0099206   0.0097222   0.0094276   0.0090909
   0.0000000  -0.0000000  -0.0000000   0.0003571   0.0007143   0.0009921   0.0011905   0.0013258   0.0014141   0.0014685
   0.0000000  -0.0000000   0.0000000  -0.0000000   0.0000227   0.0000567   0.0000928   0.0001263   0.0001554   0.0001798
   0.0000000  -0.0000000   0.0000000   0.0000000   0.0000000   0.0000014   0.0000043   0.0000081   0.0000123   0.0000167
   0.0000000   0.0000000   0.0000000  -0.0000000  -0.0000000   0.0000000   0.0000001   0.0000003   0.0000007   0.0000011
   0.0000000   0.0000000  -0.0000000  -0.0000000  -0.0000000   0.0000000   0.0000000   0.0000000   0.0000000   0.0000001
   0.0000000   0.0000000  -0.0000000  -0.0000000   0.0000000   0.0000000   0.0000000   0.0000000   0.0000000   0.0000000
   0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000   0.0000000  -0.0000000   0.0000000

b' :
   1.0000000
   1.5000000
   1.1666667
   0.6500000
   0.3000000
   0.1230159
   0.0465368
   0.0166084
   0.0056721
   0.0018717

x :
-999.7787618
97990.9369963
-2328074.8242569
23299602.7615554
-121063444.6976490
359411007.1982907
-632232854.3589545
651042931.5401901
-362276314.8985475
84055205.9700404

検算 Ax :
   1.0000000
   2.0000000
   3.0000000
   4.0000000
   5.0000000
   6.0000000
   7.0000000
   8.0000000
   9.0000000
  10.0000000

最大相対誤差 = 0.0000000055

----- 部分ピボット選択あり -----

A' :
   1.0000000   0.5000000   0.3333333   0.2500000   0.2000000   0.1666667   0.1428571   0.1250000   0.1111111   0.1000000
   0.0000000   0.0833333   0.0888889   0.0833333   0.0761905   0.0694444   0.0634921   0.0583333   0.0538721   0.0500000
   0.0000000   0.0000000   0.0064815   0.0110480   0.0138889   0.0155805   0.0165344   0.0170139   0.0171857   0.0171569
   0.0000000   0.0000000   0.0000000   0.0011364   0.0023810   0.0034341   0.0042517   0.0048611   0.0053030   0.0056150
   0.0000000   0.0000000  -0.0000000   0.0000000  -0.0000454  -0.0001177  -0.0001988  -0.0002778  -0.0003497  -0.0004125
   0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000   0.0000038   0.0000122   0.0000240   0.0000380   0.0000531
   0.0000000   0.0000000   0.0000000  -0.0000000   0.0000000   0.0000000  -0.0000001  -0.0000005  -0.0000011  -0.0000019
   0.0000000   0.0000000  -0.0000000  -0.0000000  -0.0000000   0.0000000   0.0000000   0.0000000   0.0000000   0.0000000
   0.0000000   0.0000000  -0.0000000   0.0000000   0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000  -0.0000000
   0.0000000   0.0000000  -0.0000000   0.0000000  -0.0000000  -0.0000000   0.0000000   0.0000000   0.0000000  -0.0000000

b' :
   1.0000000
   2.6666667
   6.3194444
   4.2500000
  -1.0714286
   0.8181818
  -0.1142857
   0.0144300
  -0.0029526
  -0.0002165

x :
-999.7735079
97990.4638320
-2328064.4579173
23299506.5995571
-121062979.0822431
359409712.3973743
-632230710.6777480
651040844.8420664
-362275212.9035558
84054962.4366208

検算 Ax :
   1.0000000
   2.0000000
   3.0000000
   4.0000000
   5.0000000
   6.0000000
   7.0000000
   8.0000000
   9.0000000
  10.0000000

最大相対誤差 = 0.0000000016

*/