/**
 * @author Matt Dee (medee)
 * @author Alex Franchuk (afranchu)
 */
#include "Behaviors/StateMachine.h"

#define ELLIPSE_COLOR "red"
#define LINE_COLOR "blue"

$nodeclass Polygon {

    $nodeclass GetShape : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, ELLIPSE_COLOR);
        mapreq.setAprilTagFamily();
    }

    $nodeclass CountShapes : doStart {
        NEW_SHAPEVEC(ellipses, EllipseData,
            select_type<EllipseData>(camShS));
        vector<Point> centroids;
        for (unsigned int i = 0; i < ellipses.size(); i++) {
            centroids.push_back(ellipses[i]->getCentroid());
        }
        NEW_SHAPE(poly, PolygonData, new PolygonData(camShS, centroids, true, true, true));
        NEW_SHAPEVEC(cameraTags, AprilTagData, select_type<AprilTagData>(camShS));
        for (unsigned int i = 0; i < cameraTags.size(); i++) {
            if (poly->isInside(cameraTags[i]->getCentroid())) {
                cout << "Found AprilTag with ID " << cameraTags[i]->getTagID() << " inside of polygon" << endl;
            }

        }

    }

    $setupmachine {
        GetShape =C=> CountShapes =N=> SpeechNode("Done!")
    }

}

REGISTER_BEHAVIOR(Polygon);