//
// 僼傽僀儖柤 : 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 "Driver.h"
#include "TailControl.h"
//#include "NXTway_GS.h"
//#include "Driver.h"
//#include "StartStopDriver.h"
//#include "SonarDriver.h"
//#include "LineTraceDriver.h"
//#include "DriverManager.h"

//#include "SectionKnowledge.h"
//=============================================================================
// 僨僶僀僗僆僽僕僃僋僩偺掕媊
TouchSensor touch(PORT_4);
//SonarSensor sonar(PORT_2);

//LightSensor light(PORT_3);
//GyroSensor   gyro(PORT_1);
//Motor      motorT(PORT_A);
//Motor      motorR(PORT_B);
//Motor      motorL(PORT_C);
//Nxt nxt;
Clock clock;

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

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();

bool bStart;

// 暋悢僞僗僋偐傜僐乕儖偝傟傞僪儔僀僶
//StartStopDriver ssDriver;
//SonarDriver sonarDriver;

extern "C"
{
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"
//#include "ConfigConst.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ハンドE
	SleeperMonitor(); // Nxt偍傛傃I2C僨僶僀僗偺巊梡帪偵昁梫
}

//=============================================================================
// 僞僗僋柤丂: TaskMain
// 奣梫	丂: 儊僀儞僞僗僋
//=============================================================================
TASK(TaskMain)
{
	pLcd->clear();
	pLcd->putf("s", "TOUCH:START/STOP");
	pLcd->disp();
	
	SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD); // 僪儔僀僽梡Alarm僞僀儅乕偺僙僢僩
	
	bStart = false;
	
	while(1)
	{
		// 僞僢僠僙儞僒偺忬懺傪専弌
		if (touch.isPressed() == true){
			
			pLcd->putf("s", "TOUCH:On");
			pLcd->disp();
			
			if (bStart == false){
				bStart = true;
			} else {
				bStart = false;
			}
			
		} 
		//sonarDriver.checkObstacles(sonar); // 挻壒攇僙儞僒偵傛傞忈奞暔専弌

		clock.wait(BACKGROUND_PERIOD); // 僂僃僀僩儖乕僾
	}
}

//=============================================================================
// 僞僗僋柤丂: TaskDrive
// 奣梫	丂: 僪儔僀僽僞僗僋
//=============================================================================
TASK(TaskDrive)
{
/*
	LineTraceDriver ltDriver(light);
		
	DriverManager drivers;
	//(void)drivers.createDriverTable(3);
	(void)drivers.createDriverTable(2);
	(void)drivers.add(&ssDriver); // 嵟崅桪愭搙
	//(void)drivers.add(&sonarDriver);
	(void)drivers.add(&ltDriver); // 嵟掅桪愭搙
	
	//埵抲寁應忣曬偵昁梫側傕偺(dWRadius, dWInterval偼憱峴懱傪幚應偟偰挷惍偡傞)
	float dX = 0.0;
	float dY = 0.0;
	float dW = 0.0;
	float dPI = 2 * asin(1);
	float dWRadius = 0.04; 		// 幵椫敿宎(m)
	float dWInterval = 0.16;	// 幵椫娫妘(m)
	S32   lLastLRotAngle = 0;
	S32   lLastRRotAngle = 0;
	long  lSampling = 0;		// 寁應娫妘

	NXTway_GS robot(nxt, gyro, motorL, motorR);

	while(1)
	//while(know.next(sect))
	{
        ClearEvent(EventDrive); // 僪儔僀僽僀儀儞僩偺僋儕傾
		WaitEvent(EventDrive);  // 僪儔僀僽僀儀儞僩懸偪

		drivers.notify(robot.readInternalData()); // 憱峴懱偺撪晹僨乕僞傪僪儔僀僶乕傊捠抦
		drivers.update(); // 僪儔僀僶儕僋僄僗僩傪庢摼偡傞慜偵峏怴
		switch(drivers.getRequest())
		{
			case Driver::STOP:
				robot.stop();
				break;
			case Driver::START:
				CancelAlarm(Alarm4msec);            // 僪儔僀僽梡Alarm僞僀儅乕偺僉儍儞僙儖

				robot.reset(robot.calGyroOffset()); // 僕儍僀儘僙儞僒偺僉儍儕僽儗乕僔儑儞偵栺400msec偐偐傞
				drivers.reset();
				//speaker.playTone(440U, 500U, 30U);  // 儕僙僢僩姰椆捠抦僽僓乕

				//埵抲偺弶婜壔
				dX = 0.0;
				dY = 0.0;
				dW = 0.0;
				lLastLRotAngle = 0;
				lLastRRotAngle = 0;
				lSampling = 0;

				ltDriver.setSpeed(50);

				SetRelAlarm(Alarm4msec, 1, DRIVE_PERIOD); // 僪儔僀僽梡Alarm僞僀儅乕偺僙僢僩
				break;
			case Driver::DRIVE: 
			
				//偙傟偼僒儞僾儖丅
				if(dY < -0.3){
					ltDriver.setSpeed(80);
				}else if(dX < -0.3){
					ltDriver.setSpeed(20);
				}else{
					ltDriver.setSpeed(40);
				}
			
				robot.drive(drivers.getCommand());
				break;
			case Driver::NO_REQ:
			default:
				break; // Do nothing
		}

		if(lSampling > 50){
			
			//僞僀儎偺夞揮検傪寁嶼
			S32 lMoveL = motorL.getCount() - lLastLRotAngle;
			S32 lMoveR = motorR.getCount() - lLastRRotAngle;

			float dMoveL = dPI * dWRadius * (F32)(lMoveL) / 180.0;
			float dMoveR = dPI * dWRadius * (F32)(lMoveR) / 180.0;
			
			float dVt =  (dMoveR + dMoveL) / 2.0;
			float dWt =  (dMoveR - dMoveL) / dWInterval;
			
			//埵抲寁嶼
			if (lMoveR == lMoveL){
				//捈恑
				dX = dX + dVt * cos(dW);
				dY = dY + dVt * sin(dW);
				
			}else{
				//僇乕僽
				dX = dX + 2 * (dVt / dWt) * sin(dWt / 2) * cos(dW + dWt / 2);
				dY = dY + 2 * (dVt / dWt) * sin(dWt / 2) * sin(dW + dWt / 2);
			}
			dW = dW + dWt;

			//
			lLastLRotAngle = motorL.getCount();
			lLastRRotAngle = motorR.getCount();
			
			lSampling = 0;
		}else{
			lSampling++;
		}

		//儌僯僞偵昞帵
		lcd.clear();
		S32 sx = static_cast<S32>(100 * dX);				// cm
		S32 sy = static_cast<S32>(100 * dY);				// cm
		S32 sw = static_cast<S32>(180 * dW / dPI);		// 搙昞帵
		lcd.putf("sdn",  "x: ", sx,0);
		lcd.putf("sdn",  "y: ", sy,0);
		lcd.putf("sdn",  "w: ", sw,0);
		lcd.disp();
	}

*/	
	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 (bStart == true){
			pDriver->run();
		}
	}
	
	delete pTailControl;
	delete pLightSensor;
	delete pGyroSensor;
	delete pMotorT;
	delete pMotorR;
	delete pMotorL;
	delete pNxt;

}
};

