#ifndef _AcquireCard_
#define _AcquireCard_

#include "Events/LocomotionEvent.h"
#include "Behaviors/Nodes/MotionSequenceNode.h"
#include "Behaviors/Nodes/WalkNode.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/WalkMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/WorldState.h"
#include "CardRecognition.h"
#include "Motion/WalkMC.h"
#include "TurnToDealer.h"

#include "DualCoding/DualCoding.h"
using namespace DualCoding;

class AcquireCard : public VisualRoutinesStateNode {
 public:
	int cardValue;
	AcquireCard() : VisualRoutinesStateNode("AcquireCard"),
		cardValue(0), walk_mc(), walk_id(MotionManager::invalid_MC_ID),
		head_mc(), head_id(MotionManager::invalid_MC_ID), approachAngle(0),
		headPanDuringGrab(0), cr(NULL){}
		
		void DoStart() {
			VisualRoutinesStateNode::DoStart();
			cout << "Acquire Card starting up." << endl;
			
			walk_id = motman->addPersistentMotion(walk_mc);
			TurnToDealer *turn = new TurnToDealer();
			turn->DoStart();   
			erouter->addTimer(this,3000,15000,false);
			
		}
		
		void processEvent(const EventBase& event) {
			
			if(event.getSourceID() == 3000) {
				head_mc->setJoints(0,state->outputs[HeadOffset+PanOffset],0);
				head_id = motman->addPersistentMotion(head_mc);
				erouter->addTimer(this,2000,250,true);
			}
			
			else if(event.getSourceID() == 2000) {
				camShS.clear();
				int const green_index = ProjectInterface::getColorIndex("green");
				NEW_SHAPEVEC(gblobs,BlobData,getBlobsFromRegionGenerator(green_index,200,BlobData::groundplane,1));
				if ( gblobs.size() == 0 ) {
					double pan=state->outputs[HeadOffset+PanOffset];
					double tilt=state->outputs[HeadOffset+TiltOffset];
					double nod=state->outputs[HeadOffset+NodOffset];
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt, pan+.2, nod);
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);
					cout << "needs to see the green pad\n";
					return;
				}
				
				cout << "Green Blob size: " << gblobs[0]->getArea()  << "\n";
				
				if ( gblobs[0]->getArea() > 6000) {
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(1,0,0,0);
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);
					erouter->removeTimer(this);
					Point gpoint = gblobs[0]->getCentroid();
					double const head_nod_angle = state->outputs[HeadOffset+NodOffset];
					double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
					double const head_nod_move = (camSkS.getHeight()/2 - gpoint.coordY()) / camSkS.getHeight() * CameraFOV;
					double const head_pan_move = (camSkS.getWidth()/2 - gpoint.coordX()) / camSkS.getWidth() * CameraFOV;
					
