///////////////////////////////////////////////////////////
//  DriverMgr.cpp
//  Implementation of the Class DriverMgr
//  Created on:      14-7-2011 17:15:33
//  Original author: u90499
///////////////////////////////////////////////////////////

#include "DriverMgr.h"
#include "Course.h"
#include "stdio.h"
DriverMgr::	DriverMgr(LightSensor* pLightSensor, GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, Lcd* pLcd, TailControl* pTailControl,CourseMgr* pCourseMgr){
	m_pDriver = new Driver(pLightSensor, pGyroSensor, pMotorL, pMotorR, pNxt, pLcd, pTailControl);
	m_bStartFlag = false;
	m_pTailControl = pTailControl;
	m_pCourseMgr = pCourseMgr;
	m_pMotorR = pMotorR;
}

DriverMgr::~DriverMgr(){
	delete m_pDriver;
}

void DriverMgr::startDrive(){
	CancelAlarm(Alarm4msec); 
	SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD); // ドライブ用Alarmタイマーのセット
		
/*	float fPI = 2.0 * asin(1);
	float fWRadius     = 40;
	F32 fDist          = 0.0;
	F32 fTarget        = 0.0;*/
	st_section sect;
	sect.isectionID = 99;
	sect.lDistance_mm = 0;
	sect.lSpeed       = 50;
	sect.Position.mX = 0;
	sect.Position.mY = 0;
	sect.bLineTrace   = true;

	m_pDriver->setRunWay(sect);

	while(1){
        ClearEvent(EventDrive); // ドライブイベントのクリア
		WaitEvent(EventDrive);  // ドライブイベント待ち
		if (m_bStartFlag == false){
			m_pTailControl->tail_control(TAIL_ANGLE_STAND_UP);
		}else{
			m_pTailControl->tail_control(TAIL_ANGLE_DRIVE);
			m_pDriver->run();
/*			if(fDist > fTarget) {
				if (m_pCourseMgr->getNextSection(sect) == true) {
					fTarget += sect.lDistance_mm;
					m_pDriver->setRunWay(sect);
				} else {
					m_pDriver->setRunWay(sect);
					fTarget += 10000;
				}
			}
			fDist = fWRadius * fPI * static_cast<F32>(m_pMotorR->getCount()) / 180.0;
*/
		}
		
	}

}

void DriverMgr::createDriver(){
	

}
