//
// PositionControl.cpp
//
#include "math.h"
#include "PositionControl.h"
//=============================================================================
PositionControl::PositionControl(Motor* pMotorL, Motor* pMotorR, Lcd* pLcd)
	: m_pMotorL(pMotorL), 
	  m_pMotorR(pMotorR),
	  m_pLcd(pLcd)	//for Debug
{
	
	//位置計測情報に必要なもの(dWRadius, dWIntervalは走行体を実測して調整する)
	m_dX = 0.0;
	m_dY = 0.0;
	m_dW = 0.0;
/*
	m_dX = 0.0;//2.30;//0.0;
	m_dY = 0.0;//1.50;//0.0
	m_dW = 0.0;//2 * asin(1);//0.0;
*/
	m_dPI = 2 * asin(1);
	m_dWRadius = 0.04; 		// 車輪半径(m)
	m_dWInterval = 0.16;	// 車輪間隔(m)
	m_lLastLRotAngle = 0;
	m_lLastRRotAngle = 0;
	m_lSampling = 0;		// 計測間隔
#ifdef ENABLE_BT_LOG
	m_iLogCnt = 0;
	m_dsx0 = 0;
	m_dsy0 = 0;
#endif

}

PositionControl::~PositionControl()
{
}

void PositionControl::updateCurrentPosition()
{

#ifdef ENABLE_BT_LOG
	S32 sDiff_X = 0;
	S32 sDiff_Y = 0;
#endif
	if(m_lSampling > 50){
		
		//タイヤの回転量を計算
		S32 lMoveL = m_pMotorL->getCount() - m_lLastLRotAngle;
		S32 lMoveR = m_pMotorR->getCount() - m_lLastRRotAngle;

		float dMoveL = m_dPI * m_dWRadius * (F32)(lMoveL) / 180.0;
		float dMoveR = m_dPI * m_dWRadius * (F32)(lMoveR) / 180.0;
		
		float dVt =  (dMoveR + dMoveL) / 2.0;
		float dWt =  (dMoveR - dMoveL) / m_dWInterval;
		
		//位置計算
		if (lMoveR == lMoveL){
			//直進
			m_dX = m_dX + dVt * cos(m_dW);
			m_dY = m_dY + dVt * sin(m_dW);
			
		}else{
			//カーブ
			m_dX = m_dX + 2 * (dVt / dWt) * sin(dWt / 2) * cos(m_dW + dWt / 2);
			m_dY = m_dY + 2 * (dVt / dWt) * sin(dWt / 2) * sin(m_dW + dWt / 2);
		}
		m_dW = m_dW + dWt;

		//
		m_lLastLRotAngle = m_pMotorL->getCount();
		m_lLastRRotAngle = m_pMotorR->getCount();

#ifdef ENABLE_BT_LOG
		sDiff_X = static_cast<S32>(100 * m_dX) - m_dsx0;
		sDiff_Y = static_cast<S32>(100 * m_dY) - m_dsy0;

		m_dsx0 = static_cast<S32>(100 * m_dX);
		m_dsy0 = static_cast<S32>(100 * m_dY);	
		ecrobot_bt_data_logger(static_cast<S8>(sDiff_X), static_cast<S8>(sDiff_Y));
#endif
		m_lSampling = 0;

	}else{
		m_lSampling++;
	}

	//モニタに表示
	m_pLcd->clear();
	S32 sx = static_cast<S32>(100 * m_dX);				// cm
	S32 sy = static_cast<S32>(100 * m_dY);				// cm
	S32 sw = static_cast<S32>(180 * m_dW / m_dPI);		// 度表示

	m_pLcd->putf("sdn",  "x: ", sx,0);
	m_pLcd->putf("sdn",  "y: ", sy,0);
	m_pLcd->putf("sdn",  "w: ", sw,0);
	m_pLcd->disp();

}

VectorT<S32> PositionControl::getCurrentPosition(){

	VectorT<S32> currentPosition;
	currentPosition.mX = static_cast<S32>(100 * m_dX);;
	currentPosition.mY = static_cast<S32>(100 * m_dY);
	return currentPosition;
}


float PositionControl::calcDirectTarget(float dTPosX, float dTPosY, bool& bRightTurn){
	//現在位置の走行体から目標物へのどちら向きに回転をすればよいか計算する
	
	//まず、走行体の位置で、スタート時と同じ方向を向いた状態から目標物への角度を求める
	float dDisX = dTPosX - m_dX;
	float dDisY = dTPosY - m_dY;
	
	float dDis = sqrt(dDisX * dDisX + dDisY * dDisY);
	if(fabs(dDis) < 0.001){
		//目標物と同じ位置にいれば0とする。
		return m_dW;
	}
	
	float dTheta = acos(dDisX / dDis);
	if(dDisY < 0.0){
		dTheta = 2 * m_dPI - dTheta;
	}
	
	//走行体の向きと上記結果の差分をとって、回転すべき角度を計算する。(＋は右回転)
	
	if ((180 * dTheta / m_dPI) > 270){
		dTheta = 0;
	}
	
	if (m_dW - dTheta > 0){
		bRightTurn = true;
	} else {
		bRightTurn = false;
	}

/*
	m_pLcd->clear();
	S32 sx = static_cast<S32>(100 * m_dX);				// cm
	S32 sy = static_cast<S32>(100 * m_dY);				// cm
	S32 sw = static_cast<S32>(180 * m_dW / m_dPI);		// 度表示
	S32 tx = static_cast<S32>(100 * dTPosX);				// cm
	S32 ty = static_cast<S32>(100 * dTPosY);				// cm
	S32 tw = static_cast<S32>(180 * dTheta / m_dPI);		// 度表示

	m_pLcd->putf("sdn",  "x: ", sx,0);
	m_pLcd->putf("sdn",  "y: ", sy,0);
	m_pLcd->putf("sdn",  "w: ", sw,0);
	m_pLcd->putf("sdn",  "tx: ", tx,0);
	m_pLcd->putf("sdn",  "ty: ", ty,0);
	m_pLcd->putf("sdn",  "tw: ", tw,0);

	m_pLcd->disp();
*/
	return dTheta;
}

bool PositionControl::isTurnExec(float fTargetAngle){

	//if (fabs((180 * fabs(m_dW) / m_dPI) - 30) < 5){
	if ((180 * fabs(m_dW - fTargetAngle) / m_dPI) < 5){
		return false;
	}
	
	return true;
}
	
void PositionControl::reset(){
	m_dX = 0;
	m_dY = 0;
	m_dW = 0;
}


void PositionControl::setPosition(float dX, float dY, float dW){
	m_dX = dX;
	m_dW = dW;
}

