#include "Behaviors/StateMachine.h"

$nodeclass Polygons : VisualRoutinesStateNode {

	$nodeclass FindEllipses : MapBuilderNode : doStart {

		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "red");
	}

	// Construct a polygon using 3 or more green ellipses, and determine whether a red
	// ellipse is inside of the polygon
	$nodeclass InsideOrOutside : VisualRoutinesStateNode : doStart {

		vector<Point> green;

		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		NEW_SHAPEVEC(red_ellipses, EllipseData, subset(ellipse_shapes, IsColor("red")));
		NEW_SHAPEVEC(green_ellipses, EllipseData, subset(ellipse_shapes, IsColor("green")));

		// Filter the green ellipses
		SHAPEVEC_ITERATE(green_ellipses, EllipseData, myellipse)

			// Filter the green ellipses
			if (myellipse->getSemimajor() > 15 && myellipse->getSemiminor() > 15) {
				green.push_back(myellipse->getCentroid());
			} else {
				camShS.deleteShape(myellipse);
			}
		END_ITERATE

		// Identify the largest red ellipse and remove others from the sketch space
		Shape<EllipseData> red = red_ellipses[0];
		SHAPEVEC_ITERATE(red_ellipses, EllipseData, myellipse)
			if (myellipse->getArea() > red->getArea()) {
				red = myellipse;
			}
			camShS.deleteShape(myellipse);
		END_ITERATE
		NEW_SHAPE(largestRed, EllipseData, new EllipseData(camShS, red->getCentroid(),
			red->getSemimajor(), red->getSemiminor(), red->getOrientation()));
		largestRed->setColor("red");	
		
		// Check for a valid number of ellipses
		NEW_SHAPEVEC(filtered, EllipseData, select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(filteredGreen, EllipseData, subset(filtered, IsColor("green")));
		NEW_SHAPEVEC(filteredRed, EllipseData, subset(filtered, IsColor("red")));

		if (filteredGreen.size() < 3 || filteredRed.size() != 1) {
			cout << "Require 1 red ellipse and at least 3 green ellipses." << endl;
		} 
		
		// Construct the polygon and draw it in the shape space
		else {

			NEW_SHAPE(polygon, PolygonData, 
				new PolygonData(camShS, green, true, true, true));
			polygon->setColor("green");

			// Determine if the red ellipse is inside the green polygon
			if (polygon->isInside(red->getCentroid())) {
				cout << "The red ellipse is inside the green polygon." << endl;
			} else {
				cout << "The red ellipse is outside of the green polygon." << endl;
			}
		}
	}

	$setupmachine {

		FindEllipses =C=> InsideOrOutside
	}

}

REGISTER_BEHAVIOR(Polygons);