#include "Os.h"
#include "t_syslog.h"
#include "t_stdlib.h"
#include "sysmod/serial.h"
#include "sysmod/syslog.h"
#include "rc_car.h"

#include "sysmod/banner.h"
#include "target_sysmod.h"
#include "target_serial.h"
#include "prc_sil.h"

#include "de0_nano.h"
#include "comuart.h"
#include "nces_can.h"
#include "rcb3.h"

/*

○仕様

・コマンド
  SBXBTの PS3コントローラ/USBゲームパッドUART変換ファームウェア
  (http://runningele.web.fc2.com/) でサポートしているRCB3に対応
  している．
  
・起動時
  全てのLEDを0.5秒周期で3秒点滅させる．

・ボタン操舵
  左・右 ボタン
   ・ステアリングのニュートラルを調整．
   ・ボタンを押すと方向指示器がON，離すとOFF
  上ボタン
   ・ドライブにする．
  下ボタン
   ・リバースににする．
  L1/R1
   ・方向指示器をON/OFFする．
  L2/R2
   ・モータ制御のゲインを変更
  ×
   ・ブレーキ
  △
   ・ハザード
  ○
   ・メインヘッドライトON/OFF
  □
   ・ニュートラルにする．
 */

/*
 ボディ系コマンドCANメッセージ仕様

 送信周期     : 50ms
 メッセージID : 16 
 メッセージ長 : 11bit

                  offset  bits   bit assign
  ヘッドライト1  :  0  :   1   : 1:ON 0:OFF 
  ヘッドライト2  :  1  :   1   : 1:ON 0:OFF 
  ヘッドライト3  :  2  :   1   : 1:ON 0:OFF  
  ブレーキランプ :  3  :   1   : 1:ON 0:OFF  
  バックランプ   :  4  :   1   : 1:ON 0:OFF
  ブザー         :  5  :   1   : 1:ON 0:OFF 2:BLINK
  ウィンカーL    :  8  :   2   : 1:ON 0:OFF 2:BLINK
  ウィンカーR    : 10  :   2   : 1:ON 0:OFF 2:BLINK
  ハザードランプ : 12  :   1   : 1:ON 0:OFF 2:BLINK


  制御系コマンドCANメッセージ仕様

 送信周期     : メッセージ到達毎
 メッセージID : 3
 メッセージ長 : 64bit
  
 */

/*
 *  各種パラメータ定義
 */

/*
 *  CAN関連 : ボディ系コマンド用CANメッセージID
 */
#define BODY_CAN_MSG_ID  16

/*
 *  CAN関連 : 制御系コマンド用CANメッセージID
 */
#define CONT_CMD_CAN_MSG_ID    3

/*
 *  CAN関連 : ボディ系コマンド用CAN送受信メールボックスID
 */
#define BODY_CAN_MSG_RMB  1
#define BODY_CAN_MSG_TMB  1

/*
 *  CAN関連 : 制御系コマンド用CANメッセージ用CAN送受信メールボックスID
 */
#define CONT_CMD_CAN_MSG_RMB  0
#define CONT_CMD_CAN_MSG_TMB  0


/*
 *  制御系 : ニュートラル時のステアリング角度の初期値
 */
#define STEER_NEUTRAL_INIT   0

/*
 *  制御系 : 起動時のボディー系の接続テスト用設定
 */
#define INIT_BODY_BLINK_COUNT      3 
#define INIT_BODY_BLINK_CYCLE_MS 500

/*
 *  制御系 : 起動周期
 */
#define RC_CONTRL_TASK_CYCLE_MS  10

/*
 *  制御系 : CAN送信周期
 */
#define BODY_CAN_SEND_CYCLE_MS   500


#define SPEED_GAIN_INIT 10
#define SPEED_GAIN_MIN   1
#define SPEED_GAIN_MAX  20

/*
 *  未整理
 */

/*
 *  内部データバッファ
 */
static volatile uint8		command_tbl[8];         /* コマンド引渡しテーブル */

/*
 *  ファイル名，行番号の参照用の変数
 */
extern const char8	*fatal_file_name;   /* ファイル名 */
extern sint32		fatal_line_num;     /* 行番号 */

/*
 *  APIエラーログマクロ
 *
 *  ErrorHookが有効の場合はErrorHookから
 *  エラーログを出力し, ErrorHookが無効の場合は
 *  以下のマクロよりエラーログ出力を行う
 */
#if defined(CFG_USE_ERRORHOOK)
#define error_log(api)	(api)
#else /* !defined( CFG_USE_ERRORHOOK ) */
#define	error_log(api)										   \
	{														   \
		StatusType ercd;									   \
		ercd = api;     /* 各API実行 */						   \
		if (ercd != E_OK) {									   \
			syslog(LOG_INFO, "Error:%d", atk2_strerror(ercd)); \
		}													   \
	}
#endif /* defined( CFG_USE_ERRORHOOK ) */

void
set_led(uint8 pattern){
	sil_wrw_iop((void *)LED_PIO_BASE, pattern);
}

uint8
get_dipsw(void){
	return (uint8)sil_rew_iop((void *)DIPSW_PIO_BASE);
}

volatile boolean IsBodyOnECU = FALSE;
volatile boolean IsOpeOnECU  = FALSE;
volatile boolean IsCntOnECU  = FALSE;

void
appmode_error(void){
	volatile int i;
	uint8 pattern = 0;
	while(1){
		for(i = 0; i < 1000000; i++);
		set_led(pattern);
		pattern = ~pattern;
	}
}

/*
 *  ユーザメイン関数
 *
 *  アプリケーションモードの判断と，カーネル起動
 */
sint32
main(void)
{
	AppModeType	crt_app_mode;
	uint8 dipsw;

	crt_app_mode = AppMode_ALL;

	dipsw = get_dipsw();

	switch (dipsw & 0x7) {
	  case 0x00:
		appmode_error();
		break;
	  case 0x01:
		crt_app_mode = AppMode_Body;
		IsBodyOnECU = TRUE;
		break;
	  case 0x02:
		crt_app_mode = AppMode_Cnt;
		IsCntOnECU = TRUE;
		break;
	  case 0x04:
		crt_app_mode = AppMode_Ope;
		IsOpeOnECU = TRUE;
		break;
	  case 0x01|0x02:
		crt_app_mode = AppMode_Body_Cnt;
		IsBodyOnECU = TRUE;
		IsCntOnECU = TRUE;
		break;
	  case 0x01|0x04:
		crt_app_mode = AppMode_Body_Ope;
		IsBodyOnECU = TRUE;
		IsOpeOnECU = TRUE;
		break;
	  case 0x02|0x04:
		crt_app_mode = AppMode_Cnt_Ope;
		IsCntOnECU = TRUE;
		IsOpeOnECU = TRUE;
		break;
	  case 0x07:
		crt_app_mode = AppMode_ALL;
		IsBodyOnECU = TRUE;
		IsCntOnECU  = TRUE;
		IsOpeOnECU  = TRUE;
		break;
	  default:
		appmode_error();
		break;
	}

	/*
	 *  カーネル起動
	 */
	StartOS(crt_app_mode);

	while (1) {
	}
}

