#include "Behaviors/StateMachine.h"
#ifdef TGT_CHIARA

$nodeclass Lab10 : VisualRoutinesStateNode {

  $nodeclass FindObstacles : MapBuilderNode($,MapBuilderRequest::localMap) : doStart {
    mapreq.addObjectColor(ellipseDataType, "pink");
    mapreq.addObjectColor(ellipseDataType, "blue");
    mapreq.addObjectColor(ellipseDataType, "green");
  }

  $nodeclass PlanPath : GrasperNode($,GrasperRequest::reach) : doStart {
    graspreq.envObstacles = localShS.allShapes();
    // move from one side to the other: target uses negative y coord of current position
    const KinematicJoint* gripper = kine->getKinematicJoint(ChiaraInfo::GripperFrameOffset);
    fmat::Column<3> where = gripper->getWorldPosition();
    cout << "Gripper is at " << where << endl;
    NEW_SHAPE(target, PointData, new PointData(localShS, DualCoding::Point(where[0], -where[1], where[2], egocentric)));
    graspreq.object = target;
    graspreq.restType = GrasperRequest::stationary;
    graspreq.numberOfStatesForRRT = 5000;
    graspreq.verbosity = 1;
  }

  $setupmachine {
      lookdown: HeadPointerNode[getMC()->setJoints(0, -0.167, -1.2)] =C=> loop
  
      loop: SpeechNode($,"ready") =B(GreenButOffset)=> FindObstacles =MAP=> plan

      plan: PlanPath
      plan =GRASP(noError)=> loop
      plan =GRASP(someError)=> SpeechNode($,"No path found.") =T(2500)=> loop
  }

}

REGISTER_BEHAVIOR(Lab10);

#endif