#include "SimpleSoccer.h"
#include "Events/EventRouter.h"
#include "Events/VisionObjectEvent.h"
#include "Shared/WorldState.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/WalkMC.h"
#include "Shared/WMclass.h"
#include "Shared/ProjectInterface.h"

//! Converts degrees to radians
inline double DtoR(double deg) { return (deg/180.0*M_PI); }

double Abs(double Nbr)
{
	if( Nbr >= 0 )
		return Nbr;
	else
		return -Nbr;
}

double MySquareRoot(double Nbr)
{
	double Number = Nbr / 2;
	const double Tolerance = 1.0e-7;

	do Number = (Number + Nbr / Number) / 2;
	while( Abs(Number * Number - Nbr) > Tolerance);

	return Number;
}


void SimpleSoccer::DoStart() {

	BehaviorBase::DoStart();

	//INITIALIZE VARS
	//PlayerType... default to Attacker
	player_type = ATTACKER_ID;
	desired_area = ATTACKER_DESIRED_AREA;
	//previous Area... used to track how close the ball is... default to far away
	previous_area = 999;
	prior_di = 999;
	//play_mode.. default to paused.
	play_mode = PLAY_PAUSED;
	prior_mode = 999;
	//Timer off
	timer_mode = TIMER_OFF;
	//timer Count = 0
	timer_counter = 0;
	//motion_waiting = false;

	//load sounds so that we can play them
	sndman->LoadFile("SoccerPr.wav");
	sndman->LoadFile("GoalieOn.wav");
	sndman->LoadFile("ForwardS.wav");
	sndman->LoadFile("ImGonnaS.wav");
	sndman->LoadFile("whimper.wav");

	//SharedObject<MediumMotionSequenceMC>  p("/ms/data/motion/pounce.pos");

	//prepare motion manager to move head and walk
	headpointer_id = motman->addPersistentMotion(SharedObject<HeadPointerMC>());
	walker_id = motman->addPersistentMotion(SharedObject<WalkMC>());
	//listen for events from vision (ID ball) and from buttons (switch player type/pause)
	erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visOrangeBallSID);
	erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visYellowGoalSID);
	erouter->addListener(this,EventBase::buttonEGID);
	//1000 every second
	erouter->addTimer(this,0,1000,true);

}

void SimpleSoccer::DoStop() {
	erouter->removeListener(this);
	erouter->removeTimer(this);
	motman->removeMotion(headpointer_id);
	motman->removeMotion(walker_id);
	sndman->ReleaseFile("SoccerPr.wav");
	sndman->ReleaseFile("GoalieOn.wav");
	sndman->ReleaseFile("ImGonnaS.wav");
	sndman->ReleaseFile("ForwardS.wav");
	sndman->ReleaseFile("whimper.wav");
	BehaviorBase::DoStop();
}

