#include "Behaviors/StateMachine.h"

$nodeclass VeeSketch : VisualRoutinesStateNode {

	$nodeclass ExtractLines : MapBuilderNode : doStart {
		mapreq.addObjectColor(lineDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
	}

	$nodeclass CreateSketch : VisualRoutinesStateNode : doStart {

		camSkS.clear();
		
		// Extract the longest 2 orange lines found
		NEW_SHAPEVEC(linevec1, LineData, select_type<LineData>(camShS));
		NEW_SHAPE(longest1, LineData, max_element(linevec1, LineData::LengthLessThan()));
		camShS.deleteShape(longest1);

		NEW_SHAPEVEC(linevec2, LineData, select_type<LineData>(camShS));
		NEW_SHAPE(longest2, LineData, max_element(linevec2, LineData::LengthLessThan()));
		camShS.deleteShape(longest2);
		
		// Create the vee wedge
		NEW_SKETCH(lineSketch1, bool, visops::topHalfPlane(longest1));	
		NEW_SKETCH(lineSketch2, bool, visops::topHalfPlane(longest2));

		NEW_SKETCH(vee, bool, visops::min(lineSketch1, lineSketch2));	

		// Find all ellipses and remove noise
		NEW_SKETCH(camFrame, uchar, sketchFromSeg());

		NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame, "green"));
		NEW_SKETCH(green_cc, uint, visops::labelcc(green_stuff));
		NEW_SKETCH(green_area, uint, visops::areacc(green_cc));
		NEW_SKETCH(green_eggs, bool, green_area > 20);
	
		NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));
		NEW_SKETCH(blue_cc, uint, visops::labelcc(blue_stuff));
		NEW_SKETCH(blue_area, uint, visops::areacc(blue_cc));
		NEW_SKETCH(blue_eggs, bool, blue_area > 20);
		
		NEW_SKETCH(all_eggs, bool, visops::max(green_eggs, blue_eggs));
	
		// Produce new sketch containing the intersection of the wedge and the eggs
		NEW_SKETCH(vee_eggs, bool, visops::mask(all_eggs, vee));

		camShS.clear();	
	}

	$setupmachine {
		ExtractLines =C=> CreateSketch
	}

}

REGISTER_BEHAVIOR(VeeSketch);