/*
 *	clipncss.c
 *
 *	ステートレスNCS補間シーケンサ
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2016 Naoyuki Sawa
 *
 *	* Mon Mar 21 21:34:20 JST 2016 Naoyuki Sawa
 *	- 1st リリース。
 *	- clipncs.cのNCS_new()をベースに、以下の変更を行って作成しました。
 *	  ・NCS_new()は全体を通じてfloat演算でした。
 *	    当モジュールは時刻をuint16_t,座標をint16_t,演算はdouble,補間係数の格納は1次以上の成分をfp16,0次の成分(=座標)をint16_tとしました。
 *	    このように、それぞれの箇所に適切な型を選択する事によって、演算精度の向上とデータ容量を低減するようにしました。(効果は未検証です。)
 *	  ・NCS_new()は実行時に補間係数を求める方法しか有りませんでした。
 *	    当モジュールはNCS_new()と同様に実行時に補間係数を求める事も出来ますし、実行前にツールを使って補間係数を求めておく事も出来ます。
 *	    節点が十数個以上ぐらいになると、補間係数の計算には数秒〜数十秒以上時間が掛かるので、実行時に補間係数を求めるのは難しいからです。
 *	    当モジュールの方法ならば、ビルド時間が許す限り多くの節点の補間データを作成しておくことが可能です。(ROM化を考えた場合も有利です。)
 *	    時間が掛かるのはあくまでも補間係数を求める処理だけであり、節点の数が増えても実行時の補間処理の時間には影響有りません。
 *	  ・NCS_new()は一つの構造体ごとに一つの軸しか補間出来ませんでした。
 *	    複数の軸を補間するためには複数の構造体を作成する必要が有り、メモリの無駄が生じていました。
 *	    当モジュールは一つの構造体で複数の軸を管理出来、ノード毎に一つの時刻を共有する複数の軸の補間を行うので、容量と速度を改善出来ます。
 *	- 以上のような改善を行ったのですが、とは言え、アルゴリズム自体はclipncs.cのNCS_new()と全く同じです。
 *	  アルゴリズムの詳細については、clipncs.cのNCS_new()を参照して下さい。
 *	* Thu Mar 24 21:04:15 JST 2016 Naoyuki Sawa
 *	- NcsSeq_GetVal_subr()の丸め処理が、+∞方向に偏っていた問題を修正しました。
 *	  単純に0.5を足して切り捨てるのは間違いで、正負を考慮して丸めを行うのが正しい処理でした。
 *	* Mon Mar 28 21:40:28 JST 2016 Naoyuki Sawa
 *	- NcsSeq_GetVal_subr()の中で、積和演算処理にfma()を使うように変更しました。
 *	  コードサイズが僅かに減りました。結果は変更前と同じです。
 *	* Thu Mar 31 21:48:20 JST 2016 Naoyuki Sawa
 *	- ST_NcsSeq構造体にtEndフィールドを追加しました。
 *	- NcsSeq_GetLen(),NcsSeqNo_GetLen()を追加しました。
 *	* Tue Sep 06 21:06:46 JST 2016 Naoyuki Sawa
 *	- (int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
 *	  詳細は、NcsSeg_compar(),NcsSeq_GetVal_subr()内のコメントを参照して下さい。
 */
#include "clip.h"
/****************************************************************************
 *	グローバル関数
 ****************************************************************************/
