#include "Behaviors/StateMachine.h"

$nodeclass AprilClassifier : VisualRoutinesStateNode {

	$setupmachine{

		FindTagAndEllipses =C=> DetermineEllipsesStatus =C=> SpeechNode("done");

	}	

	$nodeclass FindTagAndEllipses : MapBuilderNode : doStart {
		mapreq.setAprilTagFamily();
                mapreq.addObjectColor(ellipseDataType, "red");

	}
	
	$nodeclass DetermineEllipsesStatus : MapBuilderNode : doStart {
		
		
		NEW_SHAPEVEC(localTags, AprilTagData, select_type<AprilTagData>(camShS));
		 NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
		
		if(localTags.size() == 0)
		{
			cout<<"no tags found" << endl;
			return;
		}

		Point aprilTagCenter = localTags[0]->getCentroid();
		
		int numLeft = 0;
		int numRight = 0;
                SHAPEVEC_ITERATE(ellipses, EllipseData, currellipse)
			if(aprilTagCenter.isLeftOf(currellipse->getCentroid()))
				numLeft++;
			else numRight++;
                END_ITERATE;

		if(numRight ==0 && numLeft ==0)
                {
                        cout << "no ellipses found"<< endl;
                        return;
                }
                if(numRight ==0)
                cout << "all ellipses to the left" << endl;
                if(numLeft == 0)
                cout << "all ellipses to the right" << endl;
                if(numRight != 0 && numLeft != 0)
                cout << "mixed"<<endl;


	}

}

REGISTER_BEHAVIOR(AprilClassifier);