#include "Behaviors/StateMachine.h"

$nodeclass SceneBuilder2 : VisualRoutinesStateNode {

	$nodeclass FindV : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.addAllObjectColors(lineDataType);
	}

	$nodeclass MakeV : StateNode : doStart {

		NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));
        if (line_shapes.size() > 1) {
			NEW_SKETCH(line0, bool, visops::topHalfPlane(line_shapes[0]));
			NEW_SKETCH(line1, bool, visops::topHalfPlane(line_shapes[1]));
			NEW_SKETCH(wedge, bool, visops::min(line0, line1));

			NEW_SKETCH(max, bool, visops::min(line0, line1));

			camSkS.clear();
			NEW_SKETCH(camFrame, uchar, sketchFromSeg());

			// find all the blue stuff in the segmented camera frame
			NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));

			// label the blue componenets
			NEW_SKETCH(blue_cc, uint, visops::labelcc(blue_stuff));

			// find the area of each blue component
			NEW_SKETCH(blue_area, uint, visops::areacc(blue_cc));	

			// get rid of blue blobs with an area smaller than 10
			NEW_SKETCH(blue_blobs, bool, blue_area > 10);

			// intersect the remaining blue blobs with the wedge
			NEW_SKETCH(blue_inside_wedge, bool, blue_blobs & wedge);
        }
	}
	$nodeclass MyHeadPointerNode : HeadPointerNode : preStart {
		HeadPointerNode::preStart();
		getMC()->setMaxSpeed(0, 0.4);
		getMC()->setMaxSpeed(1, 0.4);
	}

	$setupmachine {
		MyHeadPointerNode[getMC()->lookAtPoint(600, 0, 0);]
		=C=> FindV =C=> MakeV
		=N=> SpeechNode("done")  
	}
}
REGISTER_BEHAVIOR(SceneBuilder2);