//
// 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_dPI = 2 * asin(1);
	m_dWRadius = 0.04; 		// 車輪半径(m)
	m_dWInterval = 0.16;	// 車輪間隔(m)
	m_lLastLRotAngle = 0;
	m_lLastRRotAngle = 0;
	m_lSampling = 0;		// 計測間隔
}

PositionControl::~PositionControl()
{
}

void PositionControl::updateCurrentPosition()
{
	
	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();
		
		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;
	
	return currentPosition;
}