//////////////////////////////////////////////////////////////////////////
/*
 *   
 *  ボディー制御関連
 * 
 */

/*
 *  ボディーECUへの状態指示構造体
 */
typedef struct {
	uint8 headlight1;
	uint8 headlight2;
	uint8 headlight3;
	uint8 breaklamp;
	uint8 backlamp;
	uint8 buzzer;
	uint8 blinker_l;
	uint8 blinker_r;
	uint8 hazardlamp;
}BODY_CONT_INFO;

/*
 *  ボディーのアイテムのON/OFFマクロ
 */
#define BODY_CONT_ON     1
#define BODY_CONT_OFF    0
#define BODY_CONT_BLINK  2

/*
 *  ボディーECUへの状態指示用変数
 */
volatile BODY_CONT_INFO g_body_cont_info;

void
set_tlu01(unsigned int pattern){
	sil_wrw_iop((void *)TLU01_PIO_BASE, pattern);
}

unsigned int
get_tlu01(void){
	return (sil_rew_iop((void *)TLU01_PIO_BASE));
}

/*
 *  tlu01_set() で指示する値
 */
#define BLINKER_L_NO    0
#define BLINKER_R_NO    1
#define BREAK_LAMP_NO   2
#define HEAD_LAMP1_NO   3
#define HEAD_LAMP2_NO   4  /* 未実装 */
#define HEAD_LAMP3_NO   5  /* 未実装 */
#define BACK_LAMP_NO    6
#define BUZZER_NO       7

#define TLU01_PORT_MIN 0
#define TLU01_PORT_MAX 7

static const uint8 tlu01_pinfo[8] ={
	0x01,
	0x40,
	0x08|0x20, //BREAK
	0x04|0x10,  
	0x10,
	0x00,
	0x80,
	0x02, //BUZZER
};

void
tlu01_set(uint8 no, boolean on)
{
	uint32 wk;
	uint16 pattern;

	if (no > TLU01_PORT_MAX) {
		return;
	}

	pattern = tlu01_pinfo[no - 1];

	wk = get_tlu01();
	if (on) {
		wk |=  pattern;
	}
	else {
		wk &= ~pattern;
	}
	set_tlu01(wk);
}


/*
 *  TLU01との接続の初期化
 */
void
tlu01_init(void)
{
	/* 全消灯 */
	sil_wrw_iop((void *)TLU01_PIO_BASE, 0x00);
}

#define BLINK_CYCLE_MS 500

volatile int buzzer_state = 0;

void
UnpackBodyContCanMsg(uint8 *p_rx_data)
{
	g_body_cont_info.headlight1 = (p_rx_data[0] & 0x80)? BODY_CONT_ON : BODY_CONT_OFF;
	g_body_cont_info.headlight2 = (p_rx_data[0] & 0x40)? BODY_CONT_ON : BODY_CONT_OFF;
	g_body_cont_info.headlight3 = (p_rx_data[0] & 0x20)? BODY_CONT_ON : BODY_CONT_OFF;
	g_body_cont_info.breaklamp  = (p_rx_data[0] & 0x10)? BODY_CONT_ON : BODY_CONT_OFF;
	g_body_cont_info.backlamp   = (p_rx_data[0] & 0x08)? BODY_CONT_ON : BODY_CONT_OFF;

	g_body_cont_info.buzzer     = (p_rx_data[0] & 0x06) >> 1;
	g_body_cont_info.blinker_l  = (p_rx_data[1] & 0xc0) >> 6;
	g_body_cont_info.blinker_r  = (p_rx_data[1] & 0x30) >> 4;
	g_body_cont_info.hazardlamp = (p_rx_data[1] & 0x08)? BODY_CONT_ON : BODY_CONT_OFF;
}

TASK(RCBodyTask){
	int cnt = 0;
	uint32 pattern;
	boolean blink_on = FALSE;
	uint8 rx_data[8];
	uint8 rx_size;

	syslog(LOG_NOTICE, "RCBodyTask : Start!");

	tlu01_init();

	while(1){
		WaitEvent(RCBodyEvt);     /* 10msの作業時間待ち */
		ClearEvent(RCBodyEvt);

		/*
		 *  制御系が別ECUならCANからメッセージを取得する．
		 */
		if (!IsCntOnECU) {
			NcanGetRxData(0, BODY_CAN_MSG_RMB, &rx_data[0], &rx_size );
			if (rx_size != 0) {
				/* 受信メッセージの解析 */
				UnpackBodyContCanMsg(rx_data);
			}
		}

		pattern = 0;

		/* ON/OFF処理 */
		pattern |= (g_body_cont_info.headlight1 == BODY_CONT_ON)? tlu01_pinfo[HEAD_LAMP1_NO] : 0x00;
		pattern |= (g_body_cont_info.breaklamp  == BODY_CONT_ON)? tlu01_pinfo[BREAK_LAMP_NO] : 0x00;
		pattern |= (g_body_cont_info.backlamp   == BODY_CONT_ON)? tlu01_pinfo[BACK_LAMP_NO] : 0x00;
		if(g_body_cont_info.hazardlamp == BODY_CONT_OFF){
			pattern |= (g_body_cont_info.blinker_l  == BODY_CONT_ON)? tlu01_pinfo[BLINKER_L_NO]  : 0x00;
			pattern |= (g_body_cont_info.blinker_r  == BODY_CONT_ON)? tlu01_pinfo[BLINKER_R_NO]  : 0x00;
		}

		if(blink_on == TRUE){
			/* 点滅処理 */
			if (g_body_cont_info.hazardlamp == BODY_CONT_ON){
				pattern |= tlu01_pinfo[BLINKER_L_NO]|tlu01_pinfo[BLINKER_R_NO];
			}else{
				if (g_body_cont_info.blinker_l == BODY_CONT_BLINK){
					pattern |= tlu01_pinfo[BLINKER_L_NO];
				}
				if (g_body_cont_info.blinker_r == BODY_CONT_BLINK){
					pattern |= tlu01_pinfo[BLINKER_R_NO];
				}
			}
		}

		if(cnt++ == BLINK_CYCLE_MS/10){
			cnt = 0;
			blink_on = (blink_on)? FALSE : TRUE;
		}

		if(buzzer_state == 1){
			set_tlu01(pattern|tlu01_pinfo[BUZZER_NO]);
		}
		else{
			set_tlu01(pattern);
		}
	}
}


ALARMCALLBACK(BuzzerCycAlmCb)
{
	static uint8 buzzer_on = 0;
	static uint8 pre_buzzer = BODY_CONT_OFF;
	static uint32 cnt = 0;

	if (pre_buzzer != g_body_cont_info.buzzer){
		pre_buzzer = g_body_cont_info.buzzer;
		cnt = 0;
	}

	if(g_body_cont_info.buzzer == BODY_CONT_ON){
		cnt++;
		if (cnt < 500) {
			if(buzzer_on == 0){
				buzzer_on = 1;
				buzzer_state = 1;
				set_tlu01(get_tlu01() | tlu01_pinfo[BUZZER_NO]);
			}else{
				buzzer_on = 0;
				buzzer_state = 0;
				set_tlu01(get_tlu01() & ~tlu01_pinfo[BUZZER_NO]);
			}
		}
		if(cnt > 1000){
			cnt = 0;
		}
	}
	
}


