/*
 *	clipruds.c
 *
 *	パルスモーターの台形制御シーケンサ
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2001-2014 Naoyuki Sawa
 *
 *	* Thu Apr 24 19:33:48 JST 2014 Naoyuki Sawa
 *	- 1st リリース。
 *	* Sat Apr 26 09:27:47 JST 2014 Naoyuki Sawa
 *	- シーケンサコンパイラを作成しました。⇒clip/tool/dRudSeqC/
 *	  もう命令コード定義用マクロを使ってデータを作成する必要はありません。
 *	  スクリプトを書いて、シーケンサコンパイラでデータを作成してください。
 *	* Mon May 05 19:19:03 JST 2014 Naoyuki Sawa
 *	- cliprudc.c に停止励磁時間を追加したことに伴い、「wait stop」の条件を変更しました。
 *	  [変更前] 停止中になるまで待つ。
 *	  [変更後] 停止中,かつ,停止励磁終了するまで待つ。
 */
#include "clip.h"

/****************************************************************************
 *	ローカル関数宣言
 ****************************************************************************/
static void RudSeqCh_Exec(int iRudSeqCh);

/****************************************************************************
 *	パルスモーターの台形制御シーケンサ
 ****************************************************************************/
ST_RudSeq stRudSeq;	/* 台形制御シーケンサ */
/*--------------------------------------------------------------------------*/
void RudSeq_Init(void (*fnUser)(int iRudSeqCh, int no)) {
	/* 台形制御シーケンサをクリアする。 */
	memset(&stRudSeq, 0, sizeof stRudSeq);
	/* アプリケーション定義命令処理関数を格納する。 */
	stRudSeq.fnUser = fnUser;
}
/*--------------------------------------------------------------------------*/
void RudSeqCh_Play(int iRudSeqCh, const uint16_t* pInst) {
	ST_RudSeqCh* pRudSeqCh;
	/* 台形制御シーケンサチャネルを確実に停止する。 */
	RudSeqCh_Stop(iRudSeqCh);
	/* 台形制御シーケンサチャネルを取得する。 */
	if((unsigned)iRudSeqCh > RudSeqCh_Max) { DIE(); }
	pRudSeqCh = &stRudSeq.TBL_RudSeqCh[iRudSeqCh];
	/* 次に実行する命令ポインタを格納する。 */
	pRudSeqCh->pInst = pInst;
}
/*--------------------------------------------------------------------------*/
void RudSeqCh_Stop(int iRudSeqCh) {
	ST_RudSeqCh* pRudSeqCh;
	/* 台形制御シーケンサチャネルを取得する。 */
	if((unsigned)iRudSeqCh > RudSeqCh_Max) { DIE(); }
	pRudSeqCh = &stRudSeq.TBL_RudSeqCh[iRudSeqCh];
	/* 台形制御シーケンサチャネルをクリアする。 */
	memset(pRudSeqCh, 0, sizeof(ST_RudSeqCh));
}
/*--------------------------------------------------------------------------*/
void RudSeq_Stop() {
	int iRudSeqCh;
	/* 各台形制御シーケンサチャネルについて… */
	for(iRudSeqCh = 0; iRudSeqCh <= RudSeqCh_Max; iRudSeqCh++) {
		/* 台形制御シーケンサチャネルを停止する。 */
		RudSeqCh_Stop(iRudSeqCh);
	}
}
/*--------------------------------------------------------------------------*/
void RudSeq_Exec() {
	int iRudSeqCh;
	/* 各台形制御シーケンサチャネルについて… */
	for(iRudSeqCh = 0; iRudSeqCh <= RudSeqCh_Max; iRudSeqCh++) {
		/* 台形制御シーケンサチャネルを実行する。 */
		RudSeqCh_Exec(iRudSeqCh);
	}
}
/*--------------------------------------------------------------------------*/
static void RudSeqCh_Exec(int iRudSeqCh) {
	ST_RudSeqCh* pRudSeqCh;
	int Hi, Lo;
	/* 台形制御シーケンサチャネルを取得する。 */
	if((unsigned)iRudSeqCh > RudSeqCh_Max) { DIE(); }
	pRudSeqCh = &stRudSeq.TBL_RudSeqCh[iRudSeqCh];
	/* 再生中でなければ、何もしない。 */
	if(!pRudSeqCh->pInst) { return; }
#ifdef  WIN32
//	printf("%*s""AbsTime=%u,RelTime=%u\n",pRudSeqCh->StackLevel,"",pRudSeqCh->AbsTime,pRudSeqCh->RelTime);
#endif//WIN32
	for(;;) {
		/* 命令コードを取得する。 */
		Hi = *pRudSeqCh->pInst;
		Lo = Hi & 0xFFF;	/* 命令コードの下位12bit */
		Hi >>= 12;		/* 命令コードの上位 4bit */
		/* 命令コードの上位4bitによって… */
		switch(Hi) {
		case 0:
			/* 命令コードの下位12bitによって… */
			switch(Lo) {
			case 0: /* end */
#ifdef  WIN32
				printf("%*s""end\n",pRudSeqCh->StackLevel,"");
#endif//WIN32
				/* スタックが有れば… */
				if(pRudSeqCh->StackLevel) {
					/* スタックを減らす。 */
					pRudSeqCh->StackLevel--;
					/* Repeatスタックならば… */
					if(pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].Repeat) {
						/* 無限,又は,残り繰り返し回数を減らして0でなければ… */
						if((pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].Repeat < 0) ||
						 (--pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].Repeat)) {
							/* 命令ポインタを設定する。 */
							pRudSeqCh->pInst = pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].pInst;
							/* スタックを増やす。 */
							pRudSeqCh->StackLevel++;
						}
					/* Callスタックならば… */
					} else {
						/* 命令ポインタを設定する。 */
						pRudSeqCh->pInst = pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].pInst;
					}
				/* スタックが無ければ… */
				} else {
					/* 再生終了する。 */
					RudSeqCh_Stop(iRudSeqCh);
					return; /* ここまで */
				}
				break;
			case 1: /* move init */
