//
// ファイル名 : 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"
//#include "Speaker.h"
//using namespace ecrobot;

#include "UI.h"
#include "Driver.h"
#include "TailControl.h"
//=============================================================================
// デバイスオブジェクトの定義
TouchSensor touch(PORT_4);
Clock clock;
//Lcd lcd;
//Nxt nxt;
//Speaker speaker;

LightSensor* pLightSensor = new LightSensor(PORT_3);
GyroSensor* pGyroSensor = new GyroSensor(PORT_1);
Motor* pMotorT = new Motor(PORT_A);
Motor* pMotorR = new Motor(PORT_B);
Motor* pMotorL = new Motor(PORT_C);
Nxt* pNxt = new Nxt();

//for Debug
Lcd* pLcd = new Lcd();

UI ui;

bool bStartFlag;

extern "C"
{
//#include "kernel.h"
//#include "kernel_id.h"
//#include "ecrobot_interface.h"
//#include "Monitor.h"
//#include "stdio.h"
//=============================================================================
// TOPPERS/ATK1の宣言
DeclareCounter(SysTimerCnt);
DeclareAlarm(Alarm4msec);
DeclareEvent(EventDrive);

// タスクの起動周期[msec]
#define DRIVE_PERIOD       (4)
#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); // ･ﾉ･鬣､･ﾖﾍﾑAlarm･ｿ･､･ﾞ｡ｼ､ﾎ･ｻ･ﾃ･ﾈ

	bool bOKNGflag = false;
	//ui.set_ssDriver(&ssDriver);	//XXX fujino
	//ui.set_ltDriver(&ltDriver);	//XXX fujino
	ui.set_TouchSensor(&touch);
	ui.set_LightSensor(pLightSensor);
	do{
		
		//
		// Cource select
		//
		ui.printCourceSelect();
		bool bInOutFlag = ui.selectBotton("In","Out");
		
		//
		// DifficultStage select
		//
		ui.printDifficultStage();
		bool bDifficultStageFlag = ui.selectBotton("Exec","None");

		//
		// OK/NG select
		//
		ui.printResult(bInOutFlag, bDifficultStageFlag);
		bOKNGflag = ui.selectOKNG();
		ui.printResult(bInOutFlag, bDifficultStageFlag);

	}while(!bOKNGflag);

//	ui.printcalibration();
	
	//XXX fujino
	// ui.waitForTouchSenserOn();
	bStartFlag = false;
	while(1)
	{
		//m_ssDriver->checkInput(m_touch->isPressed());
		// タッチセンサの状態を検出
		if (touch.isPressed() == true){

			pLcd->putf("s", "TOUCH:On");
			pLcd->disp();
			
			bStartFlag = (bStartFlag == false) ? true : false;
		} 
		clock.wait(BACKGROUND_PERIOD);
	}
	
}

//=============================================================================
// タスク名　: TaskDrive
// 概要	　: ドライブタスク
//=============================================================================
TASK(TaskDrive)
{
	TailControl* pTailControl = new TailControl(pMotorT);
	Driver* pDriver = new Driver(pLightSensor, pGyroSensor, pMotorL, pMotorR, pNxt, pLcd, pTailControl);
	
	CancelAlarm(Alarm4msec); 
	SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD); // ドライブ用Alarmタイマーのセット
	while(1){
        ClearEvent(EventDrive); // ドライブイベントのクリア
		WaitEvent(EventDrive);  // ドライブイベント待ち
		pTailControl->tail_control(TAIL_ANGLE_STAND_UP);
		if (bStartFlag == true){
			pDriver->run();
		}
	}
	
	delete pTailControl;
	delete pLightSensor;
	delete pGyroSensor;
	delete pMotorT;
	delete pMotorR;
	delete pMotorL;
	delete pNxt;
}
};
