#include "Behaviors/StateMachine.h"

$nodeclass GraspRequest : StateNode {

  $provide Shape<CylinderData> to_grasp;

  $nodeclass TakeAPictureLocal : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    $reference GraspRequest::to_grasp;

    mapreq.addObjectColor(cylinderDataType,"red");

    NEW_SHAPEVEC(cylinders, CylinderData, select_type<CylinderData>(camShS));
    to_grasp = cylinders[0];
  }

  $nodeclass TurnTo : PilotNode(PilotTypes::walk) : doStart {
    $reference GraspRequest::to_grasp;
    double theta1 = atan((to_grasp->getCentroid().coordY()-to_grasp->getRadius())/(to_grasp->getCentroid().coordX()-to_grasp->getRadius()));
    double z = sqrt(pow((to_grasp->getCentroid().coordX()-to_grasp->getRadius()),2) + pow((to_grasp->getCentroid().coordY()-to_grasp->getRadius()),2));
    double theta2 = atan(120.0/z);
    pilotreq.da = theta1 + theta2;
  }

  $nodeclass WalkTo : PilotNode(PilotTypes::walk) : doStart {
    $reference GraspRequest::to_grasp;
    double z = sqrt(pow((to_grasp->getCentroid().coordX()-to_grasp->getRadius()),2) + pow((to_grasp->getCentroid().coordY()-to_grasp->getRadius()),2));
    pilotreq.dx = z;
  }

  $nodeclass Grasp : StateNode : doStart {
    $reference GraspRequest::to_grasp;
    
  }

  $setupmachine {
    TakeAPictureLocal =C=> PostureNode("pregrab.pos") =C=> TurnTo =C=>
      WalkTo =C=> Grasp =C=> SpeechNode("Got it!")
  }
}
REGISTER_BEHAVIOR(GraspRequest);