#include "Behaviors/StateMachine.h"

$nodeclass Lab4Part3 : VisualRoutinesStateNode {
    $nodeclass LookForTags : MapBuilderNode($,MapBuilderRequest::worldMap) : constructor {
        mapreq.setAprilTagFamily();
        mapreq.addAllObjectColors(ellipseDataType);
    }

    $nodeclass Report : VisualRoutinesStateNode : doStart {
        NEW_SHAPEVEC(localTags, AprilTagData, select_type<AprilTagData>(localShS));
	if (localTags.size() == 0) {
	    cout << "No april tag found!" << endl;
	}
	else {
	    if (localTags.size() > 1) cout << "Found more than one tag, picking one." << endl;
	    Shape<AprilTagData> tag = localTags[0];
	    int centroidX = tag->getCentroid().coordX();
	    NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
	    bool left = false;
	    bool right = false;
	    SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myEllipse)
	        if (myEllipse->getCentroid().coordX() < centroidX) {
		    right = true;
		}
		else {
		    left = true;
		}
	    END_ITERATE;
	    if (left && right) {
	        cout << "There are ellipses on both sides." << endl;
	    }
	    else if (left) {
	        cout << "There are ellipses on the left." << endl;
	    }
	    else if (right) {
	        cout << "There are ellipses on the right." << endl;
	    }
	    else {
	        cout << "No ellipses found." << endl;
	    }
	}
    }

    $setupmachine {
        LookForTags =MAP=> Report
    }
}

REGISTER_BEHAVIOR(Lab4Part3);