#include "Behaviors/StateMachine.h"

$nodeclass Lab6Part5 : VisualRoutinesStateNode {
    $nodeclass FindRedShapes : MapBuilderNode : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
    }
    $nodeclass Report : VisualRoutinesStateNode : doStart {
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
        if (ellipse_shapes.size() == 0) {
            postStateFailure();
        }
	postStateSuccess();
    }
    $setupmachine {
        launch : StateNode =N=> pickup

	pickup : DynamicMotionSequenceNode("pickup.mot")
	putdown : DynamicMotionSequenceNode("putdown.mot")
	reporter : Report


	pickup =C=> FindRedShapes =C=> reporter
	reporter =F=> SpeechNode("Please put the can back") =T(5000)=> pickup
	reporter =S=> putdown
	putdown =C=> pickup
    }
}

REGISTER_BEHAVIOR(Lab6Part5);