/*	
 *	clipmath.c
 *
 *	ユーティリティ：数学関数
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2001-2016 Naoyuki Sawa
 *
 *	* Thu Dec 31 09:54:58 JST 2015 Naoyuki Sawa
 *	- 作成開始。
 *	- math.hには含まれない非標準の、数学関数をまとめるためのモジュールです。
 *	  今後、非標準の数学関数を追加する場合は、当モジュールに追加して下さい。
 *	* Sat Mar 19 21:19:21 JST 2016 Naoyuki Sawa
 *	- determinant(),cofactor(),solve()を追加しました。
 */
#include "clip.h"
/*****************************************************************************
 *	線形補間
 *****************************************************************************/
#ifndef PIECE
double lerp(double x, double y, double s) {
    //{{式変形して最適化しました。
    //	return (x * (1.0 - s)) + (y * s);	//乗算×2,加減算×2
    //↓式変形して最適化しました。
	return x - (s * (x - y));		//乗算×1,加減算×2
    //}}式変形して最適化しました。
}
#else //PIECE
double lerp(double x, double y, double s);
asm("
		.code
		.align		1
		.global		lerp
lerp:
		pushn		%r1
		;//%r12     := xl
		;//%r13     := xh
		;//%r14     := yl
		;//%r15     := yh
		;//[%sp+ 0] := %r0
		;//[%sp+ 4] := %r1
		;//[%sp+ 8] := retp
		;//[%sp+12] := sl
		;//[%sp+16] := sh
		ld.w		%r0, %r12		;//%r0       := xl
		xcall.d		__subdf3		;//%r11:%r10 :=          (x - y)
		ld.w		%r1, %r13		;//%r1       := xh			*delay*
		xld.w		%r12, [%sp+12]		;//%r12      :=      sl
		xld.w		%r13, [%sp+16]		;//%r13      :=      sh
		ld.w		%r14, %r10		;//%r14      :=          (x - y)l
		xcall.d		__muldf3		;//%r11:%r10 :=     (s * (x - y))
		ld.w		%r15, %r11		;//%r15      :=          (x - y)h	*delay*
		ld.w		%r12, %r0		;//%r12      := xl
		ld.w		%r13, %r1		;//%r13      := xh
		popn		%r1
		ld.w		%r14, %r10		;//%r14      :=     (s * (x - y))l
		xjp.d		__subdf3		;//%r11:%r10 := x - (s * (x - y))
		ld.w		%r15, %r11		;//%r15      :=     (s * (x - y))h	*delay*
");
#endif//PIECE
/*****************************************************************************
 *	連立方程式
 *****************************************************************************/
//既存のmatnf_determinant()を、単制度⇒倍制度に変更した関数です。
//使い方やアルゴリズムは、matnf_determinant()と同じです。
//詳細は、clipflt1.cのmatnf_determinant()の実装を参照して下さい。
double determinant(const double* m/*[n][n]*/, int n) {
	int i;
	double d;
	if(n < 1) { DIE(); }
	//余因子展開を用いて、行列式(determinant)を求めます。
	d = 0.0;
	for(i = 0; i < n; i++) {
		d += (m[(n * i) + 0/*j*/] * cofactor(m, n, i, 0/*j*/));
	}
	return d;
}
/*---------------------------------------------------------------------------*/
//既存のmatnf_cofactor()を、単制度⇒倍制度に変更した関数です。
//使い方やアルゴリズムは、matnf_cofactor()と同じです。
//詳細は、clipflt1.cのmatnf_cofactor()の実装を参照して下さい。
double cofactor(const double* m/*[n][n]*/, int n, int i, int j) {
	int k, l;
	double d;
	double* s/*[n-1][n-1]*/;
	double* t/*[n-1][n-1]*/;
	if((n < 1) ||
	   (i < 0) || (i > (n - 1)) ||
	   (j < 0) || (j > (n - 1))) { DIE(); }
	//m[n行][n列]からi行j列を除いた、小行列(submatrix)を作成します。
	if(n == 1) { return 1; } //小行列が0行0列になる場合
#if     !(defined(_WIN32)&&defined(_MT))
//Win32シングルスレッドの場合,P/ECEの場合,その他の環境の場合に有効なブロック
	s = malloc(sizeof(double) * ((n - 1) * (n - 1)));
	if(!s) { DIE(); }
#else //!(defined(_WIN32)&&defined(_MT))
//Win32マルチスレッドの場合に有効なブロック
	s = alloca(sizeof(double) * ((n - 1) * (n - 1)));
#endif//!(defined(_WIN32)&&defined(_MT))
	t = s;
	for(k = 0; k < n; k++) {
		for(l = 0; l < n; l++) {
			if((k != i) && (l != j)) {
				*t++ = *m;
			}
			m++;
		}
	}
	//小行列式(minor determinant)を求めます。
	d = determinant(s, (n - 1));
	//余因子(cofactor)を求めます。
	if((i ^ j) & 1) { d = -d; }
	//小行列を解放します。
#if     !(defined(_WIN32)&&defined(_MT))
//Win32シングルスレッドの場合,P/ECEの場合,その他の環境の場合に有効なブロック
	free(s);
#endif//!(defined(_WIN32)&&defined(_MT))
	return d;
}
/*---------------------------------------------------------------------------*/
//既存のsolve_simultaneous_equations()を、単制度⇒倍制度に変更した関数です。
//使い方やアルゴリズムは、solve_simultaneous_equations()と同じです。
//詳細は、clipflt1.cのsolve_simultaneous_equations()の実装を参照して下さい。
#if     !(defined(_WIN32)&&defined(_MT))
//Win32シングルスレッドの場合,P/ECEの場合,その他の環境の場合に有効なブロック
double solve(const double* A/*[n][n]*/, const double* b/*[n]*/, double* x/*[n]*/, int n) {
	int i, j;
	double d;
	//行列式(determinant)を求めます。
	d = determinant(A, n);
	//クラメルの公式を用いて、Aの逆行列を求めながら、bを掛けて、xを求めます。
	for(i = 0; i < n; i++) {
		x[i] = 0.0;
		for(j = 0; j < n; j++) {
			x[i] += (cofactor(A, n, j, i) / d) * b[j];
			//            順序注意!!~~~~
		}
	}
	//行列式を返します。
	return d;
}
#else //!(defined(_WIN32)&&defined(_MT))
//Win32マルチスレッドの場合に有効なブロック
struct _ST_solve {
	const double*		A/*[n][n]*/;	//┬solve()の引数と同じです。solve_start_routine()からは参照専用です。書き換えてはいけません。
	const double*		b/*[n]*/;	//│
	double*			x/*[n]*/;	//│
	int			n;		//┘
	double			d;		//─solve()が値を格納します。solve_start_routine()からは参照専用です。書き換えてはいけません。
	volatile int		i;		//┬i＝0→n。solve_start_routine()が読み書きします。iを読み書きする時はcsをロックして下さい。
	CRITICAL_SECTION 	cs;		//┘
};
static unsigned __stdcall solve_start_routine(void* _arg) {
	struct _ST_solve* arg = _arg;
	const double* const A = arg->A;
	const double* const b = arg->b;
	double*       const x = arg->x;
	int           const n = arg->n;
	double        const d = arg->d;
	for(;;) {
		int i, j;
	    EnterCriticalSection(&arg->cs);	//{{arg->iの読み書き:ここから
		i = arg->i;
		if(i != n) { arg->i = (i + 1); }
	    LeaveCriticalSection(&arg->cs);	//}}arg->iの読み書き:ここまで
		if(i == n) { break; }
		x[i] = 0.0;
		for(j = 0; j < n; j++) {
			x[i] += (cofactor(A, n, j, i) / d) * b[j];
			//            順序注意!!~~~~
		}
	}
	return 0;
}
double solve(const double* A/*[n][n]*/, const double* b/*[n]*/, double* x/*[n]*/, int n) {
	struct _ST_solve arg;
	HANDLE hThread[16/*調整可*/];	//当関数が作成するスレッドの数はこの配列サイズが上限となる。2016/03/19現在は16スレッドを上限とした。もし今後16コア以上のCPUが一般的になった場合はこの配列サイズを増やせ。
	int nThread, iThread;
	SYSTEM_INFO si;
	//構造体を初期化する。
	arg.A = A;
	arg.b = b;
	arg.x = x;
	arg.n = n;
	arg.d = determinant(A, n);	//行列式(determinant)を求めます。
	arg.i = 0;
	//クリティカルセクションを作成する。
	InitializeCriticalSection(&arg.cs);
	//スレッド数を決めるために、CPUの論理コア数を取得する。
	GetSystemInfo(&si);
	nThread = si.dwNumberOfProcessors;
	//CPUの論理コア数が、hThread[]配列のサイズよりも多い場合は、切り詰める。
	if(nThread > ARRAY_SIZE(hThread)) { nThread = ARRAY_SIZE(hThread); }
	//CPUの論理コア数が、配列の次元数よりも多い場合は、切り詰める。
	// - 配列の次元数より多いスレッドを生成しても、動作上は問題無いが、何もしないスレッドが生じて無駄である。
	if(nThread > n) { nThread = n; }
	//スレッドを作成する。
	for(iThread = 0; iThread < nThread; iThread++) {
		hThread[iThread] = (HANDLE)_beginthreadex(NULL,
			0,
			solve_start_routine,
			&arg,
			0,
			NULL);
		if(!hThread[iThread]) { DIE(); }	//バグ,又は,リソース不足
	}
	//スレッドの完了を待つ。
	iThread = WaitForMultipleObjects(nThread, hThread, TRUE, INFINITE);
	if((unsigned)(iThread - WAIT_OBJECT_0) >= (unsigned)nThread) { DIE(); }	//バグ
	//スレッドを開放する。
	for(iThread = 0; iThread < nThread; iThread++) {
		if(!CloseHandle(hThread[iThread])) { DIE(); }	//バグ
	}
	//クリティカルセクションを削除する。
	DeleteCriticalSection(&arg.cs);	//忘れないで!!
	//行列式を返します。
	return arg.d;
}
#endif//!(defined(_WIN32)&&defined(_MT))
