/**
 * 圍繞對手作來回的移動，并在此基礎上添加了與對手保持一定距離，不撞墻的功能
 * @author xiemin
 */
#include <airobot/c/SimpleRobot.h>

//在一個方向上最大的移動時間
#define MOVE_TIME 25
//理想距離
#define PREFER_DISTANCE 300
//計算目標點時采用的搜索半徑
#define SEARCH_DIS 100
//每次迭代的遞增量
#define GAP toRadians(5)
//離墻的距離
#define AWAY_FROM_WALL 50

//移動的方向,這個變量的值只能取1和-1，1表示前進，-1表示后退
int direction = 1;
//當前在一個方向上的移動時間
long moveTime = 0;

//設置移動的方向
void setDirection(void);
//執行移動
void doMove(void);
//執行轉動
void doTurn(void);
//得到圓心位置
void getCenter(double* x, double* y);
//得到用于保持理想距離的修正角deltaAngle
double getDeltaAngle(double centerX, double centerY);
//得到用來修正移動方向避免機器人撞墻的修正角needTurn
double getNeedTurn(double headingTo);
//判斷給定的點是否是一個有效的目標點
int isValid(double x, double y);

void onTick(struct TickAction* action)
{
	setDirection();
	doMove();
	doTurn();
}
	
void setDirection(void)
{
	moveTime++;
	if(moveTime>MOVE_TIME)
	{
		direction *= -1; //變換移動方向
		moveTime = 0;
	}
}

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對headingTo進行修正
	deltaAngle = getDeltaAngle(centerX, centerY);
	headingTo -= deltaAngle;
	//為了不撞墻用needTurn對headingTo進行修正
	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;
	//進入迭代
	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);
}
		
//啟動機器人程序
int main(int argC, char* argV[])
{
	tickHook = onTick;
	return startup(argC, argV);
}