#include "Behaviors/StateMachine.h"

$nodeclass AprilTagsEllipse : VisualRoutinesStateNode {
	$nodeclass FindShapes : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.setAprilTagFamily();
	}
	
	$nodeclass ReportResult : doStart {
		NEW_SHAPEVEC(ellipse_shapes, EllipseData,select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(cameraTags, AprilTagData, select_type<AprilTagData>(camShS));
		DualCoding::Shape<DualCoding::AprilTagData> t = cameraTags[0];
		
		int leftCount = 0;
		int rightCount = 0;
		Point tagCenter = t->getCentroid();

		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse) {
			if (myellipse->getCentroid().isLeftOf(tagCenter)) {
				leftCount++;
			} else {
				rightCount++;
			}
		} END_ITERATE;

		if (leftCount==0) {
			cout << "All on LHS!" << endl;
		} else if (rightCount==0) {
			cout << "All on RHS!" << endl;
		} else {
			cout << "Mixed!" << endl;
		}
		cout << "left : " << leftCount << endl;
		cout << "right: " << rightCount << endl;
	}
	$setupmachine {
		findstuff: FindShapes
		report: ReportResult
		findstuff=C=>ReportResult
	}
}
REGISTER_BEHAVIOR(AprilTagsEllipse);