// JGS_hinagata.c
// 連立一次方程式の反復解法: 
// ヤコビ法, ガウス-ザイデル法
// 雛形プログラム

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include<time.h>

// 行列のサイズは大域変数としてここで宣言する。
int	size;

////////// マクロ等の定義 //////////

#define trace 0	// 途中経過表示 -> 1 / 非表示 -> 0 に設定せよ
#define N 128	// 行列の最大サイズ
#define RANDOM(n) (rand()%(2*n+1)-n)	// 絶対値 |n| 以下の整数乱数
#define REALRANDOM(x) (x*((2.0*rand()/RAND_MAX)-1))	// 絶対値 |x| 以下の実数乱数 

////////// 関数のプロトタイプ宣言 //////////

// 行列 A, ベクトル b の初期設定
void initialize(double a[][N], double b[]);
// ヤコビ法で近似ベクトルを更新する関数
void Jacobi(double a[][N], double b[], double x[]);
// ガウス-ザイデル法で近似ベクトルを更新する関数
void GaussSeidel(double a[][N], double b[], double x[]);
// 行列と列ベクトルの積を計算する関数
void matvecmul(double a[][N], double b[], double c[]);
// 列ベクトルの差を計算する関数
void vecsub(double u[], double v[], double w[]);
// 列ベクトルの長さを返す関数
double norm(double v[]);
// 行列を出力する関数
void matprint(double a[][N]);
// 列ベクトルを出力する関数
void vecprint(double b[]);

////////// メインルーティン //////////

main()
{
	double a[N][N];		// 行列 A を表す２次元配列
	double b[N];		// ベクトル b を表す配列
	double x[N];		// 解ベクトル x を表す配列
	double w[N];		// ベクトル Ax を格納する配列
	double c[N];		// 誤差ベクトル Ax - b を格納する配列
	double e;			// 誤差ベクトル Ax - b の長さを格納する変数
	int maxrec = 128;	// 最大反復回数
	double adm = 1e-7;	// 許容誤差
	int i, j, step;

	////////// 方程式の設定 //////////

	initialize(a, b);

	printf("A :\n"); matprint(a); printf("\n");
	printf("b :\n"); vecprint(b); printf("\n");
	printf("リターンキーを押してください\n"); getchar();

	////////// Jacobi 法 //////////

	printf("----- Jacobi 法 -----\n\n");
	for(i = 0; i < size; i++) x[i] = 0.;
	step = 0;
	e = 1.;	// 誤差の初期値はダミー
	while(e > adm && step < maxrec){
		if(trace){ printf("%d-th step :\n\n", step); vecprint(x); printf("\n"); }
		Jacobi(a, b, x);	// 近似ベクトル x の更新
		matvecmul(a, x, w);	// w = Ax
		vecsub(w, b, c);	// c = w - b = Ax - b
		e = norm(c);		// e = (c の長さ)
		step++;
	}
	printf("%d-th step :\n\n", step); 
	printf("x:\n"); vecprint(x); printf("\n");
	printf("Ax:\n"); vecprint(w); printf("\n");
	printf("b:\n"); vecprint(b); printf("\n");
	printf("検算 Ax - b :\n\n");
	vecprint(c); printf("\n");
	printf("リターンキーを押してください\n"); getchar();
	
	////////// Gauss-Seidel 法 //////////

	printf("----- Gauss-Seidel 法 -----\n\n");
	for(i = 0; i < size; i++) x[i] = 0.;
	step = 0;
	e = 1.;	// 誤差の初期値はダミー
	while(e > adm && step < maxrec){
		if(trace){ printf("%d-th step :\n\n", step); vecprint(x); printf("\n"); }
		GaussSeidel(a, b, x);	// 近似ベクトル x の更新
		matvecmul(a, x, w);		// w = Ax
		vecsub(w, b, c);		// c = w - b = Ax - b
		e = norm(c);			// e = (c の長さ)
		step++;
	}
	printf("%d-th step :\n\n", step); 
	printf("x:\n"); vecprint(x); printf("\n");
	printf("Ax:\n"); vecprint(w); printf("\n");
	printf("b:\n"); vecprint(b); printf("\n");
	printf("検算 Ax - b :\n\n");
	vecprint(c); printf("\n");
}

////////// 関数定義部 //////////

