//
// ファイル名 : Main.cpp
// 概要       : 2輪倒立振子ライントレースロボットのTOPPERS/ATK1(OSEK)用C++サンプルプログラム
//

//#include "math.h"

// ECRobot C++ API
#include "TouchSensor.h"
//#include "SonarSensor.h"
#include "LightSensor.h"
#include "GyroSensor.h"
#include "Motor.h"
#include "Nxt.h"
#include "Clock.h"
#include "Lcd.h"
//using namespace ecrobot;

#include "UI.h"
#include "Driver.h"
#include "DriverMgr.h"
#include "TailControl.h"
#include "CourseMgr.h"
#include "Em_CourseKind.h"

//=============================================================================
// デバイスオブジェクトの定義

Clock clock;

GyroSensor gyro(PORT_1);
LightSensor light(PORT_3);
TouchSensor touch(PORT_4);
Motor motorT(PORT_A);
Motor motorR(PORT_B);
Motor motorL(PORT_C);


Nxt nxt;
Lcd lcd;
UI ui;
CourseMgr courseMgr;

bool bInOutFlag = false;
bool bDifficultStageFlag;

extern "C"
{
//=============================================================================

// タスクの起動周期[msec]
#define BACKGROUND_PERIOD (48)

//=============================================================================
// 関数名	: user_1ms_isr_type2
// 引数	: なし
// 戻り値	: なし
// 概要	: 1msec周期割り込みフック関数(OSEK ISR type2カテゴリ)
//=============================================================================
void user_1ms_isr_type2(void)
{
	(void)SignalCounter(SysTimerCnt); // Alarmハンドラ
	SleeperMonitor(); // NxtおよびI2Cデバイスの使用時に必要
}

//=============================================================================
// タスク名　: TaskMain
// 概要	　: メインタスク
//=============================================================================
TASK(TaskMain)
{
	
	SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD);

	ui.set_TouchSensor(&touch);
	ui.set_LightSensor(&light);
	
	ui.selectCource(bInOutFlag, bDifficultStageFlag);
	courseMgr.createCourse(static_cast<Em_CourseKind::eCourseKind>(bInOutFlag));

//	ui.printcalibration();

	//XXX fujino
	ui.waitForTouchSenserOn();

	
}

//=============================================================================
// タスク名　: TaskDrive
// 概要	　: ドライブタスク
//=============================================================================
TASK(TaskDrive)
{
	TailControl* pTailControl = new TailControl(&motorT);
	
	DriverMgr* pDriverMgr = new DriverMgr(&light, &gyro, &motorL, &motorR, &nxt, &lcd, pTailControl, &courseMgr);
	ui.set_DriverMgr(pDriverMgr);

	pDriverMgr->startDrive();

	
	delete pTailControl;
}
};
