#include "Behaviors/StateMachine.h"

$nodeclass PolygonTriangle : VisualRoutinesStateNode {
	$nodeclass FindShapes : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
	}
	
	$nodeclass ReportResult : doStart {
		//constitute polygon
		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(red_stuff, bool, visops::colormask(camFrame,"red"));
		NEW_SHAPEVEC(red_ellipses, EllipseData, EllipseData::extractEllipses(red_stuff));
		
		//to be determined whether in or out
		NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame,"blue"));
		NEW_SHAPEVEC(blue_ellipses, EllipseData, EllipseData::extractEllipses(blue_stuff));
		
		Point targetPt = blue_ellipses[0]->centerPt();
		
		//initalize vector of polygon points (red ellipses)
		vector<Point> polygonPts;

			//add points to vector
		SHAPEVEC_ITERATE(red_ellipses, EllipseData, ellipse) {
			polygonPts.push_back(ellipse->getCentroid());
		} END_ITERATE;
		
		//construct polygon (triangle) with vector
		PolygonData triangle(camShS, polygonPts, true);
		
		//inside or out?
		if(triangle.isInside(targetPt)) {
			cout<< "blue ellipse inside polygon" << endl;
		} else {
			cout<< "blue ellipse outside polygon" << endl;
		}
	}
	$setupmachine{
		findstuff: FindShapes
		report: ReportResult
		findstuff=C=>report
	}
}
REGISTER_BEHAVIOR(PolygonTriangle);