/**
 * 圍繞對(duì)手作來(lái)回的移動(dòng)，并在此基礎(chǔ)上添加了與對(duì)手保持一定距離，不撞墻的功能，
 * 并且運(yùn)動(dòng)帶有隨機(jī)性。
 * @author xiemin
 */
#include <airobot/c/SimpleRobot.h>
 
//理想距離
#define PREFER_DISTANCE 300
//計(jì)算目標(biāo)點(diǎn)時(shí)采用的搜索半徑
#define SEARCH_DIS 100
//每次迭代的遞增量
#define GAP toRadians(5)
//離墻的距離
#define AWAY_FROM_WALL 50

//移動(dòng)的方向,這個(gè)變量的值只能取1和-1，1表示前進(jìn)，-1表示后退
int direction = 1;
//在一個(gè)方向上最大的移動(dòng)時(shí)間
long maxTime = 30;
//當(dāng)前在一個(gè)方向上的移動(dòng)時(shí)間
long moveTime = 0;

//設(shè)置移動(dòng)的方向
void setDirection(void);
//執(zhí)行移動(dòng)
void doMove(void);
//執(zhí)行轉(zhuǎn)動(dòng)
void doTurn(void);
//得到圓心位置
void getCenter(double* x, double* y);
//得到用于保持理想距離的修正角deltaAngle
double getDeltaAngle(double centerX, double centerY);
//得到用來(lái)修正移動(dòng)方向避免機(jī)器人撞墻的夾角needTurn
double getNeedTurn(double headingTo);
//判斷給定的點(diǎn)是否是一個(gè)有效的目標(biāo)點(diǎn)
int isValid(double x, double y);

void onTick(struct TickAction* action)
{
	setDirection();
	doMove();
	doTurn();
}
	
void setDirection(void)
{
	moveTime++;
	if(moveTime>=maxTime)
	{
		moveTime=0;
		//隨機(jī)生成在這個(gè)方向上移動(dòng)的時(shí)間
		maxTime = rand()%60;
		//改變運(yùn)動(dòng)方向
		direction *= -1;
	}
}

void doMove(void)
{
	move(10*direction);
}

void doTurn(void)
{
	double centerX, centerY, lineHeading, headingTo, deltaAngle, needTurn, bea;
	
	getCenter(&centerX, &centerY);
	lineHeading = heading(getX(), getY(), centerX, centerY);
	headingTo = lineHeading + PI/2;
	//為了保持距離用deltaAngle對(duì)headingTo進(jìn)行修正
	deltaAngle = getDeltaAngle(centerX, centerY);
	headingTo -= deltaAngle;
	//為了不撞墻用needTurn對(duì)headingTo進(jìn)行修正
	needTurn = getNeedTurn(headingTo);
	headingTo -= needTurn;
			
	bea = bearing(headingTo, getHeading());
	turn(bea);
}

void getCenter(double* x, double* y)
{
	struct Bot* bot = getFirstOpponent();
	if(bot==NULL)
	{
		*x = getCourtWidth()/2;
		*y = getCourtHeight()/2;
	}
	else
	{
		*x = bot->x;
		*y = bot->y;
	}
}

double getDeltaAngle(double centerX, double centerY)
{
	double dis = distance(centerX, centerY, getX(), getY());
	double deltaDistance = dis-PREFER_DISTANCE;
	//deltaDistance = deltaDistance/fmax(distance, PREFER_DISTANCE);
	deltaDistance = deltaDistance/500;
	return direction*PI/3*deltaDistance;
}

double getNeedTurn(double headingTo)
{
	double needTurn = 0, nextX, nextY;
	//進(jìn)入迭代
	while(1)
	{
		nextPoint(getX(), getY(), headingTo-needTurn, 
			direction*SEARCH_DIS, &nextX, &nextY);
		if(isValid(nextX, nextY)) break;
		needTurn += direction*GAP;
	}
	return needTurn;
}

int isValid(double x, double y)
{
	return inCourt(x, y, AWAY_FROM_WALL);
}
		
//啟動(dòng)機(jī)器人程序
int main(int argC, char* argV[])
{
	tickHook = onTick;
	return startup(argC, argV);
}