///////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 *  操作系関連
 */

/*
 *  制御系へのコマンド更新通知用変数(同一ECU時)
 */
volatile boolean UpDateCntrolCmd = FALSE;

/*
 *  操作タスク
 */
TASK(RCOperatorTask)
{
	unsigned char c;
	syslog(LOG_INFO, "RCOperatorTask : Start!");

	comuart_init();

	while(1){
		WaitCOMUartEvent();
		while(comuart_receive(&c, 1)) {
			if (rcb3_AddReceivedByte(c)) {
				UpDateCntrolCmd = TRUE;
//				syslog(LOG_NOTICE, "%d,%d,%d,%d,%d,%d", g_rcb3_rmsg[1], g_rcb3_rmsg[2], g_rcb3_rmsg[3], g_rcb3_rmsg[4], g_rcb3_rmsg[5], g_rcb3_rmsg[6]);
				/*
				 *  RCControlTaskが別ECUの場合はCANで送信
				 */
				if (!IsCntOnECU) {
					NcanSetTxData(0, CONT_CMD_CAN_MSG_TMB, CONT_CMD_CAN_MSG_ID, &g_rcb3_rmsg[0], RCB3_MSG_LEN);
				}
			}
		}
	}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 *
 *  制御系関連
 *  
 */


float g_steer_neutral = STEER_NEUTRAL_INIT;
float g_angle = 0;

typedef enum{
	SHIFT_N,
	SHIFT_D,
	SHIFT_R,
}SHIFT_STATE;

/*
 *  制御関連の変数
 */
volatile boolean  g_blinker_l_blink = FALSE;
volatile boolean  g_blinker_r_blink = FALSE;
volatile boolean  g_isBrake = FALSE;
volatile int      g_accelerator = 0;
volatile int      g_speed_gain  = SPEED_GAIN_INIT;
volatile SHIFT_STATE g_ShiftState = SHIFT_N;

/*
 *  PS3コントローラのアナログボタンのパラメータ
 */
#define PS3ANALOG_MAX  63
#define PS3ANALOG_MIN -64


void
AdjustSteerNeturalL(void)
{
	if(g_steer_neutral < 30) {
		g_steer_neutral++;
		g_angle++;
		syslog(LOG_NOTICE, "Adjust steer neutral = %d.%d", (int)g_steer_neutral, (int)(g_steer_neutral*100) % 100);
	}
}

/*
 *  ステアリングニュートラルを右側に
 */
void
AdjustSteerNeturalR(void)
{
	if(g_steer_neutral > -30) {
		g_steer_neutral--;
		g_angle--;
		syslog(LOG_NOTICE, "Adjust steer neutral = %d.%d", (int)g_steer_neutral, (int)(g_steer_neutral*100) % 100);
	}
}

/*
 *  左アナログコントローラの左右動作
 *  ステアリングを操作 
 */
void
onPS3AnalogL_LR(void)
{
	float angle;
	float cur_analog = p_g_cur_ps3analog[NO_PS3ANALOG_L_LR];

	if ((cur_analog <= PS3ANALOG_MAX) && (cur_analog >= PS3ANALOG_MIN)) {
		/* 左 30 から 右 -30 範囲 */
		angle = -(cur_analog/2);
		angle += g_steer_neutral;
		if (angle != g_angle) {
			g_angle = angle;
			if(g_angle > 0){
				syslog(LOG_NOTICE, "Steer angle = %d.%d", (int)g_angle, (int)(g_angle*100) % 100);
			}
			else {
				syslog(LOG_NOTICE, "Steer angle = %d.%d", (int)g_angle, (int)(-g_angle*100) % 100);
			}
		}
	}
}

/*
 *  左アナログコントローラの上下動作
 */
void
onPS3AnalogL_UD(void)
{

}

/*
 *  右アナログコントローラの左右動作
 */
void
onPS3AnalogR_LR(void)
{

}

/*
 *  右アナログコントローラの上下動作
 *  上 - 方向
 *  下 + 方向   
 */
void
onPS3AnalogR_UD(void)
{
	float cur_analog = p_g_cur_ps3analog[NO_PS3ANALOG_R_UD];
	g_accelerator = cur_analog;

	if(cur_analog <= 0){
		g_body_cont_info.backlamp = BODY_CONT_OFF;
		g_body_cont_info.buzzer   = BODY_CONT_OFF;
	}
	else {
//		g_accelerator = -cur_analog;
//		syslog(LOG_NOTICE, "Curanalog = %d.%d", (int)cur_analog, (int)(-cur_analog*100) % 100);
		g_body_cont_info.backlamp = BODY_CONT_ON;
		g_body_cont_info.buzzer   = BODY_CONT_ON;
	}
}

/*
 *  PS3コントローラのデジタルボタン毎の処理
 */

#define TOGGLE_ON_OFF(item) item = (item == BODY_CONT_ON)?  BODY_CONT_OFF : BODY_CONT_ON;

/*
 *  × : ブレーキ
 */
void
onPS3ButtonCrossEdgeOn(void) {
	g_isBrake = TRUE;
//	SetDriveSpeed(0);
	g_body_cont_info.breaklamp = BODY_CONT_ON;
}


void
onPS3ButtonCrossEdgeOff(void) {
	g_isBrake =  FALSE;
	g_body_cont_info.breaklamp = BODY_CONT_OFF;
}

/*
 *  △ : ハザード
 */
void
onPS3ButtonTriangleEdgeOn(void) {
	TOGGLE_ON_OFF(g_body_cont_info.hazardlamp);
}

void
onPS3ButtonTriangleEdgeOff(void) {

}

/*
 *  ○ : メインヘッドライト(ヘッドライト1) 切り替え
 */
void
onPS3ButtonNoughtEdgeOn(void) {
	TOGGLE_ON_OFF(g_body_cont_info.headlight1);
}

void
onPS3ButtonNoughtEdgeOff(void) {

}

/*
 *  □ : ギア切り替え
 */
void
onPS3ButtonSquareEdgeOn(void) {
	syslog(LOG_NOTICE, "Shift N");
	g_ShiftState = SHIFT_N;
	g_body_cont_info.backlamp = BODY_CONT_OFF;
	g_body_cont_info.buzzer   = BODY_CONT_OFF;
#if 0
	TOGGLE_ON_OFF(g_body_cont_info.headlight3);
#endif
}

void
onPS3ButtonSquareEdgeOff(void) {

}

/*
 *  左ボタン : ステアリングのニュートラルを調整
 */
void
onPS3ButtonLeftEdgeOn(void) {
	AdjustSteerNeturalL();
	g_body_cont_info.blinker_l = BODY_CONT_ON;
}

void
onPS3ButtonLeftEdgeOff(void) {
	g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
}

/*
 *  右ボタン : ステアリングのニュートラルを調整
 */
void
onPS3ButtonRightEdgeOn(void) {
	AdjustSteerNeturalR();
	g_body_cont_info.blinker_r = BODY_CONT_ON;
}

void
onPS3ButtonRightEdgeOff(void) {
	g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
}

/*
 *  上ボタン : ステアリングをニュートラルにする
 */
void
onPS3ButtonUpEdgeOn(void) {
	g_steer_neutral = STEER_NEUTRAL_INIT;
	g_angle  = STEER_NEUTRAL_INIT;
	g_body_cont_info.blinker_l = BODY_CONT_ON;
	g_body_cont_info.blinker_r = BODY_CONT_ON;

#if 0	
	if(g_isBrake){
		syslog(LOG_NOTICE, "Shift D");
		g_ShiftState = SHIFT_D;
		g_body_cont_info.backlamp = BODY_CONT_OFF;
		g_body_cont_info.buzzer   = BODY_CONT_OFF;
	}else{
		syslog(LOG_NOTICE, "Shift D : error");
	}
#endif	
#if 0
	InitSteerNetural();
	g_body_cont_info.blinker_l = BODY_CONT_ON;
	g_body_cont_info.blinker_r = BODY_CONT_ON;
#endif
}

void
onPS3ButtonUpEdgeOff(void) {
	g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
	g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
}

/*
 *  下ボタン : モータ制御のゲインを初期値にする
 */
void
onPS3ButtonDownEdgeOn(void) {
	
#if 0
	if(g_isBrake){
		syslog(LOG_NOTICE, "Shift R");
		g_ShiftState = SHIFT_R;
		g_body_cont_info.backlamp = BODY_CONT_ON;
		g_body_cont_info.buzzer   = BODY_CONT_ON;
	}else{
		syslog(LOG_NOTICE, "Shift R : error");
	}
#endif

	g_speed_gain  = SPEED_GAIN_INIT;
	g_body_cont_info.blinker_l = BODY_CONT_ON;
	g_body_cont_info.blinker_r = BODY_CONT_ON;

#if 0
	char str[30];

	g_speed_gain = MOTOR_GAIN_INIT;
	SetDriveSpeed(g_speed/g_speed_gain);
	g_body_cont_info.blinker_l = BODY_CONT_ON;
	g_body_cont_info.blinker_r = BODY_CONT_ON;

	sprintf(str, "Adjust speed gain = %d \r\n", g_speed_gain);
	uart_PSendString(RC_OUTPUT_PORT, str);
#endif
}

void
onPS3ButtonDownEdgeOff(void) {
#if 0
	g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
	g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
#endif
}


/*
 *  L1/R1 : 方向指示器のON/OFF
 */
void
onPS3ButtonL1EdgeOn(void){
	if (g_blinker_l_blink) {
		g_blinker_l_blink = FALSE;
		/* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
		if (!(g_body_cont_info.blinker_l == BODY_CONT_ON)) {
			g_body_cont_info.blinker_l = BODY_CONT_OFF;
		}
	} else {
		g_blinker_l_blink = TRUE;
		/* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
		if (!(g_body_cont_info.blinker_l == BODY_CONT_ON)) {
			g_body_cont_info.blinker_l = BODY_CONT_BLINK;
		}
	}
}

void
onPS3ButtonL1EdgeOff(void) {

}

void
onPS3ButtonR1EdgeOn(void){
	if (g_blinker_r_blink) {
		g_blinker_r_blink = FALSE;
		/* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
		if (!(g_body_cont_info.blinker_r == BODY_CONT_ON)) {
			g_body_cont_info.blinker_r = BODY_CONT_OFF;
		}
	} else {
		g_blinker_r_blink = TRUE;
		/* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
		if (!(g_body_cont_info.blinker_r == BODY_CONT_ON)) {
			g_body_cont_info.blinker_r = BODY_CONT_BLINK;
		}
	}
}

void
onPS3ButtonR1EdgeOff(void) {

}

/*
 *  L2/R2 : モータ制御のゲインを変更
 */
void
onPS3ButtonL2EdgeOn(void){
	if(g_speed_gain > SPEED_GAIN_MIN){
		g_speed_gain--;
   }
	g_body_cont_info.blinker_l = BODY_CONT_ON;
#if 0
	AdjustSpeedGainD();
	g_body_cont_info.blinker_l = BODY_CONT_ON;
#endif
}

void
onPS3ButtonL2EdgeOff(void){
#if 0
	g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
#endif
}

void
onPS3ButtonR2EdgeOn(void){
	if (g_speed_gain < SPEED_GAIN_MAX){
		g_speed_gain++;
		g_body_cont_info.blinker_r = BODY_CONT_ON;
	}

#if 0
	AdjustSpeedGainU();
	g_body_cont_info.blinker_r = BODY_CONT_ON;
#endif
}

void
onPS3ButtonR2EdgeOff(void){
#if 0
	g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
#endif
}

/*
 *  ボタンの変化検出のためのマクロ
 */
#define PS3BUTTON_EDGE_ON(no)  ((p_g_pre_ps3button[no] == FALSE) && (p_g_cur_ps3button[no] == TRUE))
#define PS3BUTTON_EDGE_OFF(no) ((p_g_pre_ps3button[no] == TRUE) && (p_g_cur_ps3button[no] == FALSE))
#define PS3ANALOG_CHANGE(no)   (p_g_pre_ps3analog[no] != p_g_cur_ps3analog[no])

/*
 *  PS3コントローラからのイベントの処理
 */
void
onMsgPS3(void) {
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_CROSS)){
		onPS3ButtonCrossEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_TRIANGLE)){
		onPS3ButtonTriangleEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_TRIANGLE)){
		onPS3ButtonTriangleEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_NOUGHT)){
		onPS3ButtonNoughtEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_NOUGHT)){
		onPS3ButtonNoughtEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_SQUARE)){
		onPS3ButtonSquareEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_SQUARE)){
		onPS3ButtonSquareEdgeOff();
	}


	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_LEFT)){
		onPS3ButtonLeftEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_LEFT)){
		onPS3ButtonLeftEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_RIGHT)){
		onPS3ButtonRightEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_RIGHT)){
		onPS3ButtonRightEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_UP)){
		onPS3ButtonUpEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_UP)){
		onPS3ButtonUpEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_DOWN)){
		onPS3ButtonDownEdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_DOWN)){
		onPS3ButtonDownEdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_CROSS)){
		onPS3ButtonCrossEdgeOn();
	}

	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_L1)){
		onPS3ButtonL1EdgeOn();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_R1)){
		onPS3ButtonR1EdgeOn();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_L2)){
		onPS3ButtonL2EdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_L2)){
		onPS3ButtonL2EdgeOff();
	}
	if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_R2)){
		onPS3ButtonR2EdgeOn();
	}
	if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_R2)){
		onPS3ButtonR2EdgeOff();
	}
	if (PS3ANALOG_CHANGE(NO_PS3ANALOG_L_LR)) {
		onPS3AnalogL_LR();
	}
	if (PS3ANALOG_CHANGE(NO_PS3ANALOG_L_UD)) {
		onPS3AnalogL_UD();
	}
	if (PS3ANALOG_CHANGE(NO_PS3ANALOG_R_LR)) {
		onPS3AnalogR_LR();
	}
	if (PS3ANALOG_CHANGE(NO_PS3ANALOG_R_UD)) {
		onPS3AnalogR_UD();
	}
}

