/* JacobiMethods.c
実対称行列の固有値・固有ベクトルを求めるヤコビ法のいろいろ
2014年度 ver.
*/

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

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

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

#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 の初期設定
void initialize(double a[][N]);

// 出力と検算を行う関数
void check(double a[][N], double p[][N], double e[]);

// シリアル・ヤコビ法
void SerialJacobi(double a[][N], double p[][N], double e[]);

// シリアル・ヤコビ法(高速版)
void SerialJacobiFast(double a[][N], double p[][N], double e[]);

// 閾ヤコビ法
void ThresholdJacobi(double a[][N], double p[][N], double e[]);

// 閾ヤコビ法(高速版)
void ThresholdJacobiFast(double a[][N], double p[][N], double e[]);

// 行列をコピーする関数
void matcopy(double a[][N], double b[][N]);

// 行列を出力する関数
void matprint(double a[][N]);

// 列ベクトルを出力する関数
void vecprint(double v[]);

// 列ベクトルの長さを返す関数
double norm(double v[]);

// 単位行列を返す関数
void unitmat(double p[][N]);

// 回転の行列を返す関数
void rotmat(int i, int j, double x, double r[][N]);

// 列ベクトルのスカラー倍を計算する関数
void vecscmul(double c, double u[], double v[]);

// 列ベクトルの差を計算する関数
void vecsub(double u[], double v[], double w[]);

// 行列と列ベクトルの積を計算する関数
void matvecmul(double a[][N], double u[], double v[]);

// 行列の積を返す関数
void matmul(double a[][N], double b[][N], double c[][N]);

// 転置行列を返す関数
void transpose(double a[][N], double b[][N]);

// 序数の出力
void ordinal(int k);

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

int main()
{
	double a[N][N];		// 実対称行列 A を表す２次元配列
	double p[N][N];		// A を対角化する直交行列 P を表す２次元配列
	double e[N];		// A の固有値を格納する配列
	clock_t t0, t1;		// 実行時間を測る変数

	// 実対称行列 A の初期設定
	initialize(a);

	// シリアル・ヤコビ法
	printf("///// シリアル・ヤコビ法 /////\n\n");
	t0 = clock();	// 開始時刻
	SerialJacobi(a, p, e);
	t1 = clock();	// 終了時刻
	check(a, p, e);
	printf("処理時間 = %d ms\n\n", t1 - t0);

	// シリアル・ヤコビ法(高速版)
	printf("///// シリアル・ヤコビ法(高速版) /////\n\n");
	t0 = clock();	// 開始時刻
	SerialJacobiFast(a, p, e);
	t1 = clock();	// 終了時刻
	check(a, p, e);
	printf("処理時間 = %d ms\n\n", t1 - t0);

	// 閾ヤコビ法
	printf("///// 閾ヤコビ法 /////\n\n");
	t0 = clock();	// 開始時刻
	ThresholdJacobi(a, p, e);
	t1 = clock();	// 終了時刻
	check(a, p, e);
	printf("処理時間 = %d ms\n\n", t1 - t0);

	// 閾ヤコビ法(高速版)
	printf("///// 閾ヤコビ法(高速版) /////\n\n");
	t0 = clock();	// 開始時刻
	ThresholdJacobiFast(a, p, e);
	t1 = clock();	// 終了時刻
	check(a, p, e);
	printf("処理時間 = %d ms\n\n", t1 - t0);

	return 0;
}

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

// 実対称行列 A の初期設定
void initialize(double a[][N])
{
	int i, j;
	double x;

	// ランダムな整数対称行列
	srand(time(NULL));	// 乱数の初期化
	for(i = 0; i < size; i++)
		for(j = i; j < size; j++){
			x = RANDOM(10);
			a[i][j] = x;
			a[j][i] = x;
		}
/*
	// 課題の行列
	for(i = 0; i < size; i++)
		for(j = i; j < size; j++){
			x = cos(i * j) / 10.0;
			a[i][j] = x;
			a[j][i] = x;
		}
*/
	printf("A:\n"); matprint(a); printf("\n");
	return;
}

