#include "Behaviors/StateMachine.h"

$nodeclass AprilTagCounter : VisualRoutinesStateNode {

	$nodeclass FindEllipses : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.setAprilTagFamily();  // Use the default tag family
	}

	$nodeclass CountEllipses : 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());
		}
	
		NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
		if (april_tags.size() > 0) {
			IsLeftOfThis mytest(april_tags[0]);

			int left_ellipses = 0;
			int right_ellipses = 0;

			SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, ell)
				if (ell->getArea() >= 250) {
					if (mytest(ell))
						++left_ellipses;
					else
						++right_ellipses;
				}
			END_ITERATE;

			cout << "Ellipses left of april tag: " << left_ellipses << endl
				 << "Ellipses right of april tag: " << right_ellipses << endl;
			if (left_ellipses == 0 && right_ellipses > 0)
				cout << "All ellipses are to the right of the april tag." << endl;
			else if (right_ellipses == 0 && left_ellipses > 0)
				cout << "All ellipses are to the right of the april tag." << endl;
			else if (left_ellipses == 0 && right_ellipses == 0)
				cout << "No ellipses in image." << endl;
			else
				cout << "Ellipses are mixed between left and right of the april tag." << endl;
		}
		else {
			cout << "No april tags in picture." << endl;
		}
		
	}

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