#include "Behaviors/StateMachine.h"

$nodeclass CountEllipsesAprilTag : VisualRoutinesStateNode {
	$nodeclass FindStuff : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.setAprilTagFamily();
	}
	$nodeclass ReportEllipses : doStart {
		NEW_SHAPEVEC(ellipses_shapes, EllipseData, select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(camShS));
		if (tags.size() == 0)
		{
			cout <<"No tags detected"<<endl;
		}
		else
		{
			int left = 0;
			int right = 0;
			SHAPEVEC_ITERATE(ellipses_shapes, EllipseData, myellipse) {
				if (myellipse->getCentroid().isLeftOf(tags[0]->getCentroid()))
				{
					left++;
				}
				else
				{
					right++;
				}
				
			} END_ITERATE;
			cout << "Number of ellipses to left: " << left << endl;
			cout << "Number of ellipses to right: " << right << endl;
			if (left == 0)
			{
				cout << "All ellipses are to the right" << endl;
			}
			else
			{
				if (right == 0)
				{
					cout << "All ellipses are to the left" << endl;	
				}
				else
				{
					cout << "Ellipses are mixed" << endl;
				}
			}
		}
	}

	$setupmachine {
		FindStuff =C=> ReportEllipses
	}
}

REGISTER_BEHAVIOR(CountEllipsesAprilTag);