#include "Shared/RobotInfo.h"
#ifdef TGT_HAS_GRIPPER

#include "Behaviors/StateMachine.h"

$nodeclass GraspTest1 : VisualRoutinesStateNode {

  $nodeclass FindCyl : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    mapreq.addObjectColor(cylinderDataType, "red");
    mapreq.addObjectColor(blobDataType, "red");
    mapreq.pursueShapes = true;
  }


  $nodeclass GetIt : GrasperNode(GrasperRequest::grasp) : doStart {
    worldShS.deleteShapes<BlobData>();
    NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
    if ( !cyls.empty() ) {
      //graspreq.object = cyls[get_time() % cyls.size()];
      graspreq.object = cyls[cyls.size()-1];
	  graspreq.gripperAngleRangesX.push_back(pair<float, float>(M_PI/2, M_PI/2));
	  graspreq.gripperAngleRangesY.push_back(pair<float, float>(M_PI/2, M_PI/2));
	  graspreq.gripperAngleRangesZ.push_back(pair<float, float>(M_PI/2, M_PI/2));
      cout << "Going to grasp " << graspreq.object << endl;
    } else {
      cout << "No valid cylinders in view!" << endl;
      cancelThisRequest();
    }
  }

  $setupmachine{
    FindCyl =C=> GetIt =C=> SpeechNode("done")
  }

}

REGISTER_BEHAVIOR(GraspTest1);

#endif