/*
 *  周期送信とするか
 */
boolean g_body_can_msg_cyclic = FALSE;


void
body_can_msg_send(void)
{
	uint8 tx_data[8];
	/* 前回送付の状態 */
	static BODY_CONT_INFO g_pre_body_cont_info;

	/* 状態指示に変化が無ければ送信しない */
	if (!g_body_can_msg_cyclic){
		if((g_body_cont_info.headlight1 == g_pre_body_cont_info.headlight1) &&
		   (g_body_cont_info.headlight2 == g_pre_body_cont_info.headlight2) &&
		   (g_body_cont_info.headlight3 == g_pre_body_cont_info.headlight3) &&
		   (g_body_cont_info.breaklamp  == g_pre_body_cont_info.breaklamp) &&
		   (g_body_cont_info.backlamp   == g_pre_body_cont_info.backlamp) &&
		   (g_body_cont_info.buzzer     == g_pre_body_cont_info.buzzer) && 
		   (g_body_cont_info.blinker_l  == g_pre_body_cont_info.blinker_l) &&
		   (g_body_cont_info.blinker_r  == g_pre_body_cont_info.blinker_r) &&
		   (g_body_cont_info.hazardlamp == g_pre_body_cont_info.hazardlamp)) {
			return;
		}
	}

	/* 送信メッセージを生成 */
	tx_data[0] = ((g_body_cont_info.headlight1 << 7) &  0x80) | 
				 ((g_body_cont_info.headlight2 << 6) &  0x40) |
				 ((g_body_cont_info.headlight3 << 5) &  0x20) |
				 ((g_body_cont_info.breaklamp  << 4) &  0x10) |
				 ((g_body_cont_info.backlamp   << 3) &  0x08) |
				 ((g_body_cont_info.buzzer     << 1) &  0x06);

	tx_data[1] = ((g_body_cont_info.blinker_l  << 6) &  0xc0) | 
				 ((g_body_cont_info.blinker_r  << 4) &  0x30) | 
				 ((g_body_cont_info.hazardlamp << 3) &  0x08);

	NcanSetTxData(0, BODY_CAN_MSG_RMB, BODY_CAN_MSG_ID, tx_data, 2);

	g_pre_body_cont_info = g_body_cont_info;
}


