/* System	: 二足歩行ロボット(walk)
* File		: system.c
* function	: システム状態制御
* Version	: 1.0
* Date		: 2019/5/7
* CPU		: RL78/G12 ROM:16KB RAM:1.5KB fCLK:24MHz
* IDE		: CS+ for CC V6.01.00 [01 Dec 2017]
* Programmer	: Hajime Sugiyama
* Note		: */

#include "switch.h"
#include "led.h"
#include "serial.h"
#include "sequence.h"
#include "sequence_data.h"
#include "system.h"
#include "vb_detect.h"
#include "vb_control.h"

/* 内蔵プログラム動作モード,受信プログラム動作モード,パソコン操作モード,電池電圧異常モード */
enum OP_MODE{BUILTIN_PG,RX_PG,PC_OP,VB_FAULT};
static enum OP_MODE e_operation_mode = BUILTIN_PG;

/* 内蔵プログラム動作モードのコンポジット状態 */
/* 内蔵プログラム停止状態,内蔵プログラム実行状態 */
enum BUILTIN_MODE{BUILTIN_STP,BUILTIN_EXE};
static enum BUILTIN_MODE e_builtin_mode = BUILTIN_STP;

/* パソコン操作モードのコンポジット状態 */
/* パソコン操作停止状態,パソコン操作実行状態 */
enum PC_MODE{PC_STP,PC_EXE};
static enum PC_MODE e_pc_mode = PC_STP;

/* 受信プログラム動作モードのコンポジット状態 */
/* 受信プログラム停止状態,転送プログラム実行状態 */
enum RX_MODE{RX_STP,RX_EXE};
static enum RX_MODE e_rx_mode = RX_STP;

static unsigned int sequence_counter = 0;			/* 歩行動作シーケンス番号(up_counter) */

static void execute_pc_mode(void);				/* パソコン操作実行処理 */
static void shift_mode(enum OP_MODE to_mode);			/* 動作モード変更処理 */
static void execute_sequence_data(struct SEQDATA *seq_data);	/* 歩行動作順序データ実行処理 */

