#ifndef _TurnToDealer_
#define _TurnToDealer_

#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 "Motion/WalkMC.h"

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

class TurnToDealer : public VisualRoutinesStateNode {
 public:
  TurnToDealer() : VisualRoutinesStateNode("TurnToDealer"),
	  walk_mc(), walk_id(MotionManager::invalid_MC_ID){}

  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "Turn to dealer starting up." << endl;
	 SharedObject<MediumMotionSequenceMC> mseq_mc;
	 mseq_mc->LoadFile("neutHead.mot");
	 motman->addPrunableMotion(mseq_mc);
	 erouter->addTimer(this, 1234, 1000, false);
	 //	 erouter->addListener(this, EventBase::buttonEGID, RobotInfo::HeadButOffset);
  }

  void processEvent(const EventBase& event) {
          //if(event.getGeneratorID()==EventBase::buttonEGID){
          if(event.getSourceID() == 1234){
		  walk_id = motman->addPersistentMotion(walk_mc);
		  erouter->addTimer(this, 1000, 11000, false);
		  MMAccessor<WalkMC>(walk_id)->setTargetDisplacement(0, 0, 2.5, 29);
	  }
	  else if(event.getSourceID() == 1000){
		  cout<<"third sequence\n";
		  MMAccessor<WalkMC>(walk_id)->setTargetVelocity(0, 40, 0, 5);
		  erouter->addTimer(this, 1001, 2000, false);
		  
	  }
	  else if(event.getSourceID() == 1001){
	          erouter->addTimer(this,1002, 250, false);
	  }
	  else if(event.getSourceID() == 1002){
	    	camShS.clear();
		int const green_index = ProjectInterface::getColorIndex("green");
		NEW_SHAPEVEC(gblobs,BlobData,getBlobsFromRegionGenerator(green_index,200,BlobData::groundplane,1));
		if ( gblobs.size() == 0 )
		  {
		    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";
		
		Point gpoint = gblobs[0]->getCentroid();
		Point *centre = new Point(camSkS.getWidth()/2,camSkS.getHeight()/2);
		cout << "centre distance: " << centre->xyDistanceFrom(gpoint) << "\n";
		erouter->addTimer(this,1003, 300, false);


	  }
	  else if(event.getSourceID() == 1003){
		  motman->removeMotion(walk_id);
		  postCompletionEvent();
	  }

  }
  

  void DoStop() {
    cout << "turn to dealer stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }


 private:
  SharedObject<WalkMC> walk_mc;
  MotionManager::MC_ID walk_id;

  TurnToDealer(const TurnToDealer&);
  TurnToDealer& operator=(const TurnToDealer&);
};

#endif