//do something when an event is raised
//this could be cleaned up event-wise (only use a timer when out of view)
void SimpleSoccer::processEvent(const EventBase& event) {
	
	//If we are in debug mode and not paused, write debug info to the console... This is great, but 
	//it still spams a lot. 
	if (play_mode != PLAY_PAUSED && bDEBUG){
		cout << " ProjectInterface::visOrangeBallSID:" <<  ProjectInterface::visOrangeBallSID << endl;
		cout << " ProjectInterface::visYellowGoalSID:" <<  ProjectInterface::visYellowGoalSID << endl;
		cout << " EventBase::visObjEGID:" <<  EventBase::visObjEGID << endl;
		cout << " EventBase::statusETID:" <<  EventBase::statusETID << endl;
		cout << " EventBase::timerEGID:" <<  EventBase::timerEGID << endl;
		cout << " EventBase::buttonEGID:" <<  EventBase::buttonEGID << endl;

		cout << " event.getSourceID(): " <<  event.getSourceID() << endl;
		cout << " event.getGeneratorID(): " <<  event.getGeneratorID() << endl;
		cout << " event.getTypeID(): " <<  event.getTypeID() << endl;
		cout << " play_mode: " << play_mode << endl;
		cout << " prior_mode: " << prior_mode << endl;
	}
	
	//timer.  The timer always runs, but I only count it if I need to (TIMER_ON).
	if(event.getGeneratorID()==EventBase::timerEGID) {
		if (timer_mode == TIMER_ON){
			//cout<<"timer event"<<endl;
			timer_counter++;
		}
	}
	else
	{

		if (event==HBtn && event.getGeneratorID()==EventBase::buttonEGID){//if it is the head button, switch player type and pause
				cout<<"Head Button Pressed"<<endl;
				prior_mode = play_mode;
				play_mode = PLAY_PAUSED;
				previous_area = 999;
				timer_mode = TIMER_OFF;
				timer_counter = 0;
				cout << "Paused" << endl;
				
				//switch player and desired area (to trigger kick or defensive block)
				switch (player_type){
					case GOALIE_ID:
						player_type = ATTACKER_ID;
						desired_area = ATTACKER_DESIRED_AREA;
						sndman->PlayFile("ForwardS.wav");
						break;
					case ATTACKER_ID:
						player_type = GOALIE_ID;
						desired_area = GOALIE_DESIRED_AREA;
						sndman->PlayFile("GoalieOn.wav");
						break;
				}
		}


				//on press of chin button pause or unpause
		if(event.getSourceID()==ChinButOffset && event.getGeneratorID()==EventBase::buttonEGID){
			if(event.getTypeID()==EventBase::activateETID) {
				cout << "Chin Pressed" << endl;
				switch (play_mode){
					case PLAY_PAUSED:
						prior_mode = play_mode;
						play_mode = PLAY_PLAY;
						cout << "PLAY" << endl;
						break;
					case PLAY_PLAY:
						prior_mode = play_mode;
						play_mode = PLAY_PAUSED;
						cout << "Paused" << endl;
						break;
				}
			}

		}


		
		//decide action based on mode and event
		
		//If just now set to paused, stop walking...
		if (play_mode == PLAY_PAUSED && prior_mode != PLAY_PAUSED){
				//do nothing
				//erouter->addListener(this,EventBase::motmanEGID,walker_id,EventBase::deactivateETID);
				//motion_waiting = true;
				WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
				walker->setTargetVelocity(0,0,0);		
				motman->checkinMotion(walker_id);
				HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
				headpointer->setJoints(0,0,0);
				motman->checkinMotion(headpointer_id);
				cout<<"Stop Walk, Head Up"<<endl;
				timer_mode = TIMER_OFF;
				prior_mode = PLAY_PAUSED;
		}



		if (play_mode == PLAY_PLAY){	
				//determine starting mode based on player
				switch (player_type){
					case GOALIE_ID:
						prior_mode = play_mode;
						play_mode = PLAY_PAUSED;//tbd
						break;
					case ATTACKER_ID:
						prior_mode = play_mode;
						play_mode = PLAY_LOOK_FOR_BALL;
						PrevAreaCounter =0;
						cout<<"PLAY_LOOK_FOR_BALL"<<endl;
						break;
				}
		}



		if (play_mode == PLAY_LOOK_FOR_BALL){
			//if see ball, set go to ball.
			if (event.getSourceID()==ProjectInterface::visOrangeBallSID && event.getGeneratorID()==EventBase::visObjEGID 
			&& event.getTypeID()==EventBase::statusETID){
				prior_mode = play_mode;
				play_mode = PLAY_GO_TO_BALL;
			}
			else{
			//else 360 until see ball
				if (timer_mode == TIMER_OFF){
					timer_mode = TIMER_ON;
					timer_counter = 0;
				}
				if (timer_counter <=15){
					cout<<"Look for ball"<<endl;
					WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
					//walker->setTargetVelocity(12,100,39);//rotate pretty large circle with head in center		
					walker->setTargetVelocity(0,-29.9009, 0.684306);//rotate pretty large circle with head in center		
					motman->checkinMotion(walker_id);
				}else {
						prior_mode = play_mode;
						play_mode = PLAY_PAUSED;
						cout << "CAN NOT FIND BALL"<<endl;
					}
				}
			}
		




		if (play_mode == PLAY_GO_TO_BALL || play_mode == PLAY_NEAR_BALL){
			//we see the ball, so lets go to it
			//stop when near ball (see distance) then set new state/mode
	if(event.getSourceID()==ProjectInterface::visOrangeBallSID && event.getGeneratorID()==EventBase::visObjEGID 
			&& event.getTypeID()==EventBase::statusETID ) {
		//some vars to hold chasing ball data
		WMreg(chase_ball_behavior);
		WMvari_(float, horiz, 0, chase_ball_behavior);
		WMvari_(float, vert, 0, chase_ball_behavior);
		WMvari_(float, area, 0, chase_ball_behavior);

		//get info on ball
		horiz=static_cast<const VisionObjectEvent*>(&event)->getCenterX();
		vert=static_cast<const VisionObjectEvent*>(&event)->getCenterY();
		area=static_cast<const VisionObjectEvent*>(&event)->getArea ();

		//set head to follow ball
		double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7;
		double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6;
		if(tilt<DtoR(-30))
			tilt=DtoR(-30);
		if(tilt>DtoR(40))
			tilt=DtoR(40);
		if(pan>DtoR(80))
			pan=DtoR(80);
		if(pan<DtoR(-80))
			pan=DtoR(-80);
		HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id);
		//limit so that dog does not look down on top of the ball
		if (tilt <= -.44) tilt = -.44;
		headpointer->setJoints(tilt,pan,0);
		motman->checkinMotion(headpointer_id);

		if (play_mode != PLAY_PAUSED && bDEBUG){
			cout << "Area: " <<  area << "    Previous Area: " <<  previous_area << "    Desired Area: " << desired_area << endl;
			cout << "PrevAreaCounter: " << PrevAreaCounter << "   AreaCounter: " << AreaCounter << endl;
		}

		//if just starting or still farther from ball
		if ((area > desired_area) || (previous_area == 999) || (area >= previous_area)){
			//walk to ball
			//cout << "Previous Area" <<  previous_area << endl;
			if (area > desired_area && PrevAreaCounter == 3){previous_area = area; PrevAreaCounter = 0;
			}else if (PrevAreaCounter > 3){PrevAreaCounter = 0;}
				else{PrevAreaCounter++;}
			AreaCounter = 0;
			
			WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
			if(state->outputs[HeadOffset+PanOffset]<-.05 || state->outputs[HeadOffset+PanOffset]>.05)
				walker->setTargetVelocity(100,0,state->outputs[HeadOffset+PanOffset]);
			else
				walker->setTargetVelocity(160,0,0);
			
			motman->checkinMotion(walker_id);
		}
		//else if we are close to the ball, change mode
		else if (area <= desired_area && AreaCounter >= 3){
			WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
			walker->setTargetVelocity(0,0,0);
			motman->checkinMotion(walker_id);
			cout << "Near Ball" << endl;
			//simple_state= NEAR_BALL;
			play_mode=PLAY_NEAR_BALL_FIND_GOAL;
		}else if (AreaCounter < 3){AreaCounter++;}


	}



	}//end play_go_to_ball







		if (play_mode == PLAY_NEAR_BALL_FIND_GOAL){
			//rotate around ball until goal is seen
			if (event.getSourceID()==ProjectInterface::visYellowGoalSID && event.getGeneratorID()==EventBase::visObjEGID 
					&& event.getTypeID()==EventBase::statusETID)
			{
				//test for goal recognition
				//cout << "Area: " <<  area << "    Previous Area: " <<  previous_area << "    Desired Area: " << desired_area << endl;

				cout <<"GOAL IN VIEW" << endl;
				prior_mode = play_mode;
				play_mode = PLAY_BALL_GOAL_INLINE;
			}
			else{
				//else 360 until see goal
				if (timer_mode == TIMER_OFF){
					timer_mode = TIMER_ON;
					timer_counter = 0;
				}
				if (timer_counter ==1){
					cout<<"Look for Goal"<<endl;
					WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id);
					//walker->setTargetVelocity(12,100,39);//rotate pretty large circle with head in center		
					//walker->setTargetVelocity(15.388, 81.9571, -0.171076);//rotate pretty large circle with head in center		
					walker->setTargetVelocity(-100,0,0);//rotate pretty large circle with head in center		
					motman->checkinMotion(walker_id);
					//walker = (WalkMC*)motman->checkoutMotion(walker_id);
					//walker->setTargetVelocity(15, 81, -0.17);//rotate pretty large circle with head in center		
					//motman->checkinMotion(walker_id);
				}else {
					if (timer_counter ==20){
						prior_mode = play_mode;
						play_mode = PLAY_PAUSED;
						cout << "CAN NOT FIND Goal"<<endl;
						timer_mode = TIMER_OFF;
						timer_counter = 0;
						}
					}
				}
			}
		

		if (play_mode == PLAY_BALL_GOAL_INLINE){
			//Get to kick range and kick ball
			//pause
			cout<<"KICK YOU FOOL"<<endl;
			prior_mode = play_mode;
			play_mode = PLAY_PAUSED;
		}











	}//end else (thus it was not a timer that fired)		
}