// 行列 A, ベクトル b の初期設定
void initialize(double a[][N], double b[N])
{
	int i, j;
	srand(time(NULL));	// 乱数の初期化

	// 収束する例
	size = 2;
	a[0][0] = 3; a[0][1] = -2; a[1][0] = 1; a[1][1] = 3;
	b[0] = 1; b[1] = 4;
/*
	// 収束しない例
	size = 2;
	a[0][0] = 1; a[0][1] = 3; a[1][0] = 3; a[1][1] = -2;
	b[0] = 4; b[1] = 1;

	// 式・未知数を入れ替えることにより収束の速さが変わる例の１
	size = 3;
	a[0][0] = 3; a[0][1] = 2; a[0][2] =  2;
	a[1][0] = 2; a[1][1] = 5; a[1][2] = -4;
	a[2][0] = 1; a[2][1] = 1; a[2][2] =  7;
	b[0] = 7; b[1] = 3; b[2] = 9;

	// 式・未知数を入れ替えることにより収束の速さが変わる例の２
	size = 3;
	a[0][0] =  7; a[0][1] = 1; a[0][2] = 1;
	a[1][0] = -4; a[1][1] = 5; a[1][2] = 2;
	a[2][0] =  2; a[2][1] = 2; a[2][2] = 3;
	b[0] = 9; b[1] = 3; b[2] = 7;

	// ランダムな整数係数の場合
	size = 4;
	for(i = 0; i < size; i++){
		for(j = 0; j < size; j++) a[i][j] = RANDOM(10);
		a[i][i] = RANDOM(10 * size);	// 対角成分は大きく
		b[i] = RANDOM(10);
	}

	// ランダムな実数係数の場合
	size = 4;
	for(i = 0; i < size; i++){
		for(j = 0; j < size; j++) a[i][j] = REALRANDOM(10);
		a[i][i] *= size;	// 対角成分は大きく
		b[i] = REALRANDOM(10);
	}
*/
	return;
}

// ヤコビ法の１ステップで近似ベクトルを更新する関数
// （ x に上書きする）
void Jacobi(double a[][N], double b[], double x[])
{
	//////////////////////////////////////////////////////
	////// 未完成部分：                              /////
	////// 次のガウス-ザイデル法をまねてここを埋めよ /////
	//////////////////////////////////////////////////////
	return;
}

// ガウス-ザイデル法で近似ベクトルを更新する関数
// （ x に上書きする）
void GaussSeidel(double a[][N], double b[], double x[])
{
	int i, j;
	double w[N]; 	// w = 新x
	double z;
	for(i = 0; i < size; i++){
		z = b[i];
		for(j = 0; j < i; j++) z -= a[i][j] * w[j];
		for(j = i + 1; j < size; j++) z -= a[i][j] * x[j];
		w[i] = z / a[i][i];
	}
	for(i = 0; i < size; i++) x[i] = w[i];	// w を x に上書き
	return;
}

// 行列の積を計算する関数
// C = AB
void matmul(double a[][N], double b[][N], double c[][N])
{
	int i, j, k;
	double x;
	for(i = 0; i < size; i++){
		for(j = 0; j < size; j++){
			x = 0.0;
			for(k = 0; k < size; k++) x += a[i][k] * b[k][j];
			c[i][j] = x;
		}
	}
	return;
}

// 行列と列ベクトルの積を計算する関数
// c = Ab
void matvecmul(double a[][N], double b[], double c[])
{
	int i, j;
	double w;
	for(i = 0; i < size; i++){
		w = 0.0;
		for(j = 0; j < size; j++) w += a[i][j] * b[j];
		c[i] = w;
	}
	return;
}

// 行列を出力する関数
void matprint(double a[][N])
{
	int i, j;
	for(i = 0; i < size; i++){
		for(j = 0; j < size; j++)
			printf("%12.7lf ", a[i][j]);
		printf("\n");
	}
	return;
}

// 列ベクトルを出力する関数
void vecprint(double b[])
{
	int i;
	for(i = 0; i < size; i++)
		printf("%15.10lf\n", b[i]);
	return;
}

// 列ベクトルの差を計算する関数
// w = u - v
void vecsub(double u[], double v[], double w[])
{
	int i;
	for(i = 0; i < size; i++) w[i] = u[i] - v[i];
	return;
}

// 列ベクトルの長さを返す関数
double norm(double v[])
{
	int i;
	double z = 0.0;
	for(i = 0; i < size; i++) z += v[i] * v[i];
	return sqrt(z);
}