/* システム状態制御処理ユニット */
void system_control(void){
	switch(e_operation_mode){				/* 動作モード */
		case BUILTIN_PG:				/* 内蔵プログラム動作モード */
			if(f_vb_alarm.abnormal == 1U){		/* 電池電圧異常 (BD3,4)*/
				shift_mode(VB_FAULT);		/* 電池電圧異常モードへの変更 */
			}else if(e_mode_event == BUILTIN_EVM){	/* 内蔵プログラム動作モードへの変更要求 */
				shift_mode(BUILTIN_PG);		/* 内蔵プログラム動作モードへの変更 */
			}else if(e_mode_event == RX_EVM){	/* 受信プログラム動作モードへの変更要求 */
				shift_mode(RX_PG);		/* 受信プログラム動作モードへの変更 */
			}else if(e_mode_event == PC_EVM){	/* パソコン操作モードへの変更要求 */
				shift_mode(PC_OP);		/* パソコン操作モードへの変更 */
			}else{
				switch(e_builtin_mode){		/* 内蔵プログラム動作状態 */
					case BUILTIN_STP:	/* 内蔵プログラム停止状態 */
					default:
						if(f_s.complete == 1U){	/* 歩行動作シーケンス完了 */
							if(f_switch.start_stop == 1U){	/* スタート・ストップボタン押下 */
								f_switch.start_stop = 0U;
								f_serial.mode_changeable = 0U;/* 動作モード変更否 */
								sequence_counter = 0U;	/* シーケンス番号初期化 */
								e_builtin_mode = BUILTIN_EXE;/* 内蔵プログラム実行状態へ */
							}else{	/* 歩行シーケンス完了 */
								f_serial.mode_changeable = 1U;/* 動作モード変更可 */
								led_out = LED_OFF;
							}
						}
						break;
					case BUILTIN_EXE:	/* 内蔵プログラム実行状態 */
						if(f_switch.start_stop == 1U){	/* スタート・ストップボタン押下 */
							f_switch.start_stop = 0U;
							walking_order_event = STOP_EVENT;/* 歩行動作指示：停止イベント発行 */
							f_s.complete = 0U;
							s16_count_target_request = STOP_COUNTING;/* 歩数カウントを利用しない */
							e_builtin_mode = BUILTIN_STP;/* 内蔵プログラム停止状態へ */
						}else if(f_s.complete == 1U){	/* 歩行動作シーケンス完了 */
							f_s.complete = 0U;
							execute_sequence_data((struct SEQDATA *)builtin_data);
						}
						led_out = LED_PULSE;/* LEDパルス点灯 */
						break;
			}
			}
			break;
		case RX_PG:					/* 受信プログラム動作モード */
			if(f_vb_alarm.abnormal == 1U){		/* 電池電圧異常 (BD3,4) */
				shift_mode(VB_FAULT);		/* 電池電圧異常モードへの変更 */
			}else if(e_mode_event == BUILTIN_EVM){	/* 内蔵プログラム動作モードへの変更要求 */
				shift_mode(BUILTIN_PG);		/* 内蔵プログラム動作モードへの変更 */
			}else if(e_mode_event == RX_EVM){	/* 受信プログラム動作モードへの変更要求 */
				shift_mode(RX_PG);		/* 受信プログラム動作モードへの変更 */
			}else if(e_mode_event == PC_EVM){	/* パソコン操作モードへの変更要求 */
				shift_mode(PC_OP);		/* パソコン操作モードへの変更 */
			}else{
				switch(e_rx_mode){
					case RX_STP:		/* 受信プログラム停止状態 */
					default:
						if(f_s.complete == 1U){	/* 歩行動作シーケンス完了 */
							if(f_switch.start_stop == 1U){	/* スタート・ストップボタン押下 */
								f_switch.start_stop = 0U;
								f_serial.mode_changeable = 0U;/* 動作モード変更否 */
								sequence_counter = 0U;	/* シーケンス番号初期化 */
								get_sequence_data();	/* 受信歩行動作順序データの確定 */
								e_rx_mode = RX_EXE;	/* 受信プログラム実行状態へ */
							}else {	/* 歩行動作シーケンス完了 */
								f_serial.mode_changeable = 1U;/* 動作モード変更可 */
								led_out = LED_OFF;	/* LED消灯 */
							}
						}
						break;
					case RX_EXE:		/* 受信プログラム実行状態 */
						if(f_switch.start_stop == 1U){/* スタート・ストップボタン押下 */
							f_switch.start_stop = 0U;
							walking_order_event = STOP_EVENT;/* 歩行動作指示：停止イベント発行 */
							f_s.complete = 0U;
							s16_count_target_request = STOP_COUNTING;/* 歩数カウントを利用しない */
							e_rx_mode = RX_STP;	/* 受信プログラム停止状態へ */
						}else if(f_s.complete == 1U){	/* 歩行動作シーケンス完了（停止中） */
							f_s.complete = 0U;
							execute_sequence_data(sequence_data);
						}
						led_out = LED_PULSE;	/* LEDパルス点灯 */
						break;
				}
			}
			break;
		case PC_OP:					/* パソコン操作モード */
			if(f_vb_alarm.abnormal == 1U){		/* 電池電圧異常 (BD3,4) */
				shift_mode(VB_FAULT);		/* 電池電圧異常モードへの変更要求 */
			}else if(e_mode_event == RX_EVM){	/* 受信プログラム動作モードへの変更要求 */
				shift_mode(RX_PG);		/* 受信プログラム動作モードへの変更 */
			}else if(e_mode_event == BUILTIN_EVM){	/* 内蔵プログラム動作モードへの変更要求 */
				shift_mode(BUILTIN_PG);		/* 内蔵プログラム動作モードへの変更 */
			}else if(e_mode_event == PC_EVM){	/* パソコン操作モードへの変更要求 */
				shift_mode(PC_OP);		/* パソコン操作モードへの変更 */
			}else{					/* パソコン操作モード */
				switch(e_pc_mode){
					case PC_STP:		/* パソコン操作停止状態 */
					default:
						f_switch.start_stop = 0U;	/* スタート・ストップボタン押下無効 */
						if(f_s.complete == 1U){		/* 歩行動作シーケンス完了 */
							if((e_walk_event != NO_EVW) && (e_walk_event != STOP_EVW)){	/* 歩行動作開始指示有 */
								f_s.complete = 0U;
								f_serial.mode_changeable = 0U;/* 動作モード変更否 */
								e_pc_mode = PC_EXE;/* パソコン操作実行状態へ */
							}else{
								f_serial.mode_changeable = 1U;/* 動作モード変更可 */
								e_walk_event = NO_EVW;	/* 歩行動作指示：停止は消去する */
								led_out = LED_OFF;
							}
						}
						break;
					case PC_EXE:		/* パソコン操作実行状態 */
						/* 停止優先 */
						if(f_switch.start_stop == 1U){	/* スタート・ストップボタン押下 */
							f_switch.start_stop = 0U;
							e_walk_event = NO_EVW;
							walking_order_event = STOP_EVENT;/* 歩行動作指示：停止イベント発行 */
							f_s.complete = 0U;
							e_pc_mode = PC_STP;	/* パソコン操作停止状態へ */
						}else{
							execute_pc_mode();
						}
						led_out = LED_PULSE;	/* LEDパルス点灯 */
						break;
				}
			}
			break;
		case VB_FAULT:					/* 電池電圧異常モード (BD3,4) */
		default:
			if((f_vb_alarm.high == 0U) && (f_vb_alarm.low == 0)){/* 電池電圧正常 */
			   if(f_switch.start_stop == 1U){/* スタート・ストップボタン押下 */
					f_switch.start_stop = 0U;
					f_vb_alarm.abnormal = 0U;	/* 電池電圧異常消去 */
					f_serial.respose_pending = 0;	/* 肯定応答未送信消去 */
					f_vb_control.supply = VB_ON;	/* モータ電源供給 */
					f_serial.mode_changeable = 1U;	/* 動作モード変更可能 */
					shift_mode(BUILTIN_PG);	/* 内蔵プログラム動作モードへの復帰 */
				}
			}else{					/* 電池電圧異常 */
				f_switch.start_stop = 0U;	/* スタート・ストップボタン押下消去 */
				f_vb_control.supply = VB_OFF;	/* モータ電源遮断 */
			}
				led_out = LED_BLINK;		/* LED速い点滅 */
			break;
	}
}

