//
// Driver.cpp
//

#include "Driver.h"
#include "Vector.h"

//=============================================================================
Driver::Driver(LightSensor* pLightSensor, GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, Lcd* pLcd, TailControl* pTailControl){
	m_pPIDCtrl = new PIDCtrl(pLightSensor, 590, 0.7, 0.5, 13.0);
	m_pPositionControl = new  PositionControl(pMotorL, pMotorR, pLcd);
	m_pStandControl = new StandControl(pGyroSensor, pMotorL, pMotorR, pNxt, m_pPositionControl);
	m_pTailControl = pTailControl;
}

Driver::~Driver(){
	delete m_pPIDCtrl;
	delete m_pStandControl;
}

void Driver::setRunWay(st_section& sect){
	m_cntEndPosition = sect.Position;
	m_speed = sect.lSpeed;


}

void Driver::run(){
	S16 PIDCalcValue = m_pPIDCtrl->calcVal();
	VectorT<S16> command = VectorT<S16>(m_speed, PIDCalcValue);
	m_pStandControl->standRun(command);
}

bool Driver::isSenctionEnd(){
	VectorT<S32> currentPosition = m_pPositionControl->getCurrentPosition();
	if((currentPosition.mX == m_cntEndPosition.mX) &&
		(currentPosition.mY == m_cntEndPosition.mY)){
		return true;
	}

	return false;
}