// 出力と検算を行う関数
void check(double a[][N], double p[][N], double e[])
{
	double v[N], w[N], x[N], y[N];
	int i, j;
	
	for(j = 0; j < size; j++){
		printf("----- "); ordinal(j + 1); printf(" -----\n\n");
		printf("固有値 λ:\n");
		printf("%12.7f\n\n", e[j]);
		for(i = 0; i < size; i++) v[i] = p[i][j];
		printf("固有ベクトル v:\n"); 
		vecprint(v); printf("\n");
		matvecmul(a, v, w);
		printf("Av:\n"); vecprint(w); printf("\n");
		vecscmul(e[j], v, x);
		printf("λv:\n"); vecprint(x); printf("\n");
		vecsub(w, x, y);
		printf("Av - λv の長さ = %12.10f\n\n", norm(y));
	}
}

// シリアル・ヤコビ法を用いて
// 実対称行列 A を対角化する直交行列 P と、
// 固有値の配列 e を返す関数
// tP A P = diag(e_0,...,e_{n-1})
void SerialJacobi(double a0[][N], double p[][N], double e[])
{
	double a[N][N];		// 作業用に A0 をコピーする行列
	double tp[N][N];	// P の転置行列を表す２次元配列
	double r[N][N];		// 回転の行列 R を表す２次元配列
	double tr[N][N];	// R の転置行列を表す２次元配列
	int steps = 8;		// 反復回数
	int i, j, k;
	double x;
	double eps = 1e-10;	// 角度設定のための許容誤差

	matcopy(a0, a);		// 作業用に A0 をコピー
	unitmat(p);			// P の初期値は単位行列

	for(k = 1; k <= steps; k++){
		if(trace){ ordinal(k); printf(" round : \n",k); }
		for(i = 0; i < size; i++){
			for(j = i + 1; j < size; j++){
				// 回転角度設定
				if(fabs(a[i][i] - a[j][j]) < eps)	x = M_PI / 4.0;
				else x = atan(2 * a[i][j] / (a[i][i] - a[j][j])) / 2.0;
				// 回転の行列
				rotmat(i, j, x, r);
				// 直交行列の更新 新P = PR
				matmul(p, r, p);
				// A の更新 新A = tR A R
				transpose(r, tr);	// tr = (R の転置行列)
				matmul(a, r, a);
				matmul(tr, a, a);	// 新 A = tR A R
				if(trace){
					printf("A :\n"); matprint(a); printf("\n");
					printf("P :\n"); matprint(p); printf("\n");
				}
			}
		}
	}
	for(i = 0; i < size; i++) e[i] = a[i][i];
	return;
}

// シリアル・ヤコビ法(高速版)
// 必要な行・列のみ変形する version
void SerialJacobiFast(double a0[][N], double p[][N], double e[])
{
	double a[N][N];		// 作業用に A0 をコピーする２次元配列
	double aa[N][N];	// 変形中の A を格納する２次元配列
	double pp[N][N];	// 変形中の P を格納する２次元配列
	int steps = 8;		// 反復回数
	int i, j, k, m;
	double x, c, s;
	double eps = 1e-10;	// 角度設定のための許容誤差

	matcopy(a0, a);		// 作業用に A0 をコピー
	unitmat(p);			// P の初期値は単位行列

	for(k = 1; k <= steps; k++){
		if(trace){ ordinal(k); printf(" round : \n",k); }
		for(i = 0; i < size; i++){
			for(j = i + 1; j < size; j++){
				// 回転角度設定
				if(fabs(a[i][i] - a[j][j]) < eps) x = M_PI / 4.0;
				else x = atan(2 * a[i][j] / (a[i][i] - a[j][j])) / 2.0;
				c = cos(x);
				s = sin(x);
				// 直交行列の更新（必要な行・列のみの処理）
				matcopy(p, pp);
				for(m = 0; m < size; m++){
					p[m][i] =  c * pp[m][i] + s * pp[m][j];
					p[m][j] = -s * pp[m][i] + c * pp[m][j];
				}	
				// A の更新（必要な行・列のみの処理）
				matcopy(a, aa);
				for(m = 0; m < size; m++){
					a[m][i] =  c * aa[m][i] + s * aa[m][j];
					a[m][j] = -s * aa[m][i] + c * aa[m][j];
				}
				matcopy(a, aa);
				for(m = 0; m < size; m++){
					a[i][m] =  c * aa[i][m] + s * aa[j][m];
					a[j][m] = -s * aa[i][m] + c * aa[j][m];
				}
				if(trace){
					printf("A :\n"); matprint(a); printf("\n");
					printf("P :\n"); matprint(p); printf("\n");
				}
			}
		}
	}
	for(i = 0; i < size; i++) e[i] = a[i][i];
	return;
}

