#include "Behaviors/StateMachine.h"

$nodeclass PolygonFinder : VisualRoutinesStateNode {

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

	$nodeclass MakePolygon : VisualRoutinesStateNode : doStart {


		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, ell) {
			if (ell->getArea() < 250) {
				camShS.deleteShape(ell);
			}
		}
		END_ITERATE

		if (ellipse_shapes.size() > 0) {
			NEW_SKETCH(ellipse0, bool, ellipse_shapes[0]->getRendering());
		}
	
		IsColor is_blue("blue");
		IsColor is_red("red");
		vector<Point> polygon_points;
		vector<Point> other_points;

		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, ell)
			if (ell->getArea() >= 250) {
				if (is_blue(ell) && polygon_points.size() < 3)
					polygon_points.push_back(ell->getCentroid());
				else if (is_red(ell))
					other_points.push_back(ell->getCentroid());
			}
		END_ITERATE;

		if (polygon_points.size() < 3) {
			cout << "Not enough blue ellipses to form a polygon." << endl;
		}
		else {
			PolygonData triangle(camShS, polygon_points, true);
			triangle.setColor("blue");
			NEW_SKETCH(my_triangle, bool, triangle.getRendering());

			for (uint i = 0; i < other_points.size(); i++) {
				if (triangle.isInside(other_points[i])) {
					cout << "Red ellipse inside blue triangle!" << endl;
				}
				else {
					cout << "Red ellipse outside blue triangle!" << endl;
				}
			}
		}

	}

	$setupmachine {
		FindEllipses =C=> MakePolygon =N=> SpeechNode("done")
	}
}
REGISTER_BEHAVIOR(PolygonFinder);