/*
 *	clipncs.c
 *
 *	曲線補間
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2001-2016 Naoyuki Sawa
 *
 *	* Tue Sep 22 12:42:29 JST 2009 Naoyuki Sawa
 *	- 新規作成。
 *	* Wed Sep 23 12:48:12 JST 2009 Naoyuki Sawa
 *	- NCS_interpolate()の、区間を探す処理を少し高速化しました。
 *	  アセンブラ化する前の、アルゴリズムでの高速化です。
 *	* Wed Sep 23 15:38:37 JST 2009 Naoyuki Sawa
 *	- NCS_interpolate()をアセンブラ化し、高速化しました。
 *	* Sat Sep 26 16:40:28 JST 2009 Naoyuki Sawa
 *	- ferguson_coons_interpolate()、CRS_interpolate()を追加しました。
 *	* Sat Sep 26 23:15:21 JST 2009 Naoyuki Sawa
 *	- ferguson_coons_interpolate()をアセンブラ化し、高速化しました。
 *	* Thu Feb 25 21:08:23 JST 2016 Naoyuki Sawa
 *	- CRS_interpolate_circular()を追加しました。
 */
#include "clip.h"

#ifndef PIECE
#define NOASM	// このシンポルを定義すると、C言語版を使用します。 */
#endif /*PIECE*/

/****************************************************************************
 *	自然3次スプライン(NCS : Natural cubic spline)
 ****************************************************************************/

NCS*
NCS_new(const float* t, int t_stride, const float* p, int p_stride, int n)
{
#define T(i) (*(const float*)((const char*)t + (t_stride * (i))))
#define P(i) (*(const float*)((const char*)p + (p_stride * (i))))

	NCS* self;
	int n_seg;
	float* u/*[n_seg+1]*/; /* u[0]=u[n_seg]=0 */
	int i;

	/* 少なくとも一つの接点は必要 */
	if(n <= 0) {
		DIE();
	}

	/* 接点数1では補間できないので、同じ座標2つの補間と見なす */
	if(n == 1) {
		static const float _t[2] = { 0.0f, 1.0f };
		t = _t;
		t_stride = sizeof(float);
		p_stride = 0; /* 座標は*pを二回参照することにする */
		n = 2;
	}

	/* 区間数 = 接点数 - 1 */
	n_seg = n - 1;

	/* 構造体のメモリを確保 */
	self = calloc(sizeof(NCS) + sizeof(NCS_SEG) * n_seg, 1);
	if(!self) {
		DIE();
	}

	/* 区間数を格納 */
	self->n_seg = n_seg;

	/* 連立方程式の解のメモリを確保 */
	u = calloc(sizeof(float) * (n_seg + 1), 1); /* u[0]=u[n_seg]=0 */
	if(!u) {
		DIE();
	}
	if(n_seg >= 2) {
		/* 連立方程式を解く */
		float* m/*[n_seg-1][n_seg-1]*/;
		float* v/*[n_seg-1]*/;
		m = calloc(sizeof(float) * (n_seg - 1) * (n_seg - 1), 1);
		if(!m) {
			DIE();
		}
		v = calloc(sizeof(float) * (n_seg - 1), 1);
		if(!v) {
			DIE();
		}
		for(i = 0; i < n_seg - 1; i++) {
			float h0 = T(i + 1) - T(i    );
			float h1 = T(i + 2) - T(i + 1);
			m[i * (n_seg - 1) + i] = (h0 + h1) * 2.0f;
			if(i < n_seg - 2) {
				m[i * (n_seg - 1) + (i + 1)] = h1;
				m[(i + 1) * (n_seg - 1) + i] = h1;
			}
			v[i] = (((P(i + 2) - P(i + 1)) / h1) - ((P(i + 1) - P(i)) / h0)) * 6.0f;
		}
		solve_simultaneous_equations(m, v, u + 1, n_seg - 1); /* u[0]=u[n_seg]=0 */
		free(m);
		free(v);
	}

	/* 各区間の係数を格納 */
	for(i = 0; i < n_seg; i++) {
		self->seg[i].t = T(i);
		self->seg[i].a = (u[i + 1] - u[i] ) / (T(i + 1) - T(i)) / 6.0f;
		self->seg[i].b = u[i] / 2.0f;
		self->seg[i].c = ((P(i + 1) - P(i)) / (T(i + 1) - T(i))) -
		                 ((T(i + 1) - T(i)) * (u[i + 1] + u[i] * 2.0f) / 6.0f);
		self->seg[i].d = P(i);
	}

	/* 連立方程式の解のメモリを開放 */
	free(u);

	return self;

#undef T
#undef P
}

