/* sample.cpp for TOPPERS/ATK(OSEK) */ 

// ECRobot++ API
#include "Motor.h"
#include "Nxt.h"
#include "Clock.h"
#include "Lcd.h"
#include "stdio.h"
using namespace ecrobot;


extern "C"
{
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

Motor motorA(PORT_B); // brake by defalut
Motor motorB(PORT_C); // brake by defalut

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void)
{
	SleeperMonitor(); // needed for I2C device and Clock classes
}

static bool selectBotton(CHAR chTrueData[],CHAR chFalseData[]){
	Nxt nxt;
	Clock clock;
	Lcd lcd;
	
	bool bflag  = false;
	while(1){
		if (nxt.getButtons() == Nxt::ENTR_ON){
			break;
		}
		
		if (nxt.getButtons() == Nxt::RUN_ON){
			if(bflag == true){
				 bflag  = false;
			}
			else{
				bflag  = true;
			}
			lcd.clear();
			if(bflag == true){
				lcd.putf("sn", chTrueData);
			}else{
				lcd.putf("sn", chFalseData);
			}
			lcd.disp();
		}

		clock.wait(1000);
	}
	return bflag;
}

void printData(CHAR chFirstData[],CHAR chSecondData[], int iDisplayTime){
	Lcd lcd;
	Clock clock;
	
	lcd.clear();
	lcd.putf("sn", chFirstData);
	lcd.putf("sn", chSecondData);
	lcd.disp();
	clock.wait(iDisplayTime);
}



TASK(TaskMain)
{
	
	Nxt nxt;
	Clock clock;

	bool bInOutflag  = true; //in  :true, out:false
	bool bNanshoflag = true; //Exec:true, None:false
	bool bOKNGflag   = true; //OK  :true, NG:false
	
	do{

		
		//
		// In/Out コース選択
		//
		printData("Select Cource","In/Out",1000);
		bInOutflag = selectBotton("In","Out");

		//
		// 難所選択
		//
		printData("Nansho Select","Exec/None",1000);
		bNanshoflag = selectBotton("Exec","None");
		

		//
		// 結果表示
		//
		CHAR chCourceComment[32] = {""};
		CHAR chNanshoComment[32] = {""};
		
		if(bInOutflag == true)	sprintf(chCourceComment,"Cource: In");
		else sprintf(chCourceComment,"Cource: Out");
		
		if(bNanshoflag == true)	sprintf(chNanshoComment,"Nansho: Exec");
		else sprintf(chNanshoComment,"Nansho: None");	

		printData(chCourceComment, chNanshoComment, 5000);

		//
		// OK/NG選択
		//
		printData("Result Select","OK/NG?", 2000);

		bOKNGflag = selectBotton("OK","NG");

		//
		// 再エントリ受付
		//

		printData(chCourceComment, chNanshoComment, 2000);
		
		if(bOKNGflag == true) printData("GO", "GO!",1000);
		else printData("ReENTRY", ";;",1000);
		
		clock.wait(5000);
	
	}while(!bOKNGflag);
}


}
