#include "Behaviors/StateMachine.h"
#include <vector>
$nodeclass ThreeEllipsesPolygon : VisualRoutinesStateNode {
    $nodeclass BuildMap : MapBuilderNode : doStart {
        mapreq.addObjectColor(ellipseDataType,"blue");
        mapreq.setAprilTagFamily();
    }
              
    $nodeclass Reporter : VisualRoutinesStateNode : doStart {
        cout << "there were " << camShS.allShapes().size() << " shapes in the image." << endl;

        NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
        NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
        vector<Point> points;
        if (ellipse_shapes.size() >= 3) {
            int counter = 0;
            SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, ellipse) { 
                if(counter < 3){
                    points.push_back(ellipse->getCentroid());
                    counter++;
                }
            } END_ITERATE;


            NEW_SHAPE(triangle, PolygonData, new PolygonData(camShS, points, true, true, true));
            SHAPEVEC_ITERATE(april_tags, AprilTagData, april_tag) {
                if(triangle->isInside(april_tag->getCentroid())){
                    int id = april_tag->getTagID();
                    cout << "TAG ID: " << id << "\n";
                }
            } END_ITERATE;
        }
        else {
            cout << "There were less than three ellipses.\n";
        }
    }

                          
    $setupmachine{
        builder: BuildMap
        builder =MAP=> Reporter =T(5000)=> builder
    }

}

REGISTER_BEHAVIOR(ThreeEllipsesPolygon);