package provide Ncs 2016.03.26
;#//	
;#//	Ncs.tcl
;#//	
;#//	* Sat Mar 26 22:42:00 JST 2016 Naoyuki Sawa
;#//	- 1st リリース。
;#//	
set VERSION 20160326	;#//最終更新日
#//----------------------------------------------------------------------------
#//from clipmath.c - determinant()
proc determinant {_m n} {
	upvar $_m m
	set d 0.0
	if {$n < 1} then {DIE}
	#//余因子展開を用いて、行列式(determinant)を求めます。
	for {set i 0} {$i < $n} {incr i} {
		set tmp [cofactor m $n $i 0]
		set d   [expr {$d + ($m($i,0) * $tmp)}]
	}
	return $d
}
#//----------------------------------------------------------------------------
#//from clipmath.c - cofactor()
proc cofactor {_m n i j} {
	upvar $_m m
	if [expr {($n < 1) ||
	          ($i < 0) || ($i > ($n - 1)) ||
	          ($j < 0) || ($j > ($n - 1))}] {DIE}
	#//m[n行][n列]からi行j列を除いた、小行列(submatrix)を作成します。
	if {$n == 1} then {return 1.0}	;#//小行列が0行0列になる場合
	#//s[n-1][n-1]
	for {set k 0} {$k < $n} {incr k} {
		set K [expr {$k - 1}]
		for {set l 0} {$l < $n} {incr l} {
			set L [expr {$l - 1}]
			if {$k < $i} then {
				if {$l < $j} then {
					set s($k,$l) $m($k,$l)
				} elseif {$l > $j} then {
					set s($k,$L) $m($k,$l)
				}
			} elseif {$k > $i} then {
				if {$l < $j} then {
					set s($K,$l) $m($k,$l)
				} elseif {$l > $j} then {
					set s($K,$L) $m($k,$l)
				}
			}
		}
	}
	#//小行列式(minor determinant)を求めます。
	set tmp [expr {$n - 1}]
	set d   [determinant s $tmp]
	#//余因子(cofactor)を求めます。
	if {($i ^ $j) & 1} then {
		set d [expr {-$d}]
	}
	return $d
}
#//----------------------------------------------------------------------------
#//from clipmath.c - solve()
proc solve {_A _b _x n} {
	upvar $_A A
	upvar $_b b
	upvar $_x x
	#//行列式(determinant)を求めます。
	set d [determinant A $n]
	#//クラメルの公式を用いて、Aの逆行列を求めながら、bを掛けて、xを求めます。
	for {set i 0} {$i < $n} {incr i} {
		set x($i) 0.0
		for {set j 0} {$j < $n} {incr j} {
			set tmp   [cofactor A $n $j $i]
			set tmp   [expr {($tmp / $d) * $b($j)}]
			set x($i) [expr {$x($i) + $tmp}]
		}
	}
	#//行列式を返します。
	return $d
}
#//----------------------------------------------------------------------------
#//from clipncss.c - NcsSeq_New()
proc NcsSeq_New {_T _P nNode nAxs} {
	upvar $_T T
	upvar $_P P
	#//節点の数は、(1〜(UINT8_MAX+1))の範囲でなくてはならない。
	if {($nNode < 1) || ($nNode > (255+1))} then {DIE}
	#//軸の数は、(1〜(UINT8_MAX+1))の範囲でなくてはならない。
	if {($nAxs < 1) || ($nAxs > (255+1))} then {DIE}
	#//節点が一つである場合、そのままでは補間出来ないので、擬似的に異なる時間・同じ座標の節点二つの補間と見なす。
	if {$nNode == 1} then {
		set __T(0) 0
		set __T(1) 1
		for {set iAxs 0} {$iAxs < $nAxs} {incr iAxs} {
			set __P(0,$iAxs) $P(0,$iAxs)
			set __P(1,$iAxs) $P(1,$iAxs)
		}
		return [NcsSeq_New __T __P 2 $nAxs]
	}
	#//区間の数=(節点の数-1)を求める。
	set nSeg [expr {$nNode - 1}]
	#//構造体のメモリを確保する。
	set pNcsSeq [dict create]
	#//区間の数を格納する。
	dict set pNcsSeq nSeg $nSeg
	#//軸の数を格納する。
	dict set pNcsSeq nAxs $nAxs
	#//A[nSeg-1][nSeg-1]
	#//b[nSeg-1]
	#//x[nSeg+1]
	#//明示的に格納しない要素を0.0のままとする。
	for {set i 0} {$i < ($nSeg - 1)} {incr i} {
		for {set j 0} {$j < ($nSeg - 1)} {incr j} {
			set A($i,$j) 0.0
		}
	}
	#//(x[0]=x[nSeg]=0.0)固定とする。
	set x(0)     0.0
	set x($nSeg) 0.0
	#//各軸について…
	set pNcsSeg [dict create]
	for {set iAxs 0} {$iAxs < $nAxs} {incr iAxs} {
		#//節点が二つ以上ならば…
		if {$nSeg >= 2} then {
			#//連立方程式を解く。
			for {set i 0} {$i < ($nSeg - 1)} {incr i} {
				set i0 [expr {$i+0}]
				set i1 [expr {$i+1}]
				set i2 [expr {$i+2}]
				set t01 [expr {$T($i1) - $T($i0)}]
				set t12 [expr {$T($i2) - $T($i1)}]
				set p01 [expr {$P($i1,$iAxs) - $P($i0,$iAxs)}]
				set p12 [expr {$P($i2,$iAxs) - $P($i1,$iAxs)}]
				if {($t01 <= 0) || ($t12 <= 0)} then {DIE}
				set A($i,$i) [expr {($t01 + $t12) * 2}]
				if {$i < ($nSeg - 2)} then {
					set A($i0,$i1) $t12
					set A($i1,$i0) $t12
				}
				set b($i) [expr {((double($p12) / double($t12)) -
				                  (double($p01) / double($t01))) * 6.0}]
			}
			solve A b _x [expr {$nSeg - 1}]
			for {set i 1} {$i < $nSeg} {incr i} {
				set x($i) $_x([expr {$i-1}])
			}
		}
		#//各区間の補間係数を格納する。
		for {set i 0} {$i < $nSeg} {incr i} {
			set i0  [expr {$i+0}]
			set i1  [expr {$i+1}]
			set t01 [expr {double(  $T($i1) - $T($i0)               )}]
			set _a  [expr {double((($x($i1) - $x($i0)) / $t01) / 6.0)}]
			set _b  [expr {double(            $x($i0)          / 2.0)}]
			set _c  [expr {double((($P($i1,$iAxs) - $P($i0,$iAxs)) / $t01) - (((($x($i0) * 2.0) + $x($i1)) * $t01) / 6.0))}]
			dict set pNcsSeq $i $iAxs t $T($i0)		;#//uint16_t	複数回上書きで格納してしまうが同じ値なので問題無い。
			dict set pNcsSeq $i $iAxs a $_a			;#//  fp16
			dict set pNcsSeq $i $iAxs b $_b			;#//  fp16
			dict set pNcsSeq $i $iAxs c $_c			;#//  fp16
			dict set pNcsSeq $i $iAxs d $P($i0,$iAxs)	;#// int16_t
		}
	}
	return $pNcsSeq
}
#//----------------------------------------------------------------------------
#//from clipncss.c - NcsSeq_GetVal(),NcsSeq_GetVal_subr()
proc NcsSeq_GetVal {pNcsSeq t iAxs} {
	#//区間の数を取得する。
	set nSeg [dict get $pNcsSeq nSeg]
	#//軸の数を取得する。
	set nAxs [dict get $pNcsSeq nAxs]
	if {$iAxs >= $nAxs} then {DIE}
	#//この時刻を含む区間を取得する。
	#//最初の区間よりも前ならば、最初の区間に含まれるものとする。
	#//最後の区間よりも後ならば、最後の区間に含まれるものとする。
	for {set i 0} {$i < $nSeg} {incr i} {
		set t0 [dict get $pNcsSeq $i $iAxs t]
		if {$t0 > $t} then {break}
	}
	if {$i} then {incr i -1}
	#//この区間の開始時刻から、指定された時刻までの、相対時間を求める。
	set t0 [dict get $pNcsSeq $i $iAxs t]
	set dt [expr {double($t - $t0)}]
	#//この区間の、各軸の補間係数を使って、NCS補間を行う。
	set a [dict get $pNcsSeq $i $iAxs a]	;#//  fp16
	set b [dict get $pNcsSeq $i $iAxs b]	;#//  fp16
	set c [dict get $pNcsSeq $i $iAxs c]	;#//  fp16
	set d [dict get $pNcsSeq $i $iAxs d]	;#// int16_t
	return [expr {round((($a * $dt + $b) * $dt + $c) * $dt) + $d}]
}
#//----------------------------------------------------------------------------
proc Ncs_test {} {
	set T(0)	0
	set T(1)	10
	set T(2)	20
	set T(3)	30
	set P(0,0)	0
	set P(1,0)	1
	set P(2,0)	2
	set P(3,0)	4
	set P(0,1)	4
	set P(1,1)	2
	set P(2,1)	1
	set P(3,1)	0
#//	set pNcsSeq [NcsSeq_New T P 4 2]
	set pNcsSeq [NcsSeq_New T P 2 2]
	puts $pNcsSeq
	for {set i -10} {$i <= 40} {incr i} {
		set x [NcsSeq_GetVal $pNcsSeq $i 0]
		set y [NcsSeq_GetVal $pNcsSeq $i 1]
		puts "$i\t$x\t$y"
	}
}
#//Ncs_test
