#include <airobot/cpp/SimpleRobot.hpp>

/**
 * 這是一個繞墻走的機器人
 * @author xiemin
 */
class Walls : public SimpleRobot
{	
	private:
		/*
		 * 前進的方向。如果機器人當前的方向不在這個方向上，
		 * 則先停止運動，轉動車身到這個方向上來
		 */
		double headingTo;
	public:
		void onTick(TickAction* action);
		void onHitWall(HitWallAction* action);
		void onRoundBegin(RoundBeginAction* action);
};

///////////////////////////////////////////////////////////////
//正確的實現方案
void Walls::onTick(TickAction* action)
{
	//計算當前的方向和headingTo的夾角
	double angle =  Math::bearing(headingTo, getHeading());
	
	if( fabs(angle)>0.0001 ) //停止前進，轉動到headingTo方向上來
	{
		move(0);
		turn(angle);
	}
	else //停止轉動，繼續前進
	{
		turn(0);
		move(8);
	}
}

void Walls::onHitWall(HitWallAction* action)
{
	//改變前進的方向
	headingTo += Math::PI/2;
}

void Walls::onRoundBegin(RoundBeginAction* action)
{
	//初始化移動方向
	headingTo = 0;
}

/////////////////////////////////////////////////////////
//錯誤的實現方案
/*
void Walls::onRoundBegin(RoundBeginAction* action)
{
	move(8);
}
void Walls::onHitWall(HitWallAction* action)
{
	//headingTo 表示要轉動到的方向
	double headingTo = getHeading()+Math::PI/2;
	//停止移動
	move(0);
	while(true)
	{
		//計算要轉動的角度
		double angle = Math::bearing( getHeading(), headingTo);
		//執行轉動
		turn(angle);
		//當轉到headingTo時退出循環
		if(angle == 0) break;
	}
	//繼續向前移動
	move(8);
}
*/


//啟動機器人程序
int main(int argC, char* argV[])
{
	Robot* robot = new Walls();
	return startup(argC, argV, robot);
}