#include "Behaviors/StateMachine.h"

$nodeclass FlipCan : VisualRoutinesStateNode {

	$nodeclass FindStuff : MapBuilderNode : doStart {
		mapreq.addObjectColor(blobDataType, "blue");
	}

	$nodeclass CheckCan : doStart {
		NEW_SHAPEVEC(blobs_shapes, BlobData, select_type<BlobData>(camShS));
		int found = 0;
		SHAPEVEC_ITERATE(blobs_shapes, BlobData, myblob)
		{
			if (myblob->getArea() > 5000)
			{
				found = 1;
			}
		} END_ITERATE;
		if (found == 1)
		{
			postStateSuccess();
		}
		else
		{
			postStateFailure();
		}
	} 

	$setupmachine {
		launch: StateNode =N=> flip1 =C=> FindStuff =C=> check
		
		flip1: DynamicMotionSequenceNode("flipcan1.mot")
		flip2: DynamicMotionSequenceNode("flipcan2.mot")
		check: CheckCan
		check =S=> flip2 =C=> launch
		check =F=> SpeechNode("Can grab failed") =T(5000)=> flip2 =C=> launch
	}

}

REGISTER_BEHAVIOR(FlipCan);