#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(cylinderDataType, "green");
    mapreq.addObjectColor(cylinderDataType, "blue");
    mapreq.addObjectColor(blobDataType, "red");
    mapreq.addObjectColor(blobDataType, "green");
    mapreq.addObjectColor(blobDataType, "blue");
    mapreq.pursueShapes = true;
  }


  $nodeclass GetIt : GrasperNode(GrasperRequest::moveTo) : doStart {
    worldShS.deleteShapes<BlobData>();
    NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
    NEW_SHAPE(destination, PointData, new PointData(worldShS, Point(1200, -1000, 0, allocentric)));
    if ( !cyls.empty() ) {
      //graspreq.object = cyls[get_time() % cyls.size()];
      graspreq.object = cyls[cyls.size()-1];
      graspreq.targetLocation = destination;
      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