/*
 *  起動時のボディーの接続確認
 * 
 *  全LED点滅とブザーを鳴らす．
 */
void
body_check(void)
{
	int loop, tcnt;
	uint8 status = BODY_CONT_ON;

	for(loop = 0; loop < INIT_BODY_BLINK_COUNT*2; loop++) {
		g_body_cont_info.headlight1 = status;
		g_body_cont_info.headlight2 = status;
		g_body_cont_info.headlight3 = status;
		g_body_cont_info.breaklamp  = status;
		g_body_cont_info.backlamp   = status;
		g_body_cont_info.buzzer     = status;
		g_body_cont_info.blinker_l  = status;
		g_body_cont_info.blinker_r  = status;

		tcnt = 0;

		if (!IsBodyOnECU){
			body_can_msg_send();
		}

		while(1) {
			WaitEvent(RCOperatorEvt);
			ClearEvent(RCOperatorEvt);
			tcnt++;
			if (tcnt == INIT_BODY_BLINK_CYCLE_MS/10) {
				break;
			}
		}

		status = (status == BODY_CONT_ON)? BODY_CONT_OFF : BODY_CONT_ON;
	}
}
void
rcb3_error(){
	static int cnt = 0;
	syslog(LOG_NOTICE, "error 0x%x : 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x \r\n", cnt++, g_rcb3_rmsg[0], g_rcb3_rmsg[1], g_rcb3_rmsg[2], g_rcb3_rmsg[3], g_rcb3_rmsg[4], g_rcb3_rmsg[5], g_rcb3_rmsg[6], g_rcb3_rmsg[7]);
}

void
com_error(){
	syslog(LOG_NOTICE, "com error");
}

/*
 *  制御タスク
 */
TASK(RCControlTask)
{
	uint32 cycle = 0;
	syslog(LOG_INFO, "RCControlTask : Start!");
	uint8 rx_data[8];
	uint8 rx_size;
	int   loop;
	boolean result;

	body_check();

	while(1){
		/* 10m待ち */
		WaitEvent(RCControlEvt); 
		ClearEvent(RCControlEvt);

		/*
		 *  RCOperatorTaskが別ECUの場合はCANで受信
		 */
		if (!IsOpeOnECU){
			NcanGetRxData(0, CONT_CMD_CAN_MSG_RMB, &rx_data[0], &rx_size);
			if (rx_size != 0) {
				result = FALSE; 
				for (loop = 0; loop < rx_size; loop++){
					result = rcb3_AddReceivedByte(rx_data[loop]);
				}
				if (result) {
					onMsgPS3();
				}
				else {
					syslog(LOG_NOTICE, "RCControlTask : Received Can data Error!\r\n");
				}
			}
		}
		else {
			/*
			 *  同一ECUの場合
			 */
			if (UpDateCntrolCmd == TRUE){
				onMsgPS3();
				UpDateCntrolCmd = FALSE;
			}
		}

		/*
		 *  RCBodyTaskが別ECUの場合はCANで送信
		 */
		if (!IsBodyOnECU) {
			if (cycle++ == (BODY_CAN_SEND_CYCLE_MS/RC_CONTRL_TASK_CYCLE_MS)) {
				body_can_msg_send();
				cycle = 0;
			}
		}
	}
}

//////////////////////////////////////////////////////////////////////////
/*
 *  
 *   パワトレ関係
 *
 */
#define UEC_TO_PWMCNT(usec)  (usec*PWM_CLK_MH)
/* 100nsec 単位との変換 */
#define M100NSEC_TO_PWMCNT(m100nsec)  ((m100nsec*PWM_CLK_MH)/10)

#define SERVO_N    1570
//#define SERVO_RMAX 1830
//#define SERVO_LMAX 1400


#define SERVO_RMAX 1870 
#define SERVO_LMAX 1330

void
servo_setangle(int angle) {
	sil_wrw_iop((void *)PWM1_ADDR, UEC_TO_PWMCNT(angle));
}

#define ESC_N      15000
#define ESC_DMAX   14000
#define ESC_RMAX   16500
#define ESC_DTH    14500 
#define ESC_RTH    15800
#define ESC_BREAK  16200

