#include "Behaviors/StateMachine.h"

$nodeclass Polygons {

    $nodeclass Find : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
        mapreq.setAprilTagFamily();
    }


    $nodeclass Report : doStart {
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, 
                    select_type<EllipseData>(camShS)); 
        NEW_SHAPEVEC(localTags, AprilTagData, 
                     select_type<AprilTagData>(camShS));
        std::vector<Point> points;
        SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse) {
            points.push_back(myellipse->centerPt());
       } END_ITERATE;
        
        NEW_SHAPE(polygon0, PolygonData, new PolygonData(camShS,points,true));
        SHAPEVEC_ITERATE(localTags, AprilTagData, t) {
            if (polygon0->isInside(t->getCentroid())) {
                cout << "April Tag " << t->getTagID() << " is in the polygon\n";
            }
        } END_ITERATE;
    }
    
    $setupmachine {
        Find =C=> Report =N=> SpeechNode("done")
    }

}

REGISTER_BEHAVIOR(Polygons);