/* 動作モード変更処理ユニット */
static void shift_mode(enum OP_MODE to_mode){
	struct _F_SYSTEM{
    	unsigned char stop_event_issued:1;			/* 停止イベント発行済み */
    	unsigned char bit1_7:7;
	};
	static struct _F_SYSTEM f_system = {0,0};		/* 停止イベント発行済みフラグ */

	/* 歩行停止状態 */
	if(f_s.complete == 1U){
		e_mode_event = NO_EVM;				/* 動作モード変更指示消去 */
		switch(to_mode){				/* 変更先モード */
			case BUILTIN_PG:
				e_operation_mode = BUILTIN_PG;	/* 内蔵プログラム動作モードの */
				e_builtin_mode = BUILTIN_STP;	/* 内蔵プログラム停止状態へ */
				f_serial.walking_motion_changeable = 0U;/* パソコンからの歩行動作変更否 */
				break;
			case RX_PG:
				e_operation_mode = RX_PG;	/* 転送プログラム動作モードの */
				e_rx_mode = RX_STP;		/* 転送プログラム停止状態へ */
				f_serial.walking_motion_changeable = 0U;/* 歩行動作変更否 */
				break;
			case PC_OP:
				e_operation_mode = PC_OP;	/* パソコン操作モードへ */
				e_pc_mode = PC_STP;		/* 転送プログラム停止状態へ */
				f_serial.walking_motion_changeable = 1U;/* 歩行動作変更可 */
				break;
			case VB_FAULT:				/* (BD3,4) */
				e_operation_mode = VB_FAULT;	/* 電池電圧異常モードへ */
				f_serial.mode_changeable = 0;	/* 動作モード変更否 */
				f_serial.walking_motion_changeable = 0;	/* 歩行動作変更否 */
				f_serial.respose_pending = 0;	/* 肯定応答未送信消去 */
				break;
		}
		f_switch.start_stop = 0U;			/* パソコン操作モード中のスイッチ操作をクリア */
		walking_order_event_memory = NO_EVENT;		/* 歩行動作指示イベント記憶消去 */
		s16_count_target_request = STOP_COUNTING;	/* 目標歩数・停止時間カウントを停止へ */
		f_system.stop_event_issued = 0U;		/* モード変更時の停止イベント発行済みフラグクリア */
		if(f_serial.respose_pending == 1U){		/* 肯定応答保留 */
			f_serial.respose_pending = 0U;		/*  */
			uart0_send_mode_changed();		/* 肯定応答送信（モード変更完了） */
		}
	}else if(f_system.stop_event_issued == 0U){		/* モード変更時の歩行動作指示：停止イベントは１回のみ発行 */
		f_system.stop_event_issued = 1U;
		walking_order_event = STOP_EVENT;		/* 歩行動作指示：停止イベント発行 */
		f_s.complete = 0U;				/* 歩行動作シーケンス完了フラグクリア */
		e_builtin_mode = BUILTIN_STP;			/* 内蔵プログラム停止状態へ */
		e_rx_mode = RX_STP;				/* 受信プログラム停止状態へ */
		e_pc_mode = PC_STP;				/* パソコン操作停止状態へ */
		s16_count_target_request = STOP_COUNTING;	/* 要求目標歩数・停止時間カウントを利用しない設定へ */
	}
}

