//
// ファイル名 : 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"

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

GyroSensor gyro(PORT_1);
SonarSensor sonar(PORT_2);
LightSensor light(PORT_3);
TouchSensor touch(PORT_4);
Motor motorT(PORT_A);
Motor motorR(PORT_B);
Motor motorL(PORT_C);
Nxt nxt;
Lcd lcd;
Clock clock;

CourseMgr courseMgr;
TailControl tailControl(&motorT);
DriverMgr driverMgr(&light, &gyro, &sonar, &motorL, &motorR, &nxt, &lcd, &tailControl, &courseMgr);
UI ui(&light, &touch, &sonar, &nxt, &lcd, &driverMgr);

Em_CourseKind::eCourseKind eInOutFlag = Em_CourseKind::NONE;
bool bDifficultStageFlag;

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

// タスクの起動周期[msec]
#define BACKGROUND_PERIOD (48)
#if defined(ENABLE_BT_LOG) || defined(ENABLE_BT_START)
// Bluetoothのデバイス名（"ET" + 2011年度 宇部・レーシング のチームID "158"）
#define DEVICE_NAME     "ET158"
// Bluetoothのパスキー（規約の関係上、NXTのデフォルト "1234" 以外を設定する必要あり）
#define PASS_KEY  "uis_robocon"
#endif

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

//=============================================================================
// タスク名　: TaskMain
// 概要	　: メインタスク
//=============================================================================
TASK(TaskMain)
{
	
	SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD);
	
	ui.selectCource(eInOutFlag, bDifficultStageFlag);
	
	ui.printResult(eInOutFlag, bDifficultStageFlag);
	
	courseMgr.createCourse(eInOutFlag);

//	ui.printcalibration();

	ui.waitForTouchSenserOn();

	
}

//=============================================================================
// タスク名　: TaskDrive
// 概要	　: ドライブタスク
//=============================================================================
TASK(TaskDrive)
{
	driverMgr.startDrive();
}
};


//=============================================================================
// タスク名　: ecrobot_device_initialize
// 概要	　: デバイス初期化用フック関数。nxtOSEK起動時に実行される。
//=============================================================================
void ecrobot_device_initialize(void)
{
#if defined(ENABLE_BT_LOG) || defined(ENABLE_BT_START)
	ecrobot_init_bt_slave(PASS_KEY);
#endif

}

//=============================================================================
// タスク名　: ecrobot_device_terminate
// 概要	　: デバイス終了用フック関数。STOPまたはEXITボタンが押された時に実行される。
//=============================================================================
void ecrobot_device_terminate(void)
{
#if defined(ENABLE_BT_LOG) || defined(ENABLE_BT_START)
	ecrobot_term_bt_connection();
#endif
	driverMgr.notifyTaskEnd();
	ui.notifyTaskEnd();
	clock.wait(100);
	
	lcd.clear();
	lcd.putf("sn", "Process End!!");
	lcd.disp();	
	
	clock.wait(1000);

}