//ダミーの時刻取得関数
//ダミーの時刻取得関数は(iNode=0)に対して時刻0,(iNode=1)に対して時刻1を返す。
//異なる時刻であれば何でも良い。
static int fnGetTim_1node(int iNode, void* arg) {
	return iNode;
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
//ダミーの座標取得関数
//ダミーの座標取得関数は元の座標取得関数を(iNode=0)固定で呼び出した値を返す。
static int fnGetPos_1node( int iNode, int iAxs, void* arg) {
	                    void** arg_1node/*[2]*/ = arg;
	   int (*fnGetPos)(int iNode, int iAxs, void* arg) = arg_1node[0];
	                       iNode = 0;             arg  = arg_1node[1];
	return (*fnGetPos)(    iNode,     iAxs,       arg);
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
static int call_fnGetTim(int (*fnGetTim)(int iNode, void* arg), int iNode, void* arg) {
	int iTim = (*fnGetTim)(iNode, arg);
	if((unsigned)iTim > UINT16_MAX) { DIE(); }
	return iTim;
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
static int call_fnGetPos(int (*fnGetPos)(int iNode, int iAxs, void* arg), int iNode, int iAxs, void* arg) {
	int iPos = (*fnGetPos)(iNode, iAxs, arg);
	if((iPos < INT16_MIN) || (iPos > INT16_MAX)) { DIE(); }
	return iPos;
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
ST_NcsSeq* NcsSeq_New(int (*fnGetTim)(int iNode, void* arg), int (*fnGetPos)(int iNode, int iAxs, void* arg), int nNode, int nAxs, void* arg) {
	void* arg_1node[2];
	int nSeg, SegBytes, iAxs, i;
	ST_NcsSeq* pNcsSeq;
	ST_NcsSeg* pNcsSeg;
	double* A/*[nSeg-1][nSeg-1]*/;						//明示的に格納しない要素は0.0のままとする。
	double* b/*[nSeg-1]*/;							//全ての要素を明示的に格納する。
	double* x/*[nSeg+1]*/;							//(x[0]=x[nSeg]=0.0)固定とする。
	//節点の数は、(1〜(UINT8_MAX+1))の範囲でなくてはならない。
	if((nNode < 1) || (nNode > (UINT8_MAX+1))) { DIE(); }
	//軸の数は、(1〜(UINT8_MAX+1))の範囲でなくてはならない。
	if((nAxs < 1) || (nAxs > (UINT8_MAX+1))) { DIE(); }
	//節点が一つである場合、そのままでは補間出来ないので、擬似的に異なる時間・同じ座標の節点二つの補間と見なす。
	if(nNode == 1) {
		arg_1node[0] = fnGetPos;					//┬ダミーの座標取得関数(fnGetPos_1node())へのパラメータを格納する。
		arg_1node[1] = arg;						//┘
		fnGetTim = fnGetTim_1node;					//ダミーの時刻取得関数に置き換える。ダミーの時刻取得関数は(iNode=0)に対して時刻0,(iNode=1)に対して時刻1を返す。異なる時刻であれば何でも良い。
		fnGetPos = fnGetPos_1node;					//ダミーの座標取得関数に置き換える。ダミーの座標取得関数は元の座標取得関数を(iNode=0)固定で呼び出した値を返す。
		nNode = 2;							//節点の数を1⇒2に置き換える。
		arg = arg_1node;						//ダミーの座標取得関数(fnGetPos_1node())へのパラメータに置き換える。
	}
	//区間の数=(節点の数-1)を求める。
	nSeg = nNode - 1;
	//ST_NcsSeg構造体のバイト数を求める。
	SegBytes = sizeof(ST_NcsSeg) -
	           sizeof(ST_NcsAxs)/*ST_NcsSeg.TBL_Axs[0]の分*/ +
	          (sizeof(ST_NcsAxs) * nAxs);
	//構造体のメモリを確保する。
	pNcsSeq = malloc(sizeof(ST_NcsSeq) -					//全ての要素を明示的に格納するのでmalloc()で構いません。
	                 sizeof(ST_NcsSeg)/*ST_NcsSeq.TBL_Seg[0]の分*/ +
	                 (SegBytes * nSeg));
	if(!pNcsSeq) { DIE(); }
	//最後の節点の時刻(=最後の区間の最終時刻+1)を格納する。
	pNcsSeq->tEnd = call_fnGetTim(fnGetTim, nSeg/*=(nNode-1)*/, arg);
	//(区間の数-1)を格納する。
	pNcsSeq->nSeg_1 = nSeg - 1;
	//(軸の数-1)を格納する。
	pNcsSeq->nAxs_1 = nAxs - 1;
	//連立方程式を解くためのメモリを確保する。
	A = calloc(sizeof(double) * (nSeg-1) * (nSeg-1), 1);			//明示的に格納しない要素を0.0のままとするためにcalloc()が必須です。
	if(!A) { DIE(); }
	b = malloc(sizeof(double) * (nSeg-1));					//全ての要素を明示的に格納するのでmalloc()で構いません。
	if(!b) { DIE(); }
	x = calloc(sizeof(double) * (nSeg+1), 1);				//(x[0]=x[nSeg]=0.0)固定とするためにcalloc()が必須です。
	if(!x) { DIE(); }
	//各軸について…
	for(iAxs = 0; iAxs < nAxs; iAxs++) {
#define T(iNode) call_fnGetTim(fnGetTim,(iNode)     ,arg)
#define P(iNode) call_fnGetPos(fnGetPos,(iNode),iAxs,arg)
		//節点が二つ以上ならば…
		if(nSeg >= 2) {
			//連立方程式を解く。
			for(i = 0; i < (nSeg-1); i++) {			//　┌───アプリケーション定義のfnGetTim()関数が返す時刻の値は、昇順になっていなければならない。もしここで停止したら、このノードの時刻が前のノードの時刻と同じか逆行している。アプリケーションのデータバグか、アプリケーション定義のfnGetTim()関数のバグだろう。
				const int t01 = T(i+1) - T(i+0);	//←┤	//┐
				const int t12 = T(i+2) - T(i+1);	//←┤	//│
				const int p01 = P(i+1) - P(i+0);	//　│	//│
				const int p12 = P(i+2) - P(i+1);	//　│	//│
				if((t01 <= 0) || (t12 <= 0)) { DIE(); }	//←┘	//│
				A[(i * (nSeg-1)) + i] = (t01 + t12) * 2;	//├この範囲は計算する値が全て整数で除算も無いので整数演算で足りる。
				if(i < (nSeg-2)) {				//│
					A[((i+0) * (nSeg-1)) + (i+1)] = t12;	//│
					A[((i+1) * (nSeg-1)) + (i+0)] = t12;	//│
				}						//┘
				b[i] = (((double)p12 / (double)t12) -		//┬ここだけ除算が有るので実数演算をおこなう。
				        ((double)p01 / (double)t01)) * 6.0;	//┘
			}
			solve(A, b, (x+1), (nSeg-1));				//x[0]=x[nSeg]=0.0
		}
		//各区間の補間係数を格納する。
		pNcsSeg = pNcsSeq->TBL_Seg;
		for(i = 0; i < nSeg; i++) {
			const double t01 =   T(i+1) - T(i+0);
			const double a   = ((x[i+1] - x[i+0]) / t01) / 6.0;
			const double b   =            x[i+0]         / 2.0;
			const double c   = ((P(i+1) - P(i+0)) / t01) - ((((x[i+0] * 2.0) + x[i+1]) * t01) / 6.0);
			pNcsSeg->t               = T(i+0);			//uint16_t	複数回上書きで格納してしまうが同じ値なので問題無い。
			pNcsSeg->TBL_Axs[iAxs].a = truncdfhf2(a);		//  fp16
			pNcsSeg->TBL_Axs[iAxs].b = truncdfhf2(b);		//  fp16
			pNcsSeg->TBL_Axs[iAxs].c = truncdfhf2(c);		//  fp16
			pNcsSeg->TBL_Axs[iAxs].d = P(i+0);			// int16_t
			pNcsSeg = (ST_NcsSeg*)((char*)pNcsSeg + SegBytes);
		}
#undef  T
#undef  P
	}
	//連立方程式を解くためのメモリを開放する。
	free(A);
	free(b);
	free(x);
	return pNcsSeq;
}
/*--------------------------------------------------------------------------*/
void NcsSeq_Free(ST_NcsSeq* pNcsSeq) {
	free(pNcsSeq);
}
/*--------------------------------------------------------------------------*/
static int NcsSeg_compar(const void* x, const void* y) {
//{{2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
//	int t = (int)x;
//↓2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
#ifndef _WIN64
	int t = (int)x;		//32ビット版は、これまでと変更有りません。32ビット版も64ビット版と同じ書き方にしても動作は変わらないのですが、簡潔さを優先してこれまでのままとする事にしました。
#else //_WIN64
	int t = (intptr_t)x;	//64ビット版は、(void*)(64ビット)⇒(int)(32ビット)にキャストしようとすると警告が出ます。元の値が32ビットの整数パラメータなので(int)にキャストしても安全ではあるのですが、警告を回避するために(void*)(64ビット)⇒(intptr_t)(64ビット)⇒暗黙の(int)(32ビット)へのキャスト、という手順を取る事にしました。
#endif//_WIN64
//}}2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
	const ST_NcsSeg* pNcsSeg = y;
	if(t < pNcsSeg->t) { return -1; }
	if(t > pNcsSeg->t) { return  1; }
	                   { return  0; }
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
static void NcsSeq_GetVal_subr(const ST_NcsSeq* pNcsSeq, int t, int iAxs, int nVal, int* aVal/*[nVal]*/) {
	int nSeg, nAxs, SegBytes;
	ST_NcsSeg* pNcsSeg;
	ST_NcsAxs* pNcsAxs;
	double a, b, c, dt;
	//区間の数を取得する。
	nSeg = pNcsSeq->nSeg_1 + 1;
	//軸の数を取得する。
	nAxs = pNcsSeq->nAxs_1 + 1;
	//ST_NcsSeg構造体のバイト数を求める。
	SegBytes = sizeof(ST_NcsSeg) -
	           sizeof(ST_NcsAxs)/*ST_NcsSeg.TBL_Axs[0]の分*/ +
	          (sizeof(ST_NcsAxs) * nAxs);
	//この時刻を含む区間を取得する。
	//最初の区間よりも前ならば、最初の区間に含まれるものとする。
	//最後の区間よりも後ならば、最後の区間に含まれるものとする。
//{{2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
//	pNcsSeg = upper_bound(
//		(void*)t,
//		pNcsSeq->TBL_Seg,
//		nSeg,
//		SegBytes,
//		NcsSeg_compar);
//↓2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
#ifndef _WIN64
	pNcsSeg = upper_bound(		//32ビット版は、これまでと変更有りません。32ビット版も64ビット版と同じ書き方にしても動作は変わらないのですが、簡潔さを優先してこれまでのままとする事にしました。
		(void*)t,
		pNcsSeq->TBL_Seg,
		nSeg,
		SegBytes,
		NcsSeg_compar);
#else //_WIN64
	pNcsSeg = upper_bound(		//64ビット版は、(int)(32ビット)⇒(void*)(64ビット)に直接キャストしようとすると警告が出ます。(int)(32ビット)⇒(intptr_t)(64ビット)⇒(void*)(64ビット)の順でキャストすれば警告は出ません。
		(void*)(intptr_t)t,
		pNcsSeq->TBL_Seg,
		nSeg,
		SegBytes,
		NcsSeg_compar);
#endif//_WIN64
//}}2016/09/06変更:(int)値を(void*)にキャストして整数キーとして使用する箇所で、64ビット版ではコンパイラの警告が出ていた問題を回避しました。
	if(pNcsSeg > pNcsSeq->TBL_Seg) {
		pNcsSeg = (ST_NcsSeg*)((char*)pNcsSeg - SegBytes);
	}
	//この区間の開始時刻から、指定された時刻までの、相対時間を求める。
	dt = (double)(t - pNcsSeg->t);
	//この区間の、各軸の補間係数を使って、NCS補間を行う。
	if((unsigned)iAxs >= (unsigned)nAxs) { DIE(); }		//(iAxs≧nAxs)ならばエラーとする。アプリケーションプログラムが不正なiAxsを指定してNcsSeq_GetVal()を呼び出した場合に、エラーとするための判定です。
	nVal = min((unsigned)nVal, (unsigned)(nAxs - iAxs));	//(iAxs+nVal＞nAxs)はエラーとせずnAxsを切り詰める。NcsSeq_GetValAll()から(nVal=-1)で呼び出された場合、(nVal=軸の数)に置き換えて処理するためです。
	pNcsAxs = &pNcsSeg->TBL_Axs[iAxs];
	do {
		a = extendhfdf2(pNcsAxs->a);	//  fp16
		b = extendhfdf2(pNcsAxs->b);	//  fp16
		c = extendhfdf2(pNcsAxs->c);	//  fp16
//{{2016/03/24修正:NcsSeq_GetVal_subr()の丸め処理が、+∞方向に偏っていた問題を修正しました。単純に0.5を足して切り捨てるのは間違いで、正負を考慮して丸めを行うのが正しい処理でした。
//		*aVal++ = (int)((((a * dt + b) * dt + c) * dt) + 0.5) + pNcsAxs->d;
//		//                                                      ~~~~~~~~~~int16_t
//		//             │←───── 実数演算 ─────→│
//		//        │←-─────── 整数演算 ────────────→│
//↓2016/03/24修正:NcsSeq_GetVal_subr()の丸め処理が、+∞方向に偏っていた問題を修正しました。単純に0.5を足して切り捨てるのは間違いで、正負を考慮して丸めを行うのが正しい処理でした。
//		*aVal++ = lround(((a * dt + b) * dt + c) * dt) + pNcsAxs->d;
//		//                                               ~~~~~~~~~~int16_t
//		//              │←────実数演算───→│
//		//        │←───────整数演算──────────→│
//↓2016/03/28変更:NcsSeq_GetVal_subr()の中で、積和演算処理にfma()を使うように変更しました。コードサイズが僅かに減りました。結果は変更前と同じです。
		*aVal++ = lround(fma(fma(a, dt, b), dt, c) * dt) + pNcsAxs->d;
		//                                                 ~~~~~~~~~~int16_t
		//              │←────実数演算────→│
		//        │←───────整数演算───────────→│
//}}2016/03/28変更:NcsSeq_GetVal_subr()の中で、積和演算処理にfma()を使うように変更しました。コードサイズが僅かに減りました。結果は変更前と同じです。
		pNcsAxs++;
	} while(--nVal);
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
int NcsSeq_GetVal(const ST_NcsSeq* pNcsSeq, int t, int iAxs) {
	int iVal;
	NcsSeq_GetVal_subr(pNcsSeq, t, iAxs, 1, &iVal);
	return iVal;
}
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void NcsSeq_GetValAll(const ST_NcsSeq* pNcsSeq, int t, int* aVal/*[nAxs]*/) {
	NcsSeq_GetVal_subr(pNcsSeq, t, 0, -1, aVal);
}
/*--------------------------------------------------------------------------*/
int NcsSeq_GetLen(const ST_NcsSeq* pNcsSeq) {
	return pNcsSeq->tEnd;
}
/****************************************************************************
 *	dNcsSeqC.exeが出力したバイナリ形式を扱うためのユーティリティ関数
 ****************************************************************************/
#ifdef  USE_NCSSEQ_BIN
static const ST_NcsSeq* NcsSeqNo_GetNcsSeq(const void* pNcsSeqBin, int iNcsSeqNo) {
	const int* const TBL_NcsSeqNo = pNcsSeqBin;							//バイナリの先頭にアドレステーブルが有る。アドレスはデータ本体(HALF配列)のインデクスである。
	const int nNcsSeqNo = TBL_NcsSeqNo[0];								//アドレステーブルの先頭[0]に要素数が入っている。要素数はそれ自身([0])も含む。
	const unsigned short* const TBL_NcsSeq = (const unsigned short*)&TBL_NcsSeqNo[nNcsSeqNo];	//アドレステーブルの直後からデータ本体(HALF配列)が開始する。
	if((iNcsSeqNo <= 0) || (iNcsSeqNo >= nNcsSeqNo)) { DIE(); }
	return (const ST_NcsSeq*)&TBL_NcsSeq[TBL_NcsSeqNo[iNcsSeqNo]];
}
int NcsSeqNo_GetVal(const void* pNcsSeqBin, int iNcsSeqNo, int t, int iAxs) {
	const ST_NcsSeq* pNcsSeq = NcsSeqNo_GetNcsSeq(pNcsSeqBin, iNcsSeqNo);
	return NcsSeq_GetVal(pNcsSeq, t, iAxs);
}
void NcsSeqNo_GetValAll(const void* pNcsSeqBin, int iNcsSeqNo, int t, int* aVal/*[nAxs]*/) {
	const ST_NcsSeq* pNcsSeq = NcsSeqNo_GetNcsSeq(pNcsSeqBin, iNcsSeqNo);
	NcsSeq_GetValAll(pNcsSeq, t, aVal);
}
int NcsSeqNo_GetLen(const void* pNcsSeqBin, int iNcsSeqNo) {
	const ST_NcsSeq* pNcsSeq = NcsSeqNo_GetNcsSeq(pNcsSeqBin, iNcsSeqNo);
	return NcsSeq_GetLen(pNcsSeq);
}
#endif//USE_NCSSEQ_BIN