#ifdef  WIN32
				printf("%*s""move init\n",pRudSeqCh->StackLevel,"");
#endif//WIN32
				/* 台形制御チャネルの初期動作を実行する。 */
				RudCtlCh_MoveInit(iRudSeqCh);									//（台形制御チャネル番号＝台形制御シーケンサチャネル番号）という前提
				break;
			case 2: /* move stop */
#ifdef  WIN32
				printf("%*s""move stop\n",pRudSeqCh->StackLevel,"");
#endif//WIN32
				/* 台形制御チャネルの停止動作を実行する。 */
				RudCtlCh_Stop(iRudSeqCh);									//（台形制御チャネル番号＝台形制御シーケンサチャネル番号）という前提
				break;
			case 3: /* wait stop */
#ifdef  WIN32
//				printf("%*s""wait stop\n",pRudSeqCh->StackLevel,"");
#endif//WIN32
				/* 台形制御チャネルが動作中,又は,停止励磁中ならば… */
				if(RudCtlCh_GetSpd(iRudSeqCh) || RudCtlCh_GetStpTim(iRudSeqCh)) {				//（台形制御チャネル番号＝台形制御シーケンサチャネル番号）という前提
					goto L_EXIT; /* ここまで */
				}
				break;
			default: /* user u */
#ifdef  WIN32
				printf("%*s""user %u\n",pRudSeqCh->StackLevel,"",Lo-4);
#endif//WIN32
				/* アプリケーション定義命令を実行する。 */
				(*stRudSeq.fnUser)(iRudSeqCh, Lo-4);
				break;
			}
			break;
		case 1: /* goto */
#ifdef  WIN32
			printf("%*s""goto %d\n",pRudSeqCh->StackLevel,"",((int16_t)(Lo<<4))>>4);
#endif//WIN32
			/* 命令ポインタを設定する。 */
			if(!Lo) { DIE(); } /* データバグ */									//goto命令自身へのgotoは明らかにデータバグである。
			pRudSeqCh->pInst += (((int16_t)(Lo<<4))>>4) - 1/*共通処理の「+1」を相殺*/;
			break;
		case 2: /* call */
