#include "Behaviors/StateMachine.h"

$nodeclass Polygon {
    $nodeclass FindEllipsesAndTags : MapBuilderNode(MapBuilderRequest::cameraMap) : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
        mapreq.addObjectColor(lineDataType, "red");
        mapreq.setAprilTagFamily();
    }

    $nodeclass MakePolygon : doStart {
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

        if(ellipse_shapes.size() < 3) {
            cout << "Not enough ellipses to form a polygon" << endl;
            return;
        }

        std::vector<Point> three_points;
        for(std::vector<Shape<EllipseData> >::iterator it = ellipse_shapes.begin();
                it != ellipse_shapes.begin()+3; ++it) {
            three_points.push_back((*it)->getCentroid());
        }
        NEW_SHAPE(triangle, PolygonData, new PolygonData(worldShS, three_points, true));
        triangle->setColor("red");
        camShS.importShape(triangle);
    }

    $nodeclass CheckTags : doStart {
        NEW_SHAPEVEC(polygons, PolygonData, select_type<PolygonData>(camShS));
        if(polygons.empty()) {
            return;
        }

        NEW_SHAPEVEC(april_tags, AprilTagData, select_type<AprilTagData>(camShS));
        SHAPEVEC_ITERATE(april_tags, AprilTagData, tag) {
            if(polygons[0]->isInside(tag->getCentroid())) {
                cout << "Tag " << tag->getTagID() << " is inside the polygon" << endl;
            }
        } END_ITERATE;
    }

    $setupmachine {
        FindEllipsesAndTags =C=> MakePolygon =N=> CheckTags
    }
}

REGISTER_BEHAVIOR(Polygon);