void
RCPowerTrainTaskWait_20msec(void){
	ClearEvent(RCPowerTrainEvt);
	WaitEvent(RCPowerTrainEvt); 
	ClearEvent(RCPowerTrainEvt);
	WaitEvent(RCPowerTrainEvt); 
	ClearEvent(RCPowerTrainEvt);
}

void
esc_setspeed(int speed){
	sil_wrw_iop((void *)PWM2_ADDR, M100NSEC_TO_PWMCNT(speed));
}

void
esc_dbrak(void){
	esc_setspeed(ESC_BREAK);
}

void
esc_rbrak(void){
	esc_setspeed(ESC_DTH);
	RCPowerTrainTaskWait_20msec();
	RCPowerTrainTaskWait_20msec();
	esc_setspeed(ESC_BREAK);
}

void
esc_neutral(void){
	esc_setspeed(ESC_N);
}

void
esc_reverse(void){
	esc_neutral();
	RCPowerTrainTaskWait_20msec();
	RCPowerTrainTaskWait_20msec();
}

void
esc_change_drive_to_reverse(void){
	esc_setspeed(ESC_BREAK);
	RCPowerTrainTaskWait_20msec();
	esc_neutral();
	RCPowerTrainTaskWait_20msec();
}

TASK(RCPowerTrainTask){
	int servo_pwm;
	static boolean pre_g_isBrake = FALSE;
	static SHIFT_STATE pre_g_ShiftState = SHIFT_N;
	static int pre_g_accelerator = 0;
	int esc_pwm_100nsec = 0;

	syslog(LOG_INFO, "RCPowerTrainTask : Start!");

	/* 周期設定 : 20m周期 */
	sil_wrw_iop((void *)PWM_MAX_ADDR, UEC_TO_PWMCNT(20000));

	/* ニュートラル */
	esc_neutral();
	servo_setangle(SERVO_N);
	syslog(LOG_INFO, "RCPowerTrainTask : neutral");

	while(1){
		WaitEvent(RCPowerTrainEvt);     /* 10msの作業時間待ち */
		ClearEvent(RCPowerTrainEvt);

		/*
		 * サーボの設定
		 * 左 32 から 右 -32 範囲
		 */
/*
SERVO_LMAX       SERVO_N       SERVO_RMAX
 1400             1570           1830
  32                0             -32
*/

		if(g_angle >= 0){
			/* 左側操舵の場合 */
			servo_pwm = SERVO_N - ((((SERVO_N - SERVO_LMAX) * 100) / (32 * 100)) * g_angle);
		}
		else {
			/* 右側操舵の場合 */
			servo_pwm = SERVO_N + ((((SERVO_RMAX - SERVO_N) * 100) / (32 * 100)) * (-g_angle));
		}
		servo_setangle(servo_pwm);

		if(pre_g_isBrake != g_isBrake) {
			/* ブレーキの状態が変化した場合 */
			pre_g_isBrake = g_isBrake;
			if (g_isBrake == FALSE) {
				/* ノンブレーキ状態 */
				syslog(LOG_INFO, "To non-braking");
				if (g_ShiftState == SHIFT_R) {
					esc_reverse();
				}
			}
			else if (g_isBrake == TRUE) {
				/* ブレーキ状態 */
				syslog(LOG_INFO, "To braking");
				if (pre_g_accelerator > 0){
					esc_dbrak();
				}
				else {
					esc_rbrak();
				}
			}
		}

		if (g_accelerator < 0){
			g_ShiftState = SHIFT_D;
			pre_g_ShiftState = SHIFT_D;
		}

		if (g_accelerator == 0){
			g_ShiftState = SHIFT_N;
		}

		if (g_accelerator > 0){
			if (pre_g_ShiftState == SHIFT_D){
				esc_change_drive_to_reverse();
				syslog(LOG_INFO, "RCPowerTrainTask : change drive to reverse");
			}
			g_ShiftState = SHIFT_R;
			pre_g_ShiftState = SHIFT_R;
		}
#if 0
		if(pre_g_ShiftState != g_ShiftState) {
			pre_g_ShiftState = g_ShiftState;
			if(g_ShiftState == SHIFT_D){
				esc_pwm_100nsec = ESC_DTH;
			}else if(g_ShiftState == SHIFT_R){
				esc_pwm_100nsec = ESC_RTH;
				esc_reverse();
			}
		}
#endif

		if(g_isBrake == TRUE){
			/* ブレーキ状態 */
		}else{
			/* ノンブレーキ状態 */
			if(g_ShiftState == SHIFT_N){
				/* ニュートラル */
					esc_neutral();
			} else {
				/* ニュートラル以外 */

				/*
				 *  
				 */
				if(g_accelerator == 0){
					esc_pwm_100nsec = ESC_N;
				}else{
					if (g_ShiftState == SHIFT_D){
						esc_pwm_100nsec = ESC_DTH + ((((ESC_DTH - ESC_DMAX)/64) * g_accelerator));
						// * g_speed_gain/SPEED_GAIN_INIT
					}
					if (g_ShiftState == SHIFT_R){
						esc_pwm_100nsec = ESC_RTH + ((((ESC_RMAX - ESC_RTH)/64) * g_accelerator));
						// * g_speed_gain/SPEED_GAIN_INIT
					}
				}
				esc_setspeed(esc_pwm_100nsec);
			}
		}
		pre_g_accelerator = g_accelerator;
	}
}




//////////////////////////////////////////////////////////////////////////
/*
 *  LED制御系
 */
static uint8 led0_enable = FALSE;
static uint8 led7_enable = FALSE;

void
set_led7(boolean enable){
	led7_enable = enable;
}

void
set_led0(boolean enable){
	led0_enable = enable;
}

#define PWM_CYCLE_100US 100
#define LED_SHIFT_CYCLE  10
#define PWM_MAX         100

int led_duty[8];

ALARMCALLBACK(ActiveLEDCycCb){
	static int pwm_cyccnt = 0;
	uint8 pattern = 0;
	static int center_led = 6;
	static int direction = 1; /* 1:right, 0:left */
	static int led_shift_cnt = 0;
	int i;

	/* 100usec毎に呼び出される */

	/* PWM周期 */
	if (pwm_cyccnt++ >= PWM_CYCLE_100US){
		pwm_cyccnt = 0;
	}

	if (pwm_cyccnt == 0) {
		if(led_shift_cnt++ == LED_SHIFT_CYCLE){
			led_shift_cnt = 0;

			for(i = 0; i < 7; i++){
				int pwm = PWM_MAX;
				if (center_led > i){
					pwm = PWM_MAX >> ((center_led - i) + 1);
				}
				else if (center_led < i){
					pwm = PWM_MAX >> ((i - center_led) + 1);
				}
				else if (center_led == i){
					pwm = PWM_MAX;
				}
				led_duty[i] = pwm;
			}

			if(direction == 1){
				/* right */
				if (--center_led <= 1){
					direction = 0;
				}
			}
			else {
				/* left */
				if (++center_led >= 6){
					direction = 1;
				}
			}
		}
	}

	for(i = 1; i < 7; i++){
		if (pwm_cyccnt < led_duty[i]) {
			pattern |= 1 << i;
		}
	}

	if (led0_enable) {
		pattern |= 0x01;
	}
	if (led7_enable) {
		pattern |= 0x80;
	}

	set_led(pattern);
}

