#include "Behaviors/StateMachine.h"

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

	$nodeclass LookCan : VisualRoutinesStateNode : doStart {
		//Get all shapes and printout result
		NEW_SHAPEVEC(blob_shapes, BlobData, select_type<BlobData>(camShS));

		//Iterate through and ignore small areas
		int blobFound = false;
		SHAPEVEC_ITERATE(blob_shapes, BlobData, myBlob)
			if(myBlob->getArea() > 100)  {
				postStateSuccess();
				blobFound = true;
			}
			else {
				camShS.deleteShape(myBlob);
			}
 		END_ITERATE;

		//Post failure if blob not found
		if (!blobFound) {
			postStateFailure();
		}
	}
	$setupmachine {
		launch: DynamicMotionSequenceNode("examinecan.mot") =C=> DetectCan =C=> checkCan: LookCan
		checkCan =S=> SpeechNode("I have picked up your can") =C=> DynamicMotionSequenceNode("examinecan_complete.mot")
		checkCan =F=> SpeechNode("Please put the can where I can pick it up") =C=> DynamicMotionSequenceNode("examinecan_fail1.mot") =T(5000)=> DynamicMotionSequenceNode("examinecan_fail2.mot") =C=> DetectCan =C=> checkCan
	}
}
REGISTER_BEHAVIOR(ExamineCan);