/*--------------------------------------------------------------------------*/

void
NCS_free(NCS* self)
{
	free(self);
}

/*--------------------------------------------------------------------------*/

#ifdef NOASM
float
NCS_interpolate(NCS* self, float t)
{
//{{2009/09/23:高速化のため変更
//	NCS_SEG* seg = self->seg;
//	NCS_SEG* end = self->seg + (self->n_seg - 1);
//
//	/* この時刻を含む区間を取得します。
//	 * 最初の区間よりも前ならば、最初の区間に含まれるものとします。
//	 * 最後の区間よりも後ならば、最後の区間に含まれるものとします。
//	 */
//	while((seg < end) && ((seg + 1)->t <= t)) {
//		seg++;
//	}
//↓* Wed Sep 23 12:48:12 JST 2009 Naoyuki Sawa
//↓- 区間を探す処理を少し高速化しました。処理結果は、変っていません。
//↓- これまでは、segに現在の区間のポインタを保持し、次の区間のtと比較していましたが、
//↓  先行してsegを次の区間へ進め、最後に1つ戻して現在の区間とするように変更しました。
//↓  この変更に伴い、endは最後の区間でなく、最後の区間+1を保持するようになりました。
	NCS_SEG* seg = self->seg;
	NCS_SEG* end = self->seg + self->n_seg;

	/* この時刻を含む区間を取得します。
	 * 最初の区間よりも前ならば、最初の区間に含まれるものとします。
	 * 最後の区間よりも後ならば、最後の区間に含まれるものとします。
	 */
	do {
		seg++;
	} while((seg != end) && (seg->t <= t));
	seg--;
//}}2009/09/23:高速化のため変更

	/* 時刻を、この区間の開始時刻からの相対とします。 */
	t -= seg->t;

	/* この区間の係数を使って、NCS補間を行います。 */
	return ((seg->a * t + seg->b) * t + seg->c) * t + seg->d;
}
#else /*NOASM*/
asm("
		.code
		.align		1
		.global		NCS_interpolate
NCS_interpolate:
		ld.w		%r4, %r13		;// %r4  := t
		ld.w		%r5, [%r12]+		;// %r5  := n_seg, %r12 := seg
		ld.w		%r6, %r12		;// %r6  := seg
		ld.w		%r9, 20			;// %r9  := sizeof(NCS_SEG)
		mlt.h		%r5, %r9		;// %alr := sizeof(NCS_SEG) * n_seg
		ld.w		%r5, %alr		;// %r5  := sizeof(NCS_SEG) * n_seg
		add		%r5, %r6		;// %r5  := end = seg + n_seg
		;//
NCS_interpolate_L1:					;// for(;;) {
		add		%r6, 20			;//   %r6  := seg++
		cmp		%r6, %r5		;//   if(seg == end)
		jreq		NCS_interpolate_L2	;//     break
		ld.w		%r12, [%r6]		;//   %r12 := seg->t
		xcall.d		__fcmps			;//   if(seg->t > t)
		ld.w		%r13, %r4		;//   %r13 := t			*delay*
		jrle		NCS_interpolate_L1	;//     break
NCS_interpolate_L2:					;// }
		sub		%r6, 20			;// seg--
		ld.w		%r13, [%r6]+		;// %r13 :=      seg->t
		xcall.d		__subsf3		;// %r10 := t -= seg->t
		ld.w		%r12, %r4		;// %r12 := t			*delay*
		ld.w		%r4, %r10		;// %r4  := t
		;//-------------------------------------;//
		ld.w		%r12, [%r6]+		;// %r12 :=     seg->a
		xcall.d		__mulsf3		;// %r10 := p = seg->a * t
		ld.w		%r13, %r4		;// %r13 :=              t	*delay*
		;//
		ld.w		%r13, [%r6]+		;// %r13 :=      seg->b
		xcall.d		__addsf3		;// %r10 := p += seg->b
		ld.w		%r12, %r10		;// %r12 := p			*delay*
		;//
		ld.w		%r12, %r10		;// %r12 := p
		xcall.d		__mulsf3		;// %r10 := p *= t
		ld.w		%r13, %r4		;// %r13 :=      t		*delay*
		;//
		ld.w		%r13, [%r6]+		;// %r13 :=      seg->c
		xcall.d		__addsf3		;// %r10 := p += seg->c
		ld.w		%r12, %r10		;// %r12 := p			*delay*
		;//
		ld.w		%r12, %r10		;// %r12 := p
		xcall.d		__mulsf3		;// %r10 := p *= t
		ld.w		%r13, %r4		;// %r13 :=      t		*delay*
		;//
		ld.w		%r13, [%r6]		;// %r13 :=      seg->d
		xjp.d		__addsf3		;// %r10 := p += seg->d
		ld.w		%r12, %r10		;// %r12 := p			*delay*
");
#endif /*NOASM*/

/****************************************************************************
 *	Ferguson/Coons(ファーガソン/クーンズ)曲線補間
 ****************************************************************************/

#ifdef NOASM
float
ferguson_coons_interpolate(float p0, float p1, float v0, float v1, float t)
{
	/* 以下のコードの計算式は、Ferguson/Coons曲線補間の式:
	 *   (2p0-2p1+v0+v1)t^3 + (-3p0+3p1-2v0-v1)t^2 + v0t + p0
	 * を効率良く計算できるよう変形したもので、結果は同じです。
	 */
	float a = p0 - p1;    /* (p0 - p1)                     */
	float b = a + v0;     /* (p0 - p1)     +  v0           */
	float c = a + b + v1; /* (p0 - p1) * 2 + (v0     + v1) */
	/*               b + c = (p0 - p1) * 3 + (v0 * 2 + v1) */
	return ((c * t - b - c) * t + v0) * t + p0;
}
#else /*NOASM*/
asm("
		.code
		.align		1
		.global		ferguson_coons_interpolate
ferguson_coons_interpolate:
		;// %r12    := p0
		;// %r13    := p1
		;// %r14    := v0
		;// %r15    := v1
		;// [%sp+4] := t
		ld.w		%r4, %r12		;// %r4  := p0
		ld.w		%r5, %r14		;// %r5  := v0
		xcall.d		__subsf3		;// %r10 := a = p0 - p1
		ld.w		%r6, %r15		;// %r6  := v1					*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r6     := v1
		;// %r10    := a
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 :=     a
		ld.w		%r13, %r5		;// %r13 :=         v0
		xcall.d		__addsf3		;// %r10 := b = a + v0
		ld.w		%r7, %r10		;// %r7  := a					*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r6     := v1
		;// %r7     := a
		;// %r10    := b
		;// [%sp+4] := t
		ld.w		%r12, %r7		;// %r12 := a
		ld.w		%r13, %r10		;// %r13 :=     b
		xcall.d		__addsf3		;// %r10 := a + b
		ld.w		%r7, %r10		;// %r7  := b					*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r6     := v1
		;// %r7     := b
		;// %r10    := a + b
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 :=     a + b
		xcall.d		__addsf3		;// %r10 := c = a + b + v1
		ld.w		%r13, %r6		;// %r13 :=             v1			*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r7     := b
		;// %r10    := c
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 := c
		xld.w		%r13, [%sp+4]		;// %r13 :=     t
		xcall.d		__mulsf3		;// %r10 := c * t
		ld.w		%r6, %r10		;// %r6  := c					*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r6     := c
		;// %r7     := b
		;// %r10    := c * t
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 := c * t
		xcall.d		__subsf3		;// %r10 := c * t - b
		ld.w		%r13, %r7		;// %r13 :=         b				*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r6     := c
		;// %r10    := c * t - b
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 := c * t - b
		xcall.d		__subsf3		;// %r10 := c * t - b - c
		ld.w		%r13, %r6		;// %r13 :=             c			*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r10    := c * t - b - c
		;// [%sp+4] := t
		xld.w		%r13, [%sp+4]		;// %r13 :=                   t
		xcall.d		__mulsf3		;// %r10 := (c * t - b - c) * t
		ld.w		%r12, %r10		;// %r12 := (c * t - b - c)			*delay*
		;// %r4     := p0
		;// %r5     := v0
		;// %r10    := (c * t - b - c) * t
		;// [%sp+4] := t
		ld.w		%r12, %r10		;// %r12 := (c * t - b - c) * t
		xcall.d		__addsf3		;// %r10 := (c * t - b - c) * t + v0
		ld.w		%r13, %r5		;// %r13 :=                       v0		*delay*
		;// %r4     := p0
		;// %r10    := (c * t - b - c) * t + v0
		;// [%sp+4] := t
		xld.w		%r13, [%sp+4]		;// %r13 :=                              t
		xcall.d		__mulsf3		;// %r10 := ((c * t - b - c) * t + v0) * t
		ld.w		%r12, %r10		;// %r12 := ((c * t - b - c) * t + v0)		*delay*
		;// %r4     := p0
		;// %r10    := ((c * t - b - c) * t + v0) * t
		ld.w		%r12, %r10		;// %r12 := ((c * t - b - c) * t + v0) * t
		xjp.d		__addsf3		;// %r10 := ((c * t - b - c) * t + v0) * t + p0
		ld.w		%r13, %r4		;// %r13 :=                                  p0	*delay*
");
#endif /*NOASM*/

/****************************************************************************
 *	キャットマルロムスプライン(CRS : Catmull-Rom spline)
 ****************************************************************************/

static float CRS_get_p(const float* p, int p_stride, int n, int i);

float
CRS_interpolate(const float* p, int p_stride, int n, float t)
{
	int i;
	float pN;
	float p0;
	float p1;
	float p2;
	float v0;
	float v1;

	/* 区間を求める */
	i = floorf(t);
	t -= i;

	/* 節点を取得する */
	pN = CRS_get_p(p, p_stride, n, i - 1); /* 区間の前の節点 */
	p0 = CRS_get_p(p, p_stride, n, i    ); /* 区間の開始節点 */
	p1 = CRS_get_p(p, p_stride, n, i + 1); /* 区間の終了節点 */
	p2 = CRS_get_p(p, p_stride, n, i + 2); /* 区間の次の節点 */

	/* 接線ベクトルを求める */
	v0 = (p1 - pN) * 0.5f; /* 区間の開始節点の接線ベクトル */
	v1 = (p2 - p0) * 0.5f; /* 区間の終了節点の接線ベクトル */

	/* Ferguson/Coons曲線補間を行う */
	return ferguson_coons_interpolate(p0, p1, v0, v1, t);
}

/* インデクスiの節点を取得するか、または、推測します。
 * [in]
 *	p,p_stride,n	NCS_new()の引数使用と同じです。NCS_new()のコメントを参照してください。
 *	i		節点のインデクス。
 *			0〜(n-1)の範囲ならば、与えられた節点をそのまま返します。
 *			0未満や、(n-1)超過ならば、節点の位置を推測します。
 * [out]
 *	戻り値		節点。
 * [note]
 *	* Sat Sep 26 12:13:44 JST 2009 Naoyuki Sawa
 *	- 「端点よりも外側の節点」の算出方法について、以下に説明します。
 *	- Catmull-Romスプラインの定義では、両端点を含む、両端区間には曲線が描かれません。
 *	  両端区間に曲線を描くには、両端点の接線ベクトルが必要であり、両端点よりも1つ外側の節点が必要となるからです。
 *	- しかしながら、両端区間まで曲線を描けた方が、便利です。
 *	  たいていのCatmull-Romスプライン描画ルーチンの実装は、両端区間まで曲線を描けるようになっているようです。
 *	  両端点よりも1つ外側の節点は明示的に与えられていないので、推測する必要があり、その方法は定義されていません。
 *	  両端点よりも1つ外側の節点を推測する方法は、各実装ごとに異なるため、両端区間の描画はまちまちとなります。
 *	- いちばん単純な方法は、1つ内側の節点から端点方向へ、同じ直線距離の位置を、1つ外側の節点とする方法です。
 *	  これでも大体良いのですが、1つ内側の節点の部分で、曲線が外側へ少し膨らんで、不自然に見える欠点があります。
 *	- そこで今回は、2つ内側,1つ内側,端点の三つの節点を使って、1つ外側の節点を推測する方法を使いました。
 *	  三つの節点が、ある程度自然に並んでいれば、適切な位置に1つ外側の節点を作成でき、両端区間の曲線も自然に描けます。
 *	  アルゴリズムの具体的な内容は、CRS_get_p()の中のコメントを参照してください。
 *	  上述の方法は、D-Storm社のWebサイトにある、「モデラーカーブの計算方法」を参考にしました。
 *	  その資料を、keep/モデラーカーブの計算方法.7zに保存しました。
 *	- なお、D-Storm社の「モデラーカーブの計算方法」資料には、1つ外側の節点を求める方法までしか記述されていません。
 *	  しかし、同じ手順を繰り返して、2つ外側,3つ外側,...と、いくつでも自然な位置に端点を増やせることに気付きました。
 *	  以下のコードには、その処理を組み込んであります。
 */
static float
CRS_get_p(const float* p, int p_stride, int n, int i)
{
#define P(i) (*(const float*)((const char*)p + (p_stride * (i))))

	float a;
	float b;
	float c;
	float d;

	/* 少なくとも一つの節点は必要 */
	if(n <= 0) {
		DIE();
	}

	/* 節点数1ならば、インデクスに関係無く、唯一の節点を返す */
	if(n == 1) {
		return P(0);
	}

	/* インデクスが前方へ超過の場合… */
	if(i < 0) {
		i = -i; /* (インデクス超過数) */
		/* 節点数2ならば、(端点)＋(1つ内側→端点)×(インデクス超過数) の位置とする */
		if(n == 2) {
			return P(0) + (P(0) - P(1)) * i;
		}
		/* 節点数3以上ならば、(2つ内側)＋(1つ内側→端点)×3＝(1つ外側) の位置を求める
		 * この処理を、(インデクス超過数)、繰り返した位置とする
		 */
		a = P(2); /* (2つ内側) */
		b = P(1); /* (1つ内側) */
		c = P(0); /* (端点) */
		/*{{共通処理*/
		for(;;) {
			d = a + (c - b) * 3.0f; /* (1つ外側) */
			if(!--i) {
				return d;
			}
			a = b; /* (1つ内側)を、次回の(2つ内側)とする */
			b = c; /* (端点)   を、次回の(1つ内側)とする */
			c = d; /* (1つ外側)を、次回の(端点)   とする */
		}
		/*}}共通処理*/
	}

	/* インデクスが後方へ超過の場合… */
	if(i > n - 1) {
		i -= n - 1; /* (インデクス超過数) */
		/* 節点数2ならば、(端点)＋(1つ内側→端点)×(インデクス超過数) の位置とする */
		if(n == 2) {
			return P(n - 1) + (P(n - 1) - P(n - 2)) * i;
		}
		/* 節点数3以上ならば、(2つ内側)＋(1つ内側→端点)×3＝(1つ外側) の位置を求める
		 * この処理を、(インデクス超過数)、繰り返した位置とする
		 */
		a = P(n - 3); /* (2つ内側) */
		b = P(n - 2); /* (1つ内側) */
		c = P(n - 1); /* (端点) */
		/*{{共通処理*/
		for(;;) {
			d = a + (c - b) * 3.0f; /* (1つ外側) */
			if(!--i) {
				return d;
			}
			a = b; /* (1つ内側)を、次回の(2つ内側)とする */
			b = c; /* (端点)   を、次回の(1つ内側)とする */
			c = d; /* (1つ外側)を、次回の(端点)   とする */
		}
		/*}}共通処理*/
	}

	/* インデクスが有効範囲の場合 */
	return P(i);

#undef P
}

/*--------------------------------------------------------------------------*/

//{{2016/02/25追加:CRS_interpolate_circular()を追加しました。
static float CRS_get_p_circular(const float* p, int p_stride, int n, int i);
float CRS_interpolate_circular(const float* p, int p_stride, int n, float t) {
	int i;
	float pN, p0, p1, p2, v0, v1;
	//区間を求める
	i = floorf(t);
	t -= i;
	//節点を取得する
	pN = CRS_get_p_circular(p, p_stride, n, i - 1);	//区間の前の節点
	p0 = CRS_get_p_circular(p, p_stride, n, i    );	//区間の開始節点
	p1 = CRS_get_p_circular(p, p_stride, n, i + 1);	//区間の終了節点
	p2 = CRS_get_p_circular(p, p_stride, n, i + 2);	//区間の次の節点
	//接線ベクトルを求める
	v0 = (p1 - pN) * 0.5f;	//区間の開始節点の接線ベクトル
	v1 = (p2 - p0) * 0.5f;	//区間の終了節点の接線ベクトル
	//Ferguson/Coons曲線補間を行う
	return ferguson_coons_interpolate(p0, p1, v0, v1, t);
}
static float CRS_get_p_circular(const float* p, int p_stride, int n, int i) {
    #define P(i) (*(const float*)((const char*)p + (p_stride * (i))))
	//少なくとも一つの節点は必要。
	if(n <= 0) { DIE(); }
	//インデクスを-(n-1)〜(n-1)の範囲に切り詰める。
	i %= n;
	//インデクスを     0〜(n-1)の範囲に切り詰める。
	if(i < 0) { i += n; }
	//インデクスに対応した節点を返す。
	return P(i);
    #undef P
}
//}}2016/02/25追加:CRS_interpolate_circular()を追加しました。
