#include "Behaviors/StateMachine.h"
#include <vector>

$nodeclass InsidePolygonExtended2: VisualRoutinesStateNode {
	$nodeclass GetEggs : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
	}

	$nodeclass MapEggs : VisualRoutinesStateNode : doStart {
		//Get all shapes and printout result
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		//Delete noise shapes
		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myEllipse)
			if(myEllipse->getArea() < 170) {
				camShS.deleteShape(myEllipse);
			}
		END_ITERATE;

		//Find the lines between the 3 ellipses
		vector<Point> myPoints;
		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myEllipse)
			IsColor redtest("red");
			if(redtest(myEllipse)) {
				myPoints.push_back(myEllipse->getCentroid());
			}
 		END_ITERATE;
		
		//Create the polygon from the 3 lines found
		NEW_SHAPE(newPolygon, PolygonData,
			new PolygonData(camShS, myPoints, true, true, true));
		
		//Check if inside or outside
		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myEllipse)
			IsColor bluetest("blue");
			if(bluetest(myEllipse)) {
				if(newPolygon->isInside(myEllipse->getCentroid())) {
					cout << "The blue ellipse is inside the polygon!" << endl;
				} else {
					cout << "The blue ellipse is outside the polygon!" << endl;
				}
			}
		END_ITERATE;
	}	

	$setupmachine {
		GetEggs =C=> MapEggs
	}
}
REGISTER_BEHAVIOR (InsidePolygonExtended2);