					double const tilt = state->outputs[HeadOffset+TiltOffset];
					double new_pan = head_pan_angle + head_pan_move;
					double new_nod = head_nod_angle + head_nod_move;
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt,new_pan,new_nod-0.35);
					erouter->addTimer(this, 1234, 1000, false);
				}
				else{
					Point gpoint = gblobs[0]->getCentroid();
					double const head_nod_angle = state->outputs[HeadOffset+NodOffset];
					double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
					double const head_nod_move = (camSkS.getHeight()/2 - gpoint.coordY()) / camSkS.getHeight() * CameraFOV;
					double const head_pan_move = (camSkS.getWidth()/2 - gpoint.coordX()) / camSkS.getWidth() * CameraFOV;		 
					double const tilt = state->outputs[HeadOffset+TiltOffset];
					double new_pan = head_pan_angle + head_pan_move;
					double new_nod = head_nod_angle + head_nod_move;
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt,new_pan,new_nod);
					approachAngle = new_pan;
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(40,0,new_pan);
				}	  		 
			}
			
			//Recognize the card
			if(event.getSourceID() == 1234){
				cr = new CardRecognition();
				cr->DoStart();
				erouter->addTimer(this, 1101, 9000, false);
			}
			
			
			//open the mouth
			else if(event.getSourceID() == 1101){
				cardValue=cr->cardValue;
				motman->removeMotion(head_id);
				SharedObject<MediumMotionSequenceMC> mseq_mc;
				mseq_mc->LoadFile("acqCard.mot");
				motman->addPrunableMotion(mseq_mc);
				head_id = motman->addPersistentMotion(head_mc);
				erouter->addTimer(this, 1100, 500, false);
			}
			
			

			//approach to the card
			else if(event.getSourceID() == 1100) {
				camShS.clear();
				int const yellow_index = ProjectInterface::getColorIndex("yellow");
				NEW_SHAPEVEC(yblobs,BlobData,getBlobsFromRegionGenerator(yellow_index,200,BlobData::groundplane,1));
				if ( yblobs.size() == 0 ){
					
		         MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);		 
					double const nod = state->outputs[HeadOffset+NodOffset];
					double const pan = state->outputs[HeadOffset+PanOffset];
					double const tilt = state->outputs[HeadOffset+TiltOffset];
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt,pan,nod+0.1);
					erouter->addTimer(this, 1100, 500, false);
					cout << "needs to see the yellow strip\n";
					return;
				}
				
				cout << "Yellow Blob size: " << yblobs[0]->getArea()  << "\n";
				
				if ( yblobs[0]->getArea() > 2000) {
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(1,0,0,0);
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0,0,0,0);
					erouter->addTimer(this, 1001, 1000, false);
				}
				else{
					Point ypoint = yblobs[0]->getCentroid();
					double const head_nod_angle = state->outputs[HeadOffset+NodOffset];
					double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
					double const head_nod_move = (camSkS.getHeight()/2 - ypoint.coordY()) / camSkS.getHeight() * CameraFOV;
					double const head_pan_move = (camSkS.getWidth()/2 - ypoint.coordX()) / camSkS.getWidth() * CameraFOV;		 
					double const tilt = state->outputs[HeadOffset+TiltOffset];
					double new_pan = head_pan_angle + head_pan_move;
					double new_nod = head_nod_angle + head_nod_move;
					MMAccessor<HeadPointerMC>(head_id)->setJoints(tilt,new_pan,new_nod);
					approachAngle = new_pan;
					MMAccessor<WalkMC>(walk_id)->setTargetVelocity(20,0,new_pan);
					erouter->addTimer(this, 1100, 280, false);
				}	  
				
			}
			

			//bite
			else if(event.getSourceID() == 1001) { 
				SharedObject<MediumMotionSequenceMC> mseq_mc1;
				mseq_mc1->LoadFile("bite.mot");
				motman->addPrunableMotion(mseq_mc1);
				erouter->addTimer(this, 1002, 1000, false);
				headPanDuringGrab=state->outputs[HeadOffset + PanOffset]; //this determines how much he will need to turn around
			}
			//back off
			else if(event.getSourceID()== 1002){
				MMAccessor<WalkMC>(walk_id)->setTargetVelocity(-40, 0, 0, 20);
				erouter->addTimer(this, 1003, 7000, false);
			}
			
			else if(event.getSourceID() == 1003){
				MMAccessor<WalkMC>(walk_id)->setTargetDisplacement(0, 0, 2.7, 30);
				erouter->addTimer(this, 1005, 16000, false);
			}
			
			else if(event.getSourceID() == 1005)
				{
					motman->removeMotion(head_id);
					cout << "Acquire Card is done." << endl;
					postCompletionEvent();
				}
			
		}
		
		
		void DoStop() {
			motman->removeMotion(head_id);
			motman->removeMotion(walk_id);
			cout << "Acquire Card stopped." << endl;
			VisualRoutinesStateNode::DoStop();
		}
		
		
 private:
		SharedObject<WalkMC> walk_mc;
		MotionManager::MC_ID walk_id;
		SharedObject<HeadPointerMC> head_mc;
		MotionManager::MC_ID head_id;
		double approachAngle;
		double headPanDuringGrab;
		CardRecognition *cr;
		AcquireCard(const AcquireCard&);
		AcquireCard& operator=(const AcquireCard&);
};

#endif