#ifdef  WIN32
			printf("%*s""call %d\n",pRudSeqCh->StackLevel,"",((int16_t)(Lo<<4))>>4);
#endif//WIN32
			if((unsigned)pRudSeqCh->StackLevel >= RudSeqCh_StackSize) { DIE(); }	/* RudSeqCh_StackSizeを調整せよ */
			/* 戻り先の命令ポインタを格納する。 */
			pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].pInst = pRudSeqCh->pInst;					//「〜=pRudSeqCh->pInst+1/*次の命令*/;」ではないことに注意せよ。「+1」は戻り時のendの実行後の共通処理で行われるから。
			/* Callスタックであることを示す。 */
			pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].Repeat = 0;
			/* スタックを増やす。 */
			pRudSeqCh->StackLevel++;
			/* 命令ポインタを設定する。 */
			if(!Lo) { DIE(); } /* データバグ */									//call命令自身へのcallは明らかにデータバグである。
			pRudSeqCh->pInst += (((int16_t)(Lo<<4))>>4) - 1/*共通処理の「+1」を相殺*/;
			break;
		case 3: /* repeat */
#ifdef  WIN32
			printf("%*s""repeat %u\n",pRudSeqCh->StackLevel,"",Lo);
#endif//WIN32
			if((unsigned)pRudSeqCh->StackLevel >= RudSeqCh_StackSize) { DIE(); }	/* RudSeqCh_StackSizeを調整せよ */
			/* 戻り先の命令ポインタを格納する。 */
			pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].pInst = pRudSeqCh->pInst;					//「〜=pRudSeqCh->pInst+1/*次の命令*/;」ではないことに注意せよ。「+1」は戻り時のendの実行後の共通処理で行われるから。
			/* Repeatスタックの残り繰り返し回数を格納する。 */
			pRudSeqCh->TBL_Stack[pRudSeqCh->StackLevel].Repeat = Lo ? Lo : -1/*無限*/;
			/* スタックを増やす。 */
			pRudSeqCh->StackLevel++;
			break;
		case 4: /* speed */
#ifdef  WIN32
			printf("%*s""speed %u\n",pRudSeqCh->StackLevel,"",Lo);
#endif//WIN32
			/* 分周比を設定する。 */
			pRudSeqCh->Speed = Lo;
			break;
		case 5: /* move rel */
#ifdef  WIN32
			printf("%*s""move rel %d\n",pRudSeqCh->StackLevel,"",((int16_t)(Lo<<4))>>4);
#endif//WIN32
			/* 台形制御チャネルの動作を実行する。 */
			RudCtlCh_MoveRel(iRudSeqCh, ((int16_t)(Lo<<4))>>4, pRudSeqCh->Speed);					//（台形制御チャネル番号＝台形制御シーケンサチャネル番号）という前提
			break;
		case 6: /* move abs */
#ifdef  WIN32
			printf("%*s""move abs %d\n",pRudSeqCh->StackLevel,"",((int16_t)(Lo<<4))>>4);
#endif//WIN32
			/* 台形制御チャネルの動作を実行する。 */
			RudCtlCh_MoveAbs(iRudSeqCh, ((int16_t)(Lo<<4))>>4, pRudSeqCh->Speed);					//（台形制御チャネル番号＝台形制御シーケンサチャネル番号）という前提
			break;
		case 7: /* wait rel */
#ifdef  WIN32
//			printf("%*s""wait rel %u\n",pRudSeqCh->StackLevel,"",Lo);
#endif//WIN32
			/* 直前の命令完了からの相対時間が、指定された相対時間に到達していなければ… */
			if((unsigned)pRudSeqCh->RelTime < (unsigned)Lo) {
				goto L_EXIT; /* ここまで */
			}
			break;
		default: /* wait abs */
