#include "Behaviors/StateMachine.h"

$nodeclass VeeShapeScene : VisualRoutinesStateNode {
	//Find the two lines in field and pursue shapes until robot gets everything
	//Also find cylinders
	$nodeclass FindVeeShape : MapBuilderNode(MapBuilderRequest::cameraMap) : doStart {
		mapreq.addObjectColor(lineDataType,"blue");
		mapreq.addObjectColor(ellipseDataType, "red");
	}
	
	//Generate sketches for the top half plane for each line
	$nodeclass CreateWedges : VisualRoutinesStateNode : doStart {
		//Get all lines & ellipses in localShS
		NEW_SHAPEVEC(line_shape, LineData, select_type<LineData>(camShS));
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		//For each line in localShS, create a sketch
		NEW_SKETCH(top_half_planes1, bool, visops::topHalfPlane(line_shape[0]));
		NEW_SKETCH(top_half_planes2, bool, visops::topHalfPlane(line_shape[1]));
		
		//Create an intersection of the two lines using visops::min
		NEW_SKETCH(intersection_plane, bool, visops::min(top_half_planes1,top_half_planes2));

		//Find the region of the eggs using labelcc
		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(red_stuff, bool, visops::colormask(camFrame, "red"));
		NEW_SKETCH(red_eggs, bool, visops::labelcc(red_stuff));

		//Find the intersection of red_eggs with intersection_plane.
		//This is what was asked for by the question
		NEW_SKETCH(inside_eggs, bool, visops::min(red_eggs,intersection_plane));
	}

	$setupmachine {
		FindVeeShape =C=> CreateWedges
	}
}

REGISTER_BEHAVIOR(VeeShapeScene);