///////////////////////////////////////////////////////////
//  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"
#include "Lcd.h"
DriverMgr::	DriverMgr(LightSensor* pLightSensor, GyroSensor* pGyroSensor,SonarSensor* pSoner, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, Lcd* pLcd, TailControl* pTailControl,CourseMgr* pCourseMgr){
	m_pDriver = new Driver(pLightSensor, pGyroSensor, pSoner,pMotorL, pMotorR, pNxt, pLcd, pTailControl);
	m_bStartFlag = false;
	m_pTailControl = pTailControl;
	m_pCourseMgr = pCourseMgr;
	m_pMotorR = pMotorR;
	m_bTaskActive = true;
	m_bDoObstaclesExist = false;
}

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.EndPosition.mX = -100;
	sect.EndPosition.mY = -100;
	sect.isectionID = 99;
	sect.lSpeed       = 0;
	sect.iXY   = Em_SectionChange::XPlus;
	sect.bPIDExec = true;
	sect.bSonerUse   = false;
	m_pDriver->setRunWay(sect);

	bool bInitFlag = true;
/*
#ifdef ENABLE_BT_LOG
	S8 iLogCnt = 0;
#endif
*/
	
	int iTailCnt = 0;
	while(m_bTaskActive == true){
        ClearEvent(EventDrive); // ドライブイベントのクリア
		WaitEvent(EventDrive);  // ドライブイベント待ち
		if (m_bStartFlag == false){
			m_pTailControl->tail_control(TAIL_ANGLE_STAND_UP);
		}else{
			
			if(bInitFlag == true){
				bInitFlag = false;
				m_pDriver->initialize();
//				m_pTailControl->tail_control(TAIL_ANGLE_DRIVE);
				m_pTailControl->tail_control(TAIL_ANGLE_START);
				iTailCnt = 1;
				m_pDriver->setCourseKind(m_pCourseMgr->getCourseKind());
			} else if ((iTailCnt >= 1) && (iTailCnt < 5)){
				m_pTailControl->tail_control(TAIL_ANGLE_START);
				iTailCnt++;
			} else if (iTailCnt >= 5){
				m_pTailControl->tail_control(TAIL_ANGLE_DRIVE);
				iTailCnt = 0;
			}
			
			m_pDriver->notifyObstaclesExist(m_bDoObstaclesExist);
			
			m_pDriver->run();
			if(m_pDriver->isSenctionEnd()) {
				if (m_pCourseMgr->getNextSection(sect) == true) {
					m_bDoObstaclesExist = false;
					m_pDriver->setRunWay(sect);
				} else {
					sect.EndPosition.mX = 0;
					sect.EndPosition.mY = 99999;
					sect.iXY   = Em_SectionChange::YPlus;
					sect.lSpeed       = 0;
					m_pDriver->setRunWay(sect);
				}
			}
/*
#ifdef ENABLE_BT_LOG
			if (iLogCnt <= 0) {
				ecrobot_bt_data_logger((S8)sect.isectionID, (S8)sect.lSpeed);
				iLogCnt = 25;	//4 msec * 25 = 0.1 sec
			} else {
				iLogCnt--;
			}
#endif
*/
		}
	}

}

void DriverMgr::createDriver(){
	

}
