#include <3067F.H>
#include "2l33a.h"

void set_motion(struct ACTION *action, struct SERVO_DATA *servo_data);
/**********************************************/
/*アクションｉｄをモーションｉｄにデコードする*/
/**********************************************/
void action_decode(struct ACTION *action, struct SERVO_DATA *servo_data)
{
static unsigned char	last_step = 0;
/*デコードテーブル*/
unsigned char	motion_dat[] = { 0,  0,  0,
				 2,  3,  4,
				14, 15, 16,
				 5,  6,  7,
				 8,  9, 10,
				20, 21, 22,
				17, 18, 19,
				26, 27, 28,
				23, 24, 25,
				11, 12, 13 };


	/******************************************************/
	/*立つ*/
	if(action->id0 == 0 && last_step == 1)
	{
		servo_data->motion_id0 = 1;
		last_step = 0;
		set_motion(action, servo_data);
		action->id1 = action->id0;
		return;
	}
	/*腰を落とす*/
	if(action->id0 != 0 && action->id1 == 0)
	{
		servo_data->motion_id0 = 0;
		last_step = 1;
		set_motion(action, servo_data);
		action->id1 = 99;
		return;
	}
	/******************************************************/
	if(action->id0 != action->id1)
	{
		/*直前に出したモーションは定常モーションか？*/
		if(action->id1 == 99) action->id1 = 0;
		if(servo_data->motion_id1 != motion_dat[action->id1 * 3 + 0] || action->id1 == 0)
		{
			/*はじめの一歩*/
			servo_data->motion_id0 = motion_dat[action->id0 * 3 + 1];
			last_step = 0;
			action->id1 = action->id0;
		}
		else
		{
			/*最後の一歩*/
			servo_data->motion_id0 = motion_dat[action->id1 * 3 + 2];
			last_step = 1;
			action->id1 = 99;
		}
	}
	else
	{
		/*定常歩行*/
		servo_data->motion_id0 = motion_dat[action->id0 * 3 + 0];
		last_step = 0;
		action->id1 = action->id0;
	}
	/******************************************************/
	set_motion(action, servo_data);
	return;
}

/******************************************/
/*モーションデータへのポインタをセットする*/
/******************************************/
void set_motion(struct ACTION *action, struct SERVO_DATA *servo_data)
{
	if(servo_data->wave_datnum[servo_data->motion_id0] != 0)
	{
		servo_data->ram1 = servo_data->wave_ptr[servo_data->motion_id0];
		servo_data->wave_play = servo_data->wave_datnum[servo_data->motion_id0] - 1;

		if(action->id0 == 3 || action->id0 == 4)/*横進のとき*/
		{
			servo_data->gyro_mode[0] = 0;
			servo_data->gyro_mode[1] = 1;
		}
		else
		{
			servo_data->gyro_mode[0] = 1;
			servo_data->gyro_mode[1] = 1;
		}

		servo_data->motion_id1 = servo_data->motion_id0;
	}
	else
		switch_LED(3);/*赤*/
}