#ifdef  WIN32
//			printf("%*s""wait abs %u\n",pRudSeqCh->StackLevel,"",Lo);
#endif//WIN32
			/* データ再生開始からの絶対時間が、指定された絶対時間に到達していなければ… */
			if((unsigned)pRudSeqCh->AbsTime < (unsigned)(((Hi&7)<<12)|Lo)) {
				goto L_EXIT; /* ここまで */
			}
			break;
		}
		/* 命令ポインタを進める。 */
		pRudSeqCh->pInst++;												//end,goto,callの直後も命令ポインタが進むことに注意せよ。
		/* 直前の命令完了からの相対時間をリセットする。 */
		pRudSeqCh->RelTime = 0;
	}
L_EXIT:	/* 次回のために、データ再生開始からの絶対時間と、直前の命令完了からの相対時間を進める。
	 * - 当関数の最初で進めるのではなく、最後で進めることに注意せよ。
	 *   もし当関数の最初で進めると、AbsTimeが0で実行される時が無く、一回早く進んでしまう。
	 */
	if(pRudSeqCh->AbsTime < USHRT_MAX) { pRudSeqCh->AbsTime++; }
	if(pRudSeqCh->RelTime < USHRT_MAX) { pRudSeqCh->RelTime++; }
}

#if 0
/****************************************************************************
 *	サンプルデータ
 ****************************************************************************/
//□ｻﾝﾌﾟﾙﾃﾞｰﾀ1
const uint16_t TBL_Inst1[]={
RudSeq_Inst_move_init,		//初期動作を行う｡
RudSeq_Inst_wait_abs(1000),	//ﾃﾞｰﾀ再生開始からの絶対時間が1秒以上になるまで待つ｡
RudSeq_Inst_speed(5),		//分周比を設定する｡これ以降のmoveに対して適用される｡
RudSeq_Inst_move_abs(100),	//絶対位置への動作を開始する｡
RudSeq_Inst_wait_stop,		//動作が終了するまで待つ｡
RudSeq_Inst_move_rel(-500),	//相対位置への動作を開始する｡
RudSeq_Inst_wait_rel(200),	//直前の命令完了からの相対時間が200ﾐﾘ秒以上になるまで待つ｡
RudSeq_Inst_move_stop,		//(動作が完了していなければ､)停止動作を開始する｡
RudSeq_Inst_wait_abs(5000),	//ﾃﾞｰﾀ再生開始からの絶対時間が5秒以上になるまで待つ｡
RudSeq_Inst_move_abs(0),	//原点位置への動作を開始する｡
RudSeq_Inst_end,		//再生終了する｡(動作が完了していなければ、動作は継続する｡)
};
//↓実行結果
//move init
//speed 5
//move abs 100
//move rel -500
//move stop
//move abs 0
//end
/*--------------------------------------------------------------------------*/
//□ｻﾝﾌﾟﾙﾃﾞｰﾀ2
const uint16_t TBL_Inst2[]={
RudSeq_Inst_goto(2),		//この命令の位置を基準とした相対位置で､+2の命令へ分岐する｡
RudSeq_Inst_move_init,		//この命令は実行されない｡
//gotoの飛び先
RudSeq_Inst_repeat(2),		//2回繰り返しを開始する｡
RudSeq_Inst_call(3),		//この命令の位置を基準とした相対位置で､+3の命令を呼び出す｡
RudSeq_Inst_end,		//2回繰り返す｡このendは､repeatに対応するendである｡
RudSeq_Inst_end,		//再生終了する｡(動作が完了していなければ、動作は継続する｡)
//callの呼び先
RudSeq_Inst_move_rel(10),	//相対位置への動作を開始する｡
RudSeq_Inst_wait_stop,		//動作が終了するまで待つ｡
RudSeq_Inst_move_rel(-10),	//相対位置への動作を開始する｡
RudSeq_Inst_wait_stop,		//動作が終了するまで待つ｡
RudSeq_Inst_end,		//呼び出し元へ戻る｡このendは､callに対応するendである｡
};
//↓実行結果
//goto 2
//repeat 2
// call 3
//  move rel 10
//  move rel -10
//  end
// end
// call 3
//  move rel 10
//  move rel -10
//  end
// end
//end
/*--------------------------------------------------------------------------*/
#endif
