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

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

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

	for(k = 0; k < SIZE; 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];
		}
		//printf("k = %d\n",k);
		//printf("A :\n"); matprint(*A); printf("\n");
		//printf("b :\n"); vecprint(*b); printf("\n");
	}
}

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

	for(k = 0; k < SIZE; 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;
		}

		// 掃き出し
		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];
		}
		//printf("k = %d\n",k);
		//printf("A :\n"); matprint(*A); printf("\n");
		//printf("b :\n"); vecprint(*b); printf("\n");
	}
}

// 後退代入
VECTOR step2(MATRIX A, VECTOR b)
{
	int i,j;
	float 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;
	float e,f;

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

	/***** 課題 (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);

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

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

//	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);

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

//	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

----- ピボット選択無し -----
x :
 571.3134155
-18409.5410156
118100.0000000
-63886.4882812
-1353642.6250000
4482732.0000000
-6257889.5000000
4589384.5000000
-1918863.6250000
423641.6250000

検算 Ax :
   0.9990688
   1.9933352
   2.9893334
   3.9571936
   4.9670334
   5.9893637
   6.9863281
   8.0253687
   9.0014772
  10.0020161

最大相対誤差 = 0.0107015967

----- 部分ピボット選択あり -----
x :
 415.2503967
-11045.5390625
38510.0468750
256529.6875000
-1849798.0000000
4561142.0000000
-5911284.0000000
4814795.0000000
-2676496.2500000
779169.7500000

検算 Ax :
   0.9995986
   1.9829348
   2.9980288
   3.9649732
   4.9768186
   5.9964590
   6.9921875
   8.0226889
   9.0016251
  10.0087347

最大相対誤差 = 0.0087566972

*/