/* パソコン操作実行処理ユニット */
static void execute_pc_mode(void){
	/* PCから受信した歩行動作指示に対応するイベントを発行する */
	switch(e_walk_event){
		case FORWARD_EVW:				/* 前進指示 */
			walking_order_event = FORWARD_EVENT;/* 前進イベント発行 */
			break;
		case RIGHT_EVW:					/* 右回転指示 */
			walking_order_event = RIGHT_EVENT;/* 右回転イベント発行 */
			break;
		case BACKWARD_EVW:				/* 後退指示 */
			walking_order_event = BACKWARD_EVENT;/* 後退イベント発行 */
			break;
		case LEFT_EVW:					/* 左回転指示 */
			walking_order_event = LEFT_EVENT;/* 左回転イベント発行 */
			break;
		case DANCE_EVW:					/* ダンス指示 */
			walking_order_event = DANCE_EVENT;/* ダンスイベント発行 */
			break;
		case NO_EVW:					/* 指示なし */
			break;
		case STOP_EVW:					/* 停止指示 */
		/* 指示の異常時も停止状態へ移行するため break 省略 */
		default:					/* 異常時の処置（停止） */
			walking_order_event = STOP_EVENT;	/* 歩行停止イベント発行 */
			e_pc_mode = PC_STP;			/* パソコン操作停止へ */
			break;
	}
	e_walk_event = NO_EVW;
}

/* 歩行動作順序データ実行処理ユニット */
static void execute_sequence_data(struct SEQDATA *seq_data){
	if(sequence_counter >= SQ_NO_MAX){			/* 内蔵プログラムシーケンスカウンタ上限 */
		walking_order_event = STOP_EVENT;		/* 歩行動作指示：停止イベント発行 */
		s16_count_target_request = STOP_COUNTING;	/* 要求目標歩数カウントを利用しない設定へ */
		e_builtin_mode = BUILTIN_STP;			/* 内蔵プログラム停止状態へ */
		e_rx_mode = RX_STP;				/* 受信プログラム停止状態へ */
	}else{	/* シーケンスデータを参照し、歩行動作イベントを発行し繰返し回数を設定する */
		switch(seq_data[sequence_counter].category){
			case 0x01:				/* 前進 */
				walking_order_event = FORWARD_EVENT;
				s16_count_target_request = seq_data[sequence_counter].count;
				break;
			case 0x02:				/* 右回転 */
				walking_order_event = RIGHT_EVENT;
				s16_count_target_request = seq_data[sequence_counter].count;
				break;
			case 0x04:				/* 後退 */
				walking_order_event = BACKWARD_EVENT;
				s16_count_target_request = -(signed int)seq_data[sequence_counter].count;
				break;
			case 0x08:				/* 左回転 */
				walking_order_event = LEFT_EVENT;
				s16_count_target_request = -(signed int)seq_data[sequence_counter].count;
				break;
			case 0x06:				/* ダンス */
				walking_order_event = DANCE_EVENT;
				s16_count_target_request = seq_data[sequence_counter].count;
				break;
			case 0x0F:				/* 停止 */
				walking_order_event = STOP_EVENT;
				s16_count_target_request = 25 * (signed int)(seq_data[sequence_counter].count);
				break;
			case 0x00:				/* シーケンス終了 */
			default:
				walking_order_event = STOP_EVENT;/* 歩行動作指示：停止イベント発行 */
				s16_count_target_request = STOP_COUNTING;/* 歩数カウントを利用しない */
				e_builtin_mode = BUILTIN_STP;	/* 内蔵プログラム停止状態へ */
				e_rx_mode = RX_STP;		/* 受信プログラム停止状態へ */
				break;
		}
		sequence_counter++;				/* 内蔵プログラムシーケンスカウンタ計数 */
	}
}
