#include "Robot.h"

//用戶設置的想要轉動的轉角
static double angle = 0;
//雷達的當前方向
static double heading = 0;
//最大轉動速度
static const double MAX_VELOCITY_TURN = (45 * 3.14 / 180);

/////////////////////////////////////////////////////////////////////
/*
 * 設置所要轉動的角度,用于控制雷達的方向
 * 注意：當參數angle為正的時候，讓雷達逆時針方向旋轉
 * 當參數angle為負的時候，讓雷達順時針方向旋轉
 * 雷達的最大轉動速度為45度/單位時間，弧度就是（PI/4）/單位時間
 */
void turnRadar(double a)
{
    angle = a; 	
}

/*
 * 得到用戶想用轉動的角度的剩余量
 * 比如：上個單位時間，用戶用turn(Math.toRadians(50))讓雷達旋轉50度
 * 但是雷達的最大旋轉速度為45度/單位時間，所以這個單位時間調用getTurnRemaining()
 * 會得到返回值0.08754(5/180*PI),說明還有5度需要轉動，如果用戶此時沒有新的turn函數的調用
 * 那么下個單位時間雷達將會只轉動5度
 */
double getRadarTurnRemaining()
{
   return angle;
}
	
/*
 * 得到當前的雷達方向
 */
double getRadarHeading()
{
	return heading;
}
	
/*
 * 設置當前方向
 */
void setRadarHeading(double h)
{
    heading = h;

	//重新計算要轉的角度
    if (angle >= 0)
	{
	    if( angle < MAX_VELOCITY_TURN )
			angle = 0;
		else angle -= MAX_VELOCITY_TURN;
	}else{
		if( angle > - MAX_VELOCITY_TURN )
		    angle = 0;
		else angle += MAX_VELOCITY_TURN;
	}
}