// 閾ヤコビ法
void ThresholdJacobi(double a0[][N], double p[][N], double e[])
{
	double a[N][N];		// 作業用に A0 をコピーする２次元配列
	double tp[N][N];	// P の転置行列を表す２次元配列
	double r[N][N];		// 回転の行列 R を表す２次元配列
	double tr[N][N];	// R の転置行列を表す２次元配列
	int steps = 8;		// 反復回数
	int i, j, k, m;
	double x, c, s;
	double eps = 1e-10;	// 角度設定のための許容誤差
	double thr;			// しきい値

	matcopy(a0, a);		// 作業用に A0 をコピー
	unitmat(p);			// P の初期値は単位行列

	// しきい値の設定
	x = 0.;
	for(i = 0; i < size; i++)
		for(j = i + 1; j < size; j++)
			x += fabs(a[i][j]);
	thr = ((2.0 * x) / size) / (size - 1);

	for(k = 1; k <= steps; k++){
		if(trace){ ordinal(k); printf(" round : \n",k); }
		for(i = 0; i < size; i++){
			for(j = i + 1; j < size; j++){
				if(fabs(a[i][j]) >= thr){
					// 回転角度設定
					if(fabs(a[i][i] - a[j][j]) < eps)	x = M_PI / 4.0;
					else x = atan(2 * a[i][j] / (a[i][i] - a[j][j])) / 2.0;
					// 回転の行列
					rotmat(i, j, x, r);
					// 直交行列の更新 新P = PR
					matmul(p, r, p);
					// A の更新 新A = tR A R
					transpose(r, tr);	// tr = (R の転置行列)
					matmul(a, r, a);
					matmul(tr, a, a);	// 新 A = tR A R
					if(trace){
						printf("A :\n"); matprint(a); printf("\n");
						printf("P :\n"); matprint(p); printf("\n");
					}
				}
			}
		}
		thr /= 10.0;
	}
	for(i = 0; i < size; i++) e[i] = a[i][i];
	return;
}