//////////////////////////////////////////////////////////////////////////

/*
 *  CANの初期化ルーチン
 */
void
can_init(void){
	/*
	 *  モードによらず全てのメールボックスを初期化する
	 */
	NcanInit(0);
	NcanSetMailBoxInfo(0, BODY_CAN_MSG_TMB,     DIR_SEND, 0, BODY_CAN_MSG_ID, 0, 0);
	NcanSetMailBoxInfo(0, CONT_CMD_CAN_MSG_TMB, DIR_SEND, 0, CONT_CMD_CAN_MSG_ID, 0, 0);
	NcanSetMailBoxInfo(0, BODY_CAN_MSG_RMB,     DIR_RECV, 0, BODY_CAN_MSG_ID, 0, 0);
	NcanSetMailBoxInfo(0, CONT_CMD_CAN_MSG_RMB, DIR_RECV, 0, CONT_CMD_CAN_MSG_ID, 0, 0);
	NcanEnable(0);
}

//////////////////////////////////////////////////////////////////////////
/*
 *  スタートアップフックルーチン
 */
void
StartupHook(void)
{
	syslog_initialize();
	syslog_msk_log(LOG_UPTO(LOG_INFO));
	InitSerial();
	print_banner();
	can_init();
	rcb3_Init();
}

/*
 *  シャットダウンフックルーチン
 */
void
ShutdownHook(StatusType Error)
{
	/* 終了ログ出力 */
	syslog(LOG_INFO, "");
	syslog(LOG_INFO, "Sample System ShutDown");
	syslog(LOG_INFO, "ShutDownCode:%s", atk2_strerror(Error));
	syslog(LOG_INFO, "");

	if (Error == E_OS_SYS_ASSERT_FATAL) {
		syslog(LOG_INFO, "fatal_file_name:%s", fatal_file_name);
		syslog(LOG_INFO, "fatal_line_num:%d", fatal_line_num);
	}

	TermSerial();
}

//////////////////////////////////////////////////////////////////////////

/*
 *  コマンド受信処理
 */
static uint8
GetCommand(void)
{
	uint8 command;          /* コマンド受信バッファ */

	/*
	 *  コマンドを受信するまでループ
	 */
	command = '\0';
	do {
		WaitEvent(MainEvt);     /* 10msウェイト */
		ClearEvent(MainEvt);
		RecvPolSerialChar(&command);    /* 受信バッファポーリング */
		if (command == '\n') {
			command = '\0';
		}
	} while (command == '\0');


	return(command);
}   /* GetCommand */

/*
 *  メインタスク
 *
 *  ユーザコマンドの受信と，コマンドごとの処理実行
 */
TASK(MainTask)
{
	uint8		command;
	uint8		task_no;
	uint32		i;

	/*
	 *  タスク番号・コマンドバッファ初期化
	 */
	task_no = (uint8) (0);
	for (i = 0U; i < (sizeof(command_tbl) / sizeof(command_tbl[0])); i++) {
		command_tbl[i] = 0U;
	}

	syslog(LOG_INFO, "Main Task : start!");
#if 0
	NcanSetMailBoxInfo(0, 0, DIR_RECV, 0, 3, 0, 0);
	NcanSetMailBoxInfo(0, 0, DIR_SEND, 0, 4, 0, 0);
	NcanEnable(0);
	while(1){
		WaitEvent(MainEvt);     /* 10msの作業時間待ち */
		ClearEvent(MainEvt);
		NcanGetRxData(0, 0, &rx_data[0], &rx_size );
		if (rx_size != 0) {
			syslog(LOG_INFO, "Main Task : can receive 0x%x, 0x%x", rx_data[0], rx_data[1]);
		}
		i++;
		if(i == 200){
			syslog(LOG_INFO, "Main Task : alive");
			i = 0;
			tx_data[0] = cnt++;
			tx_data[1] = 0x02;
			NcanSetTxData(0, 0, 4, tx_data, 2);
		}
	}
#endif

	/*
	 *  コマンド実行ループ
	 */
	while (1) {
		WaitEvent(MainEvt);     /* 10msの作業時間待ち */
		ClearEvent(MainEvt);

		/*
		 *  入力コマンド取得
		 */
		syslog(LOG_INFO, "Input Command:");
		command = GetCommand();

		/*
		 *  入力コマンドチェック
		 */
		if ((command <= (uint8) (0x1fU)) || (command >= (uint8) (0x80U))) {
			syslog(LOG_INFO, "Not ASCII character");
		}
		else {
			syslog(LOG_INFO, "%c", command);

			/*
			 *  コマンド判別
			 */
			switch (command) {
			case '1':
			default:
				/* 上記のコマンド以外の場合，処理を行わない */
				break;
			}
		}
	}

	/*
	 *  ここにはこない
	 */
	syslog(LOG_INFO, "MainTask TERMINATE");
	error_log(TerminateTask());
}   /* TASK( MainTask ) */


/*
 *  エラーフックルーチン
 */
