#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/PIDMC.h"
#include "Motion/MotionPtr.h"
#include "Motion/MotionManager.h"
#include "DualCoding/MapBuilderNode.h"
#include "DualCoding/DualCoding.h"
#include "Motion/MotionSequenceMC.h"

using namespace std;

DATAEVENT_IMPLEMENTATION(Point,void*);

#nodeclass GrabCanWalk : VisualRoutinesStateNode
  /*
    Find Cans
    Steps get can on the right side of the Chiara
    Make Arm Movements 
    Walk to home marker
  */
static float goalx,goaly;

#shortnodeclass FindCans : MapBuilderNode($, MapBuilderRequest::localMap) : DoStart
      NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
      mapreq.searchArea = gazePoly;
      mapreq.motionSettleTime = 2000;
      mapreq.maxDist = 1e6;
      mapreq.clearShapes = false; 
      mapreq.addObjectColor(blobDataType,"blue");
      mapreq.addBlobOrientation("blue", BlobData::pillar);
      mapreq.rawY = true;

#shortnodeclass ClosestLargeBlob : VisualRoutinesStateNode : DoStart
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      if(blobs.empty()){
	cout<<" the mapbuiler is deleting the blue blobs"<<endl;
	postStateFailure();
	return;
      }      
      NEW_SHAPE(blob, BlobData, blobs[0]);
      const Point pt = blob->getCentroid();
      const float x =pt.coordX();
      float y = pt.coordY();
// cout<<"The bounding box is \n ymax "<< blob->getBoundingBox().ymax<<"\n Ymin "<< blob->getBoundingBox().ymin<<"\n Xmin "<< blob->getBoundingBox().xmin<<"\n Xmax "<< blob->getBoundingBox().xmax;
      cout<<"I want the can at point ("<<goalx<<", "<<goaly<<")"<<endl;
      if((y<0)&&(y!=-25)){
	y = -1*(goaly + abs(y));
      }
      if((y>0)&&(y!=-25)){
	y = -1*(goaly - y);
      }
      cout<<"The can is at location "<<x<<", "<<y<<"\n\n"<<endl;
      Point destination = Point(x-goalx, y);
      cout<<"I want to get to point ("<<destination.coordX()<<", "<<destination.coordY()<<endl;
      if((abs(destination.coordX()) <=15)&&(abs(destination.coordY())<=15)){
	  cout<<"I am in line with the can now."<<endl;  
          postStateCompletion();
      }
      postStateSignal<Point>(destination);

#nodeclass RightSideCan : StateNode: pidmc_id(MotionManager::invalid_MC_ID) 
#nodemethod DoStartEvent(const EventBase &event)
 ptr = dynamic_cast<const DataEvent<Point>&>(event).getData();
cout << "Casting the data event. " << endl;
const float y = ptr.coordY();
SharedObject<XWalkMC> walker_mc; 
motman->addPrunableMotion(walker_mc, MotionManager::kHighPriority);
walker_mc->setTargetDisplacement(0, y, 0);
cout<<"I am walking to the side " <<y<<" mm"<<endl;
erouter->addListener(this, EventBase::locomotionEGID);
#nodemethod processEvent(const EventBase &event)
     const LocomotionEvent& le = dynamic_cast<const LocomotionEvent&>(event);
     if ( le.x == 0 && le.y == 0 && le.a == 0 )  // walking has stopped
       postStateSignal<Point>(ptr);  
#endnodemethod
private:
MotionManager::MC_ID pidmc_id;
Point ptr;
#endnodeclass

#nodeclass WalkTowardsCan : StateNode: pidmc_id(MotionManager::invalid_MC_ID)
#nodemethod  DoStartEvent(const EventBase &event)
      cout<<"I made it here"<<endl; 
      const DataEvent<Point>& ptr = dynamic_cast<const DataEvent<Point>&>(event);
      const float x = ptr.getData().coordX();
      cout<<"I am walking forward "<<x<<" mm"<<endl;
      SharedObject<XWalkMC> walker_mc;      
      motman->addPrunableMotion(walker_mc, MotionManager::kHighPriority);
      walker_mc->setTargetDisplacement(x, 0, 0);
      erouter->addListener(this, EventBase::locomotionEGID);
#nodemethod processEvent(const EventBase &event)
     const LocomotionEvent& le = dynamic_cast<const LocomotionEvent&>(event);
     if ( le.x == 0 && le.y == 0 && le.a == 0 )  // walking has stopped
       postStateCompletion();  
      #endnodemethod
private:
MotionManager::MC_ID pidmc_id;
#endnodeclass
//Turn the body to make sure the arm is far right of it
//Walk towards the object
//Finish it all up

#nodemethod setup
      #statemachine
startnode: FindCans =MAP=> check: ClosestLargeBlob =F=> walk: XWalkNode($, 20, 200, 0, 0, 0, 0) =C=> startnode
	     check=S<Point>=>turntotarget:RightSideCan =S<Point>=>walktotarget:WalkTowardsCan =C=>startnode
             check =C=> rest: ArmNode[getMC()->setJoints(0, 0, 0, 0, 0, 0);] =C=>
	     wrist: ArmNode[getMC()->setJoints(0.135634, -0.191935, -1.6097, 0, 0, 0);] =C=>
             elbow: ArmNode[getMC()->setJoints(0.0537419, -1.54828, -1.6097, 0, 0, 0);] =C=>
	     grasp: ArmNode[getMC()->setJoints(0.0537419, -1.54828, -1.6097, 0, 0, 0);]
      #endstatemachine
#endnodemethod 
#endnodeclass

#endif