//
// Driver.cpp
//
#include "Speaker.h"
#include "Driver.h"
#include "Vector.h"
#include "Lcd.h"
#include "Em_SectionChange.h"
#include "math.h"
#include "Course.h"

//=============================================================================
Driver::Driver(LightSensor* pLightSensor, GyroSensor* pGyroSensor, SonarSensor* pSoner,Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, Lcd* pLcd, TailControl* pTailControl){
	m_pPIDCtrl = new PIDCtrl(pLightSensor, 590, 0.7, 0.5, 13.0);
	m_pPositionControl = new  PositionControl(pMotorL, pMotorR, pLcd);
	m_pStandControl = new StandControl(pGyroSensor, pMotorL, pMotorR, pNxt, m_pPositionControl);
	m_pTailControl = pTailControl;
	m_pSoner = pSoner;
	m_lCt = 0;
	m_TargetAngle = 0;
	m_bRightTurn = false;
	m_lMotorReset = 0;
	m_emCourseKind = Em_CourseKind::NONE;
	m_emLookUpRunState = Em_LookUpRunState::idUndef;
	m_emETSumoRunState = Em_ETSumoRunState::idUndef;
}

Driver::~Driver(){
	delete m_pPIDCtrl;
	delete m_pStandControl;
}

void Driver::initialize(){
	m_pStandControl->calGyroOffset();
}


void Driver::setRunWay(st_section& sect){
	m_sect = sect;
	m_cntEndPosition = sect.EndPosition;
	m_speed = sect.lSpeed;
	m_isectionID = sect.isectionID;
	m_iXY = sect.iXY;
	m_bDoObstaclesExist = false;
}

void Driver::run(){
	S16 PIDCalcValue = m_pPIDCtrl->calcVal();
	VectorT<S16> command;
	
	
	
	static bool bPositionInit = true;
	static bool bPIDExec = true;
	static int bStandCount = 0;
	if(m_emCourseKind == Em_CourseKind::IN){
		if (m_isectionID == 8){
			if(bPositionInit == true){
				m_pPositionControl->setPosition(0,0, 3 * asin(1));
				bPositionInit = false;
			}
		}

		if (m_isectionID == 9){
			if(bPIDExec == true){
				if(((180 * m_pPositionControl->get_dW())/(2 * asin(1))) < 175){
	//				Speaker speaker;
	//				speaker.playTone(840U, 500U, 30U);
					bPIDExec = false;
				}
			}
		}else{
			bPIDExec = true;
		}
		
		if(m_isectionID == 12){
			if (bStandCount++ > 1200){
				m_speed = 30;
		//		speaker.playTone(840U, 500U, 30U);
			}
		}

	}
	


	// 右トレース
	if(bPIDExec == true)
		command = VectorT<S16>(m_speed, -1 * PIDCalcValue);
	else command = VectorT<S16>(m_speed, 0);





	if (m_isectionID == 99){
		command = VectorT<S16>(m_speed, 0);
	}	
	
	if ((m_emCourseKind == Em_CourseKind::OUT) || (m_emCourseKind == Em_CourseKind::OUT_NANSYO)){
		
		if (m_isectionID == 11){
			
			if (m_emLookUpRunState == Em_LookUpRunState::idUndef){
				m_emLookUpRunState = Em_LookUpRunState::idStop1;
			}
			
			// 静止
			if (m_emLookUpRunState == Em_LookUpRunState::idStop1){
				command = VectorT<S16>(0, -1 * PIDCalcValue);
				m_pTailControl->tail_control(105);
			}
			// 傾く
			else if (m_emLookUpRunState == Em_LookUpRunState::idLean){
				
				command = VectorT<S16>(0, 0);
				if (m_lCt < 50){
					command.mX = 20;
					m_pTailControl->tail_control(105);
				} else if (m_lCt < 100){
					command.mX = 20;
					m_pTailControl->tail_control(100);
				} else if (m_lCt < 150){
					command.mX = 20;
					m_pTailControl->tail_control(95);
				} else if (m_lCt < 200){
					command.mX = 20;
					m_pTailControl->tail_control(85);
				}
				m_pStandControl->lean(command);
				return;
			}
			// 傾いたまま走行する
			 else if (m_emLookUpRunState == Em_LookUpRunState::idLeanRun){
				
				command = VectorT<S16>(50, m_pPIDCtrl->getlightValue());
				m_pTailControl->tail_control(85);
			 	
				m_pStandControl->leanRun(command);
				return;
			}
			// 止まる
			else if (m_emLookUpRunState == Em_LookUpRunState::idStop2){
				
				command = VectorT<S16>(0, PIDCalcValue);
				m_pTailControl->tail_control(85);
				m_pStandControl->leanRun(command);
				return;
			}
			// 立ち上がる
			else if (m_emLookUpRunState == Em_LookUpRunState::idStand){

				if((m_lCt % 250) < 10){
					m_pTailControl->stand(100);
					command = VectorT<S16>(-10, 0);
					m_pStandControl->lean(command);
				} else {
//					m_pTailControl->tail_control(115);
					m_pTailControl->tail_control(105);
					command = VectorT<S16>(-10, 0);
					m_pStandControl->lean(command);
				}
				return;
			}
			// 一旦停止
			else if (m_emLookUpRunState == Em_LookUpRunState::idStop3){

				m_pTailControl->tail_control(108);
				if (m_lCt < 20){
					command = VectorT<S16>(-10, 0);
					m_pStandControl->lean(command);
					return;
				} 
				command = VectorT<S16>(0, 0);
				m_pStandControl->lean(command);
				return;
			}
		
		// 向きを変えて、ET相撲の最初のターゲットポイントまで移動させる
		} else if ( m_isectionID == 12){
			
			if (m_emETSumoRunState == Em_ETSumoRunState::idUndef){
				m_emETSumoRunState = Em_ETSumoRunState::idStartPosRun;
				m_pPositionControl->reset();
			}
			
			m_pTailControl->tail_control(105);
			
			float fTargetPosX = 0;
			float fTargetPosY = 0;
			// 向きを変える
			if (m_emETSumoRunState == Em_ETSumoRunState::idStartPosRun){
			 	fTargetPosX = ET_SUMO_START_POS_X;
			 	fTargetPosY = ET_SUMO_START_POS_Y;
			 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id1stPosRun){
			 	fTargetPosX = ET_SUMO_TARGET1_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET1_POS_Y;
		 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id2ndPosRun){
			 	fTargetPosX = ET_SUMO_TARGET2_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET2_POS_Y;
		 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id3rdPosRun){
			 	fTargetPosX = ET_SUMO_TARGET3_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET3_POS_Y;
			 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id4thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET4_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET4_POS_Y;
		 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id5thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET5_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET5_POS_Y;
		 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id6thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET6_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET6_POS_Y;
		 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id7thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET7_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET7_POS_Y;
				
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id8thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET8_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET8_POS_Y;
				
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id9thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET9_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET9_POS_Y;
			
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id10thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET10_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET10_POS_Y;
				
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id11thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET11_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET11_POS_Y;
				
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id12thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET12_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET12_POS_Y;
				
			} else if (m_emETSumoRunState == Em_ETSumoRunState::id13thPosRun){
			 	fTargetPosX = ET_SUMO_TARGET13_POS_X;
			 	fTargetPosY = ET_SUMO_TARGET13_POS_Y;
	 	
			} else if (m_emETSumoRunState == Em_ETSumoRunState::idFinalPosRun){
			 	fTargetPosX = ET_SUMO_FINAL_POS_X;
			 	fTargetPosY = ET_SUMO_FINAL_POS_Y;
				VectorT<S32> currentPosition = m_pPositionControl->getCurrentPosition();
				if(currentPosition.mY > (static_cast<S32>(ET_SUMO_FINAL_POS_Y * 100) - 50)){	// TODO
					command = VectorT<S16>(0, PIDCalcValue);
					m_pStandControl->leanRunToGoal(command);
					return;
				}
			// 完全停止
			} else if (m_emETSumoRunState == Em_ETSumoRunState::idStop){
				command = VectorT<S16>(0, 0);
				m_pStandControl->lean(command);
				return;
			}
			
			m_TargetAngle = m_pPositionControl->calcDirectTarget(fTargetPosX, fTargetPosY, m_bRightTurn);
			
			if (m_pPositionControl->isTurnExec(m_TargetAngle) == true){
				S8 pwm = 0;
				if (m_bRightTurn == true){
					pwm = 20;
				}else{
					pwm = -20;
				}
				m_pStandControl->changeDirection(pwm);
					
			} else {
				command = VectorT<S16>(40, 0);
				m_pStandControl->lean(command);
			}
			return;
		}
	}
	
	m_pStandControl->standRun(command);
}
bool Driver::isSenctionEnd(){
	VectorT<S32> currentPosition = m_pPositionControl->getCurrentPosition();
	
	Speaker speaker;
	
	if (m_isectionID == 99){

		if(m_lCt < 50){
			m_lCt++;
			return false;
		} else{
			m_lCt = 0;
			speaker.playTone(840U, 500U, 50U);
			return true;
		}
	}
	
	if ((m_emCourseKind == Em_CourseKind::OUT) || (m_emCourseKind == Em_CourseKind::OUT_NANSYO)){
		
		
		if (m_isectionID == 11){
			
			if (m_emLookUpRunState == Em_LookUpRunState::idUndef){
				m_emLookUpRunState = Em_LookUpRunState::idStop1;
			}
			
			// 走行体を静止させる
			if (m_emLookUpRunState == Em_LookUpRunState::idStop1){
				if(m_lCt < 300){
					m_lCt++;
					return false;
				} else {
					m_lCt = 0;
					speaker.playTone(250U, 500U, 30U);
					m_emLookUpRunState = Em_LookUpRunState::idLean;
					return false;
				}
			}
			
			// 走行体を傾けさせる
			else if (m_emLookUpRunState == Em_LookUpRunState::idLean){
				
				if (m_pTailControl->isAngle(85) == false){
					m_lCt++;
					return false;
				}
				m_lCt = 0;
				speaker.playTone(250U, 500U, 30U);
				m_emLookUpRunState = Em_LookUpRunState::idLeanRun;
				return false;
			}
			// 傾いたまま走行する
			else if (m_emLookUpRunState == Em_LookUpRunState::idLeanRun){
				
				if(currentPosition.mX  > m_cntEndPosition.mX){
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emLookUpRunState = Em_LookUpRunState::idStop2;
					return false;
				}
				return false;
			}
			// 走行体を静止させる
			else if (m_emLookUpRunState == Em_LookUpRunState::idStop2){
			
				if(m_lCt < 401){
					m_lCt++;
					return false;
				} else {
					m_lCt = 0;
					m_emLookUpRunState = Em_LookUpRunState::idStand;
					return false;
				}
			}
			// 走行体を立ち上がらせる
			else if (m_emLookUpRunState == Em_LookUpRunState::idStand){
				if (m_pTailControl->isAngle(105) == false){
					m_lCt++;
					return false;
				}
				speaker.playTone(250U, 500U, 30U);
				m_lCt = 0;
				m_emLookUpRunState = Em_LookUpRunState::idStop3;
				return false;
			}
			// 走行体を一旦静止させる
			else if (m_emLookUpRunState == Em_LookUpRunState::idStop3){
				if(m_lCt < 200){
					m_lCt++;
					return false;
				} 
				m_lCt = 0;
				speaker.playTone(250U, 500U, 30U);
				m_emLookUpRunState = Em_LookUpRunState::idUndef;
				return true;
			}
		} else if (m_isectionID == 12){
			
			if (m_emETSumoRunState == Em_ETSumoRunState::idUndef){
				m_emETSumoRunState = Em_ETSumoRunState::idStartPosRun;
				m_pPositionControl->reset();
			}
			
			// ET相撲の最初の位置に移動する。
			if (m_emETSumoRunState == Em_ETSumoRunState::idStartPosRun){
				if(currentPosition.mX > static_cast<S32>((ET_SUMO_START_POS_X * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id1stPosRun;
					return false;
				}
			}
			
			// ET相撲の1番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id1stPosRun){
				if(currentPosition.mX > static_cast<S32>((ET_SUMO_TARGET1_POS_X * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id2ndPosRun;
					return false;
				}
			}
			// ET相撲の2番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id2ndPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET2_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id3rdPosRun;
					return false;
				}
			}
			// ET相撲の3番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id3rdPosRun){
				if(currentPosition.mX < static_cast<S32>((ET_SUMO_TARGET3_POS_X * 100) + 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id4thPosRun;
					return false;
				}
			}
			// ET相撲の4番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id4thPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET4_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id5thPosRun;
					return false;
				}
			}
			// ET相撲の5番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id5thPosRun){
				if(currentPosition.mX > static_cast<S32>((ET_SUMO_TARGET5_POS_X * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id6thPosRun;
					return false;
				}
			}
			// ET相撲の6番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id6thPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET6_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id7thPosRun;
					return false;
				}
			}
			// ET相撲の7番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id7thPosRun){
				if(currentPosition.mX < static_cast<S32>((ET_SUMO_TARGET7_POS_X * 100) + 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id8thPosRun;
					return false;
				}
			}
			// ET相撲の8番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id8thPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET8_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id9thPosRun;
					return true;
				}
			}
				
			// ET相撲の9番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id9thPosRun){
				if(currentPosition.mX > static_cast<S32>((ET_SUMO_TARGET9_POS_X * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id10thPosRun;
					return false;
				}
			}

			// ET相撲の10番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id10thPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET10_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id11thPosRun;
					return false;
				}
			}
			// ET相撲の11番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id11thPosRun){
				if(currentPosition.mX < static_cast<S32>((ET_SUMO_TARGET11_POS_X * 100) + 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id12thPosRun;
					return false;
				}
			}
			// ET相撲の12番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id12thPosRun){
				if(currentPosition.mY > static_cast<S32>((ET_SUMO_TARGET12_POS_Y * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::id13thPosRun;
					return false;
				}
			}

			// ET相撲の13番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::id13thPosRun){
				if(currentPosition.mX > static_cast<S32>((ET_SUMO_TARGET13_POS_X * 100) - 5)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
//					m_emETSumoRunState = Em_ETSumoRunState::id14thPosRun;
					m_emETSumoRunState = Em_ETSumoRunState::idFinalPosRun;
					return false;
				}
			}
				
			// ET相撲の最終番目の目標位置に移動する。
			else 			
			if (m_emETSumoRunState == Em_ETSumoRunState::idFinalPosRun){
				if(currentPosition.mY > static_cast<S32>(ET_SUMO_FINAL_POS_Y * 100)){	// TODO
					speaker.playTone(840U, 500U, 50U);
					m_lCt = 0;
					m_emETSumoRunState = Em_ETSumoRunState::idStop;
					return false;
				}
			} 
			else
			// 車庫の停止状態
			if (m_emETSumoRunState == Em_ETSumoRunState::idStop){
				return false;
			}
			return false;
		}
	}
	
	if(m_sect.bSonerUse == true){
		if (m_bDoObstaclesExist == true){
			m_pPositionControl->reset();
			return true;
		}
		return false;
	}
	
	if(m_iXY == Em_SectionChange::XPlus){
		if(currentPosition.mX  > m_cntEndPosition.mX){
			speaker.playTone(840U, 500U, 50U);
			m_lCt = 0;
			return true;
		}
	}
	else if(m_iXY == Em_SectionChange::XMinus){
		if(currentPosition.mX  < m_cntEndPosition.mX){
			speaker.playTone(840U, 500U, 50U);
			m_lCt = 0;
			return true;
		}
	}
	else if(m_iXY == Em_SectionChange::YPlus){
		if(currentPosition.mY  > m_cntEndPosition.mY){
			speaker.playTone(840U, 500U, 50U);
			m_lCt = 0;
			return true;
		}
	}
	else{
		if(currentPosition.mY < m_cntEndPosition.mY){
			speaker.playTone(840U, 500U, 50U);
			m_lCt = 0;
			return true;
		}
	}

	return false;
}