#ifdef CFG_USE_ERRORHOOK
void
ErrorHook(StatusType Error)
{
	/*
	 *  エラー要因ごとのパラメータログ出力
	 */
	switch (OSErrorGetServiceId()) {
	case OSServiceId_ActivateTask:
		syslog(LOG_INFO, "Error:%s=ActivateTask(%d)", atk2_strerror(Error), OSError_ActivateTask_TaskID());
		break;
	case OSServiceId_TerminateTask:
		syslog(LOG_INFO, "Error:%s=TerminateTask()", atk2_strerror(Error));
		break;
	case OSServiceId_ChainTask:
		syslog(LOG_INFO, "Error:%s=ChainTask(%d)", atk2_strerror(Error), OSError_ChainTask_TaskID());
		break;
	case OSServiceId_Schedule:
		syslog(LOG_INFO, "Error:%s=Schedule()", atk2_strerror(Error));
		break;
	case OSServiceId_GetTaskID:
		syslog(LOG_INFO, "Error:%s=GetTaskID(0x%p)", atk2_strerror(Error), OSError_GetTaskID_TaskID());
		break;
	case OSServiceId_GetTaskState:
		syslog(LOG_INFO, "Error:%s=GetTaskState(%d, 0x%p)", atk2_strerror(Error),
			   OSError_GetTaskState_TaskID(), OSError_GetTaskState_State());
		break;
	case OSServiceId_EnableAllInterrupts:
		syslog(LOG_INFO, "Error:%s=EnableAllInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_DisableAllInterrupts:
		syslog(LOG_INFO, "Error:%s=DisableAllInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_ResumeAllInterrupts:
		syslog(LOG_INFO, "Error:%s=ResumeAllInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_SuspendAllInterrupts:
		syslog(LOG_INFO, "Error:%s=SuspendAllInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_ResumeOSInterrupts:
		syslog(LOG_INFO, "Error:%s=ResumeOSInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_SuspendOSInterrupts:
		syslog(LOG_INFO, "Error:%s=SuspendOSInterrupts()", atk2_strerror(Error));
		break;
	case OSServiceId_GetISRID:
		syslog(LOG_INFO, "Error:%s=GetISRID()", atk2_strerror(Error));
		break;
	case OSServiceId_GetResource:
		syslog(LOG_INFO, "Error:%s=GetResource(%d)", atk2_strerror(Error), OSError_GetResource_ResID());
		break;
	case OSServiceId_ReleaseResource:
		syslog(LOG_INFO, "Error:%s=ReleaseResource(%d)", atk2_strerror(Error), OSError_ReleaseResource_ResID());
		break;
	case OSServiceId_SetEvent:
		syslog(LOG_INFO, "Error:%s=SetEvent(%d, 0x%x)", atk2_strerror(Error),
			   OSError_SetEvent_TaskID(), OSError_SetEvent_Mask());
		break;
	case OSServiceId_ClearEvent:
		syslog(LOG_INFO, "Error:%s=ClearEvent(0x%x)", atk2_strerror(Error), OSError_ClearEvent_Mask());
		break;
	case OSServiceId_GetEvent:
		syslog(LOG_INFO, "Error:%s=GetEvent(%d, 0x%p)", atk2_strerror(Error),
			   OSError_GetEvent_TaskID(), OSError_GetEvent_Event());
		break;
	case OSServiceId_WaitEvent:
		syslog(LOG_INFO, "Error:%s=WaitEvent(0x%x)", atk2_strerror(Error), OSError_WaitEvent_Mask());
		break;
	case OSServiceId_GetAlarmBase:
		syslog(LOG_INFO, "Error:%s=GetAlarmBase(0x%p)", atk2_strerror(Error), OSError_GetAlarmBase_AlarmID());
		break;
	case OSServiceId_GetAlarm:
		syslog(LOG_INFO, "Error:%s=GetAlarm(%d, 0x%p)", atk2_strerror(Error),
			   OSError_GetAlarm_AlarmID(), OSError_GetAlarm_Tick());
		break;
	case OSServiceId_SetRelAlarm:
		syslog(LOG_INFO, "Error:%s=SetRelAlarm(%d, %d, %d)", atk2_strerror(Error),
			   OSError_SetRelAlarm_AlarmID(), OSError_SetRelAlarm_increment(), OSError_SetRelAlarm_cycle());
		break;
	case OSServiceId_SetAbsAlarm:
		syslog(LOG_INFO, "Error:%s=SetAbsAlarm(%d, %d, %d)", atk2_strerror(Error),
			   OSError_SetAbsAlarm_AlarmID(), OSError_SetAbsAlarm_start(), OSError_SetAbsAlarm_cycle());
		break;
	case OSServiceId_CancelAlarm:
		syslog(LOG_INFO, "Error:%s=CancelAlarm(%d)", atk2_strerror(Error), OSError_CancelAlarm_AlarmID());
		break;
	case OSServiceId_StartScheduleTableRel:
		syslog(LOG_INFO, "Error:%s=StartScheduleTableRel(%d, %d)", atk2_strerror(Error),
			   OSError_StartScheduleTableRel_ScheduleTableID(), OSError_StartScheduleTableRel_Offset());
		break;
	case OSServiceId_StartScheduleTableAbs:
		syslog(LOG_INFO, "Error:%s=StartScheduleTableAbs(%d, %d)", atk2_strerror(Error),
			   OSError_StartScheduleTableAbs_ScheduleTableID(), OSError_StartScheduleTableAbs_Start());
		break;
	case OSServiceId_StopScheduleTable:
		syslog(LOG_INFO, "Error:%s=StopScheduleTable(%d)", atk2_strerror(Error), OSError_StopScheduleTable_ScheduleTableID());
		break;
	case OSServiceId_NextScheduleTable:
		syslog(LOG_INFO, "Error:%s=NextScheduleTable(%d, %d)", atk2_strerror(Error),
			   OSError_NextScheduleTable_ScheduleTableID_From(), OSError_NextScheduleTable_ScheduleTableID_To());
		break;
	case OSServiceId_GetScheduleTableStatus:
		syslog(LOG_INFO, "Error:%s=GetScheduleTableStatus(%d, 0x%p)", atk2_strerror(Error),
			   OSError_GetScheduleTableStatus_ScheduleTableID(), OSError_GetScheduleTableStatus_ScheduleStatus());
		break;
	case OSServiceId_GetActiveApplicationMode:
		syslog(LOG_INFO, "Error:%s=GetActiveApplicationMode()", atk2_strerror(Error));
		break;
	case OSServiceId_StartOS:
		syslog(LOG_INFO, "Error:%s=StartOS()", atk2_strerror(Error));
		break;
	case OSServiceId_ShutdownOS:
		syslog(LOG_INFO, "Error:%s=ShutdownOS()", atk2_strerror(Error));
		break;
	case OSServiceId_IncrementCounter:
		syslog(LOG_INFO, "Error:%s=IncrementCounter(%d)", atk2_strerror(Error), OSError_IncrementCounter_CounterID());
		break;
	case OSServiceId_TaskMissingEnd:
		syslog(LOG_INFO, "Error:%s=MissingEnd()", atk2_strerror(Error));
		break;
	default:
		syslog(LOG_INFO, "Error:%s=UnKnownFunc()", atk2_strerror(Error));
		break;
	}

}   /* ErrorHook */
#endif /* CFG_USE_ERRORHOOK */

/*
 *  プレタスクフックルーチン
 *
 *  空ルーチンを呼出す
 */
#ifdef CFG_USE_PRETASKHOOK
void
PreTaskHook(void)
{
}   /* PreTaskHook */
#endif /* CFG_USE_PRETASKHOOK */

/*
 *  ポストタスクフックルーチン
 *
 *  空ルーチンを呼出す
 */
#ifdef CFG_USE_POSTTASKHOOK
void
PostTaskHook(void)
{
}   /* PostTaskHook */
#endif /* CFG_USE_POSTTASKHOOK */


/*
 *  プロテクションフックルーチン
 */
#ifdef CFG_USE_PROTECTIONHOOK
ProtectionReturnType
ProtectionHook(StatusType FatalError)
{
	StatusType ercd;

	syslog(LOG_INFO, "");
	syslog(LOG_INFO, "ProtectionHook");

	if (FatalError == E_OS_STACKFAULT) {
		syslog(LOG_INFO, "E_OS_STACKFAULT");
		ercd = PRO_SHUTDOWN;
	}
	else if (FatalError == E_OS_PROTECTION_EXCEPTION) {
		syslog(LOG_INFO, "E_OS_PROTECTION_EXCEPTION");
		ercd = PRO_IGNORE;
	}
	else {
		ercd = PRO_SHUTDOWN;
	}

	return(ercd);
}
#endif /* CFG_USE_PROTECTIONHOOK */