// 閾ヤコビ法(高速版)
// 必要な行・列のみ変形する version
void ThresholdJacobiFast(double a0[][N], double p[][N], double e[])
{
	double a[N][N];		// 作業用に A0 をコピーする２次元配列
	double aa[N][N];	// 変形中の A を格納する２次元配列
	double pp[N][N];	// 変形中の P を格納する２次元配列
	int steps = 8;		// 反復回数
	int i, j, k, m;
	double x, c, s;
	double eps = 1e-10;	// 角度設定のための許容誤差
	double thr;			// しきい値

	matcopy(a0, a);		// 作業用に A0 をコピー
	unitmat(p);			// P の初期値は単位行列

	// しきい値の設定
	x = 0.;
	for(i = 0; i < size; i++)
		for(j = i + 1; j < size; j++)
			x += fabs(a[i][j]);
	thr = ((2.0 * x) / size) / (size - 1);

	for(k = 1; k <= steps; k++){
		if(trace){ ordinal(k); printf(" round : \n",k); }
		for(i = 0; i < size; i++){
			for(j = i + 1; j < size; j++){
				if(fabs(a[i][j]) >= thr){
					// 回転角度設定
					if(fabs(a[i][i] - a[j][j]) < eps) x = M_PI / 4.0;
					else x = atan(2 * a[i][j] / (a[i][i] - a[j][j])) / 2.0;
					c = cos(x);
					s = sin(x);
					// 直交行列の更新（必要な行・列のみの処理）
					matcopy(p, pp);
					for(m = 0; m < size; m++){
						p[m][i] =  c * pp[m][i] + s * pp[m][j];
						p[m][j] = -s * pp[m][i] + c * pp[m][j];
					}	
					// A の更新（必要な行・列のみの処理）
					matcopy(a, aa);
					for(m = 0; m < size; m++){
						a[m][i] =  c * aa[m][i] + s * aa[m][j];
						a[m][j] = -s * aa[m][i] + c * aa[m][j];
					}
					matcopy(a, aa);
					for(m = 0; m < size; m++){
						a[i][m] =  c * aa[i][m] + s * aa[j][m];
						a[j][m] = -s * aa[i][m] + c * aa[j][m];
					}
					if(trace){
						printf("A :\n"); matprint(a); printf("\n");
						printf("P :\n"); matprint(p); printf("\n");
					}
				}
			}
		}
		thr /= 10.0;
	}
	for(i = 0; i < size; i++) e[i] = a[i][i];
	return;
}

// 行列をコピーする関数
// B = A
void matcopy(double a[][N], double b[][N])
{
	int i, j;
	for(i = 0; i < size; i++)
		for(j = 0; j < size; j++)
			b[i][j] = a[i][j];
	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 u[])
{
	int i;
	for(i = 0; i < size; i++) printf("%15.10lf\n", u[i]);
	return;
}

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

// 単位行列を返す関数
void unitmat(double e[][N])
{
	int i, j;
	for(i = 0; i < size; i++) for(j = 0; j < size; j++) e[i][j] = 0.0;
	for(i = 0; i < size; i++) e[i][i] = 1.0;
	return;
}

// 回転の行列を返す関数
// R = (i,j)-座標に関する角度 x の回転の行列
void rotmat(int i, int j, double x, double r[][N])
{
	double c = cos(x), s = sin(x);
	unitmat(r);
	r[i][i] = c;
	r[i][j] = -s;
	r[j][i] = s;
	r[j][j] = c;
	return;
}

// 列ベクトルのスカラー倍を計算する関数
// v = cu
void vecscmul(double c, double u[], double v[])
{
	int i;
	for(i = 0; i < size; i++) v[i] = c * u[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;
}

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

// 行列の積を返す関数
// C = AB
void matmul(double a[][N], double b[][N], double c0[][N])
{
	int i, j, k;
	double x;
	double c[N][N];	// a や b が c0 と同一でも構わないように別の配列を用意
	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;
		}
	for(i = 0; i < size; i++)
		for(j = 0; j < size; j++)
			c0[i][j] = c[i][j];
	return;
}

// 転置行列を返す関数
// B = tA
void transpose(double a[][N], double b[][N])
{
	int i, j;
	for(i = 0; i < size; i++)
		for(j = 0; j < size; j++)
			b[i][j] = a[j][i];
	return;
}

// 序数の出力
void ordinal(int k)
{	
	int m = k % 100;
	if((m < 11) || (m > 20)){
		switch(k % 10){
			default: printf("%dth", k); break;
			case 1: printf("%dst", k); break;
			case 2: printf("%dnd", k); break;
			case 3: printf("%drd", k); break;
		}
	}
	else printf("%dth", k); 
	return;
}