#include "Behaviors/StateMachine.h"

$nodeclass Lab5part3 : VisualRoutinesStateNode {
	$nodeclass AddShapesToMap : MapBuilderNode : doStart {
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.addOccluderColor(lineDataType, "red");
	}


	$nodeclass AnalyzeShapes : VisualRoutinesStateNode : doStart {
		NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
		
		NEW_SKETCH(camFrame, uchar, sketchFromSeg());

		NEW_SKETCH(tophalf, bool, visops::topHalfPlane(lines[0]));
		NEW_SKETCH(tophalf2, bool, visops::topHalfPlane(lines[1]));
		NEW_SKETCH(topcombined, bool, tophalf&tophalf2);

		NEW_SKETCH(redstuff, bool, visops::colormask(camFrame, "red") & topcombined);

		NEW_SKETCH(redblobs, uint, visops::labelcc(redstuff));
		NEW_SKETCH(blobsizes, uint, visops::areacc(redblobs));

		NEW_SKETCH(final, bool, blobsizes >= 100); 
	}

	$setupmachine {
		launch : AddShapesToMap =C=> getShapes
		getShapes: AnalyzeShapes =C=> SpeechNode("done")			

	}
}


REGISTER_BEHAVIOR(Lab5part3);