//
// StandControl.cpp
//

#include "Lcd.h"
#include "StandControl.h"
//=============================================================================
StandControl::StandControl(GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, PositionControl* pPositionControl)
	: m_pGyroSensor(pGyroSensor), 
	  m_pMotorL(pMotorL), 
	  m_pMotorR(pMotorR), 
	  m_pNxt(pNxt),
	  m_pPositionControl(pPositionControl)
{
	//calGyroOffset();
}

StandControl::~StandControl(){
}

void StandControl::standRun(VectorT<S16> command){

	VectorT<S8> motorPwm = this->calcPWM(command);

	m_pMotorL->setPWM(motorPwm.mX);
	m_pMotorR->setPWM(motorPwm.mY);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}

VectorT<S8> StandControl::calcPWM(VectorT<S16> cmd){

	S16 gyroData = m_pGyroSensor->get();
	S16 battMv = m_pNxt->getBattMv();
	VectorT<S32> motorCount;
	motorCount.mX = m_pMotorL->getCount();
	motorCount.mY = m_pMotorR->getCount();

	VectorT<S8> pwm;

	balance_control(
		static_cast<F32>(cmd.mX),
		static_cast<F32>(cmd.mY),
		static_cast<F32>(gyroData),
		static_cast<F32>(m_offset),
		static_cast<F32>(motorCount.mX),
		static_cast<F32>(motorCount.mY),
		static_cast<F32>(battMv),
		&pwm.mX,
		&pwm.mY);

	return pwm;
}


void StandControl::lean(VectorT<S16> cmd)
{
	m_pMotorL->setPWM(cmd.mX);
	m_pMotorR->setPWM(cmd.mX);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}
	
void StandControl::leanRun(VectorT<S16> cmd)
{
	
	VectorT<S8> pwm;
//	S16 gyroData = m_pGyroSensor->get();
	
	Lcd lcd;
	lcd.clear();
	lcd.putf("sdn",  "x: ", static_cast<S32>(cmd.mY),0);
	lcd.disp();

	if (cmd.mY > 650){
		pwm.mX = cmd.mX;
		pwm.mY = 0;
	} else {
		pwm.mX = 0;
		pwm.mY = cmd.mX;
	}
	
	m_pMotorL->setPWM(pwm.mX);
	m_pMotorR->setPWM(pwm.mY);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}

void StandControl::calGyroOffset(void)
{
	U32 calGyroOffset = 0;
	SINT numOfAvg = 0;
	while (numOfAvg < 100) // it takes approximately 4*100 = 400msec
	{
		calGyroOffset += m_pGyroSensor->get();
		numOfAvg++;
		U32 tic = systick_get_ms();
		while (systick_get_ms() < 4 + tic); // wait for 4msec
	}

	m_offset = static_cast<S16>(calGyroOffset/numOfAvg);
}

void StandControl::changeDirection(S8 pwm)
{	
	m_pMotorL->setPWM(pwm);
	m_pMotorR->setPWM(-pwm);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}

void StandControl::leanRunToGoal(VectorT<S16> cmd)
{
	
	VectorT<S8> pwm;

	if (cmd.mY > 650){
		pwm.mX = cmd.mX;
		pwm.mY = 0;
	} else {
		pwm.mX = 0;
		pwm.mY = cmd.mX;
	}
	
	m_pMotorL->setPWM(pwm.mX);
	m_pMotorR->setPWM(pwm.mY);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}
