#include "Behaviors/StateMachine.h"

$nodeclass CountEllipsesLine : VisualRoutinesStateNode {
	$nodeclass FindStuff : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.addObjectColor(lineDataType, "blue");
	}
	$nodeclass ReportEllipses : doStart {
		NEW_SHAPEVEC(ellipses_shapes, EllipseData, select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));
		if (line_shapes.size() == 0)
		{
			cout <<"No line detected" << endl;
		}
		else
		{
			int left = 0;
			int right = 0;
			SHAPEVEC_ITERATE(ellipses_shapes, EllipseData, myellipse) {
				if (line_shapes[0]->pointIsLeftOf(myellipse->getCentroid()))
				{
					left++;
				}
				else
				{
					right++;
				}
				
			} END_ITERATE;
			cout << "Number of ellipses to left: " << left << endl;
			cout << "Number of ellipses to right: " << right << endl;
		}
	}

	$setupmachine {
		FindStuff =C=> ReportEllipses
	}
}

REGISTER_BEHAVIOR(CountEllipsesLine);