#ifndef INCLUDED_GrabCanWalk_h_
#define INCLUDED_GrabCanWalk_h_

#include "Events/EventRouter.h"
#include "Events/VisionObjectEvent.h"
#include "Shared/WorldState.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/ProjectInterface.h"
#include "Shared/ChiaraInfo.h"
#include "Behaviors/StateMachine.h"
#include "Motion/MotionPtr.h"
#include "Motion/MotionManager.h"
#include "DualCoding/MapBuilderNode.h"
#include "Motion/MotionSequenceMC.h"

using namespace std;

class GrabCanWalk : public StateNode {
public:
  GrabCanWalk() :StateNode("GrabCanWalk"){}
  /*
    Find Cans
    Steps get can on the right side of the Chiara
    Make Arm Movements 
    Walk to home marker
  */
  class FindCans : public MapBuilderNode {
  public:
    FindCans(string name) : MapBuilderNode("FindCans",MapBuilderRequest::localMap){}
    virtual void DoStart(){
      NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
      mapreq.searchArea = gazePoly;
      mapreq.motionSettleTime = 2000;
      mapreq.maxDist = 2000;
      mapreq.clearShapes = false; 
      mapreq.addObjectColor(blobDataType,"pink");
      mapreq.rawY = true;
    }
  };
	
  class ClosestLargeBlob : public VisualRoutinesStateNode {
  public:
    ClosestLargeBlob(std::string name) : VisualRoutinesStateNode("ClosestLargeBlob"){}
    virtual void DoStart() {
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      if(blobs.size()==0){
	postStateFailure();
	return;
      }
      NEW_SHAPE(blob, BlobData, blobs[0]);
      const Point pt = blob->getCentroid();
      const float x = pt.coordX();
      const float y = pt.coordY();
      cout<<"The blob is at location"<<x<<", "<<y<<endl;
    }
  };   
  //  class RightSidetheCan() : XWalkNode(){
 
  //};

  virtual void setup(){
      #statemachine
  startnode: FindCans =MAP=> check: ClosestLargeBlob =F=> walk: XWalkNode(20, 200, 0, 0, 0, 0) =C=> startnode
      #endstatemachine
      }
};
#endif