#include "Behaviors/StateMachine.h"
$nodeclass GrabCan : VisualRoutinesStateNode{

	$nodeclass LookForCan : MapBuilderNode  : doStart{
		//if the can is in the gripper
		mapreq.addObjectColor(ellipseDataType, "red");
	}
	$nodeclass CheckIfCanFound : StateNode : doStart {
		NEW_SHAPEVEC(reds, EllipseData, select_type<EllipseData>(camShS));	
		bool canFound = false;
		SHAPEVEC_ITERATE(reds, EllipseData, red)
			if(red->getArea() > 10000)
			{
				canFound = true;
			}
		END_ITERATE;
			if(canFound==true)
			{
				cout << "Found Can" << endl;
				postStateSuccess();
			}
			else
			{
				cout << "Can not Found" << endl;
				postStateFailure();
			}
	}
        
	$setupmachine{
                grab: DynamicMotionSequenceNode("justApproachCan.mot")
		check: CheckIfCanFound	
		grab =C=> LookForCan=C=>check
		check=S=>DynamicMotionSequenceNode("justRotateCan.mot")
		check=F=>SpeechNode("Could you place the can in the starting position?")=T(10000)=>grab
        }
}
REGISTER_